room_scan_sim/mklines.py

# Constructs line equastions from a set of polar cords that represent a room

# Invoke python mklines.py <filename>

import csv
import sys
import math
from os.path import exists as file_exists
from basictypes import PointWithID, StraightLine, point_from_polar


def main():
    filename = get_filename()
    if filename == -1: return -1
    if not file_exists(filename):
        perr('File does not exist')
        return -1
    points = read_csv(filename, remove_header=True)
    if not valid_data(points): # Assumes header is removed
        perr('Invalid or corrupt samples')
        return -1
    #for val in points: print(val.get_cartesian())
    lines = list()
    for i in range(0, len(points), 2):
        p1 = points[i]
        p2 = points[i+1]
        print('p1 ', p1.get_x(), p1.get_y())
        print('p2 ', p2.get_x(), p2.get_y())
        line = StraightLine(p1, p2)
        lines.append(line)
        print('line ', line.get_m(), line.get_b())
    intercepts = list()
    for i, line in enumerate(lines):
        p = None # This is risky
        if i == len(lines) - 1:
            print("last")
            p = lines[i].intercept(lines[0])
            # Last line is a special case
        else:
            p = lines[i].intercept(lines[i+1])
        print(p.get_x(), p.get_y())
    return 0

def read_csv(filename: str, remove_header=False) -> list[PointWithID]:
    tmp_data = list() # New list of points (PointWithID)
    with open(filename, 'r') as csv_file:
        reader = csv.reader(csv_file)
        for i, row in enumerate(reader):
            if remove_header and i == 0: continue # Skip header
            # Assumes wall_id,r,theta
            point_tmp = PointWithID(float(row[1]), float(row[2]), row[0], PointWithID.FROM_POLAR)
            tmp_data.append(point_tmp)

    return tmp_data

def valid_data(samples:list) -> bool:
    # Check if we have an even number of samples
    return len(samples) % 2 == 0


def get_filename() -> str:
    try:
        filename = sys.argv[1]
        return filename
    except IndexError:
        perr('Provide target file name')
        return -1

def perr(*args):
    print(*args, file=sys.stderr)

if __name__ == '__main__':
    exit(main())