import re
import math
import cv2
import numpy as np

def loadFromFile(file):
    distances = {v:0 for v in range(0, 360)}
    max = 0
    pattern = re.compile(r'(?<=\[)(.*?)(?=\])')
    with open(file) as f:
        data = f.read()
        for datapoint in re.split(',', data):
            s = pattern.search(datapoint)
            print(s)
            dist = float(datapoint.split('=')[-1])
            distances[int(s.group(0))] = dist

    return distances

def computeCoords(angleCoords, datapoint, scale, center, degrees):
    print(degrees)
    coord = (math.floor(scale * datapoint * math.cos(math.radians(degrees))), math.floor(scale * datapoint * math.sin(math.radians(degrees))))
    angleCoords[degrees] = (coord[0] + center[0], coord[1] + center[1])

def graph(angleCoords, img, size):
    for i in angleCoords:
        if angleCoords[i][0] != 0: print(angleCoords[i])
        img[angleCoords[i][1]][angleCoords[i][0]] = 255

def normalize(max, d):
    return {x:d[x]/max for x in d}

def main():
    size = (501, 501)
    scale = 20
    img = np.zeros(size)
    cv2.imshow("lidar", img)
    cv2.waitKey(1)
    max = 0
    data = {v:0 for v in range(0, 360)}
    normalized = {v:() for v in range(0, 360)}
    realdata = loadFromFile('values.txt')
    angleCoords = {v:(0, 0) for v in range(0, 360)}
    center = (math.floor(size[0]/2), math.floor(size[1]/2))
    for x in realdata:
        data[x] = realdata[x]
        if realdata[x] > max:
            normalized = normalize(realdata[x], data)
            max = realdata[x]

        img = np.zeros(size)

        computeCoords(angleCoords, realdata[x], scale, center, x)
        graph(angleCoords, img, size)
        cv2.imshow("lidar", img)
        cv2.waitKey(1)

main()