from gridEyeReader import GridEye
from dataFuser import DataFuser

import cv2
import logging
import time
import numpy as np

logging.basicConfig(level=logging.DEBUG)

g = GridEye('/dev/ttyACM0')
d = DataFuser(10) # distance between sensors.
cv2.namedWindow("Image", cv2.WINDOW_NORMAL)
for _ in range(1000000):
  frame = g.readData()
  out = d.mergeFrame(np.array(frame), 100)
  if type(out) is np.ndarray:
    cv2.imshow('Image', cv2.cvtColor(out, cv2.COLOR_GRAY2BGR))
  k = cv2.waitKey(1)
  if k == ord('q'):
    break
  elif k == ord('c'):
    cv2.imwrite('shot.png', out)


cv2.destroyAllWindows()