import cv2
import serial
import threading
import time

class Frame():
    def __init__(self, time, data):
        self.time = time
        self.data = data

class GridEye():
    def __init__(self, serialPort, baudrate):
        self.port = serial.Serial(serialPort, baudrate)
        self.frames = []
        self.reading = True
        self.thread = threading.Thread(target = self.reader)
        self.thread.setDaemon(True)

    def start(self):
        self.thread.start()
        
    def stop(self):
        self.reading = False
        self.thread.join()
        
    def reader(self):
        data = []
        data_time = 0;
        while (self.reading):
            line = b''
            while (self.reading):
                c = self.port.read()
                if c == b'\r':
                    c = self.port.read()
                    break
                if c == b'\n':
                    break
                line += c
            #line = self.port.readline()#.decode('utf-8')
            # if line:
                # print (line)
                # time.sleep(0.01)
            # if self.port.in_waiting > 0:
                # print (self.port.in_waiting)
            if b'#' in line:
                if len(data) == 8:
                    print (data_time)
                    #print (data)
                    self.frames.append(Frame(data_time, data))
                    if len(self.frames) > 100:
                        self.frames.pop(0)
                else:
                    print ('something wrong', len(data))
                data_time = time.time()
                data = []
            else:
                try:
                    row = [float(x) for x in line.split()] 
                    if len(row) == 8:
                        data.append(row)
                except ValueError as e:
                    print ('error', e)
                    data_time = time.time()
                    data = []
                if len(data) > 8:
                    data.pop(0)
                    
                    
        
if __name__ == '__main__':
    import cv2
    import numpy as np
    SIZE = 128
    overlap = 120
    grideye = GridEye('COM12', 115200)
    grideye.start()
    cv2.imshow('sample', np.empty((SIZE*2,SIZE*2), np.uint8))
    while True:
        if len(grideye.frames) > 0:
            frame = grideye.frames.pop(0)
            img = (np.array(frame.data)-15)*10
            img = cv2.resize(img.astype(np.uint8), (SIZE,SIZE), interpolation = cv2.INTER_LINEAR)
            img2 = np.copy(img)
            
            out = np.empty((SIZE*2, SIZE*2), dtype=np.uint16)
            out[:SIZE, :SIZE] = img
            out[:SIZE, SIZE:] = img2
            
            out[SIZE:, :SIZE] = img
            out[SIZE:, (SIZE-overlap):SIZE] += img2[:,:overlap]
            out[SIZE:, (SIZE-overlap):SIZE] = out[SIZE:, (SIZE-overlap):SIZE]/2
            
            out[SIZE:, SIZE:SIZE+(SIZE-overlap)] = img2[:,overlap:SIZE]
            out = out.astype(np.uint8)
            cv2.imshow('sample', out)
            if cv2.waitKey(1) == ord('q'):
                break
        time.sleep(0.001)
    grideye.stop()