import readGridEyeData

import sys
import numpy as np
import cv2

SIZE = 8
visible_size = 128
distanceBetweenSensors_w = 2.6 #cm
distanceBetweenSensors_h = 2.6 #cm
distance2Object = 60.0 #cm
OFFSET = ((0, 0), ())

TEMP_SCALE = 20
RED = (0, 0, 255)
GREEN = (0, 255, 0)
BLUE = (255, 0, 0)

background = readGridEyeData.GridEyeData(sys.argv[1])

background_frames = [np.zeros((SIZE,SIZE)), np.zeros((SIZE,SIZE)),
                     np.zeros((SIZE,SIZE)), np.zeros((SIZE,SIZE))]
cnt = 1
Horizon_array = np.array([[x for x in range(8)] for _ in range(8)])
Vertical_array = np.array([[x for _ in range(8)] for x in range(8)])

# Gets background of each pixel.
while background.readFrame():
  frames = background.frames
  for i in range(4):
    background_frames[i] += np.array(frames[i].data)
  cnt += 1
  
for i in range(4):
  background_frames[i]/=cnt

# Reads data from object frames.
objects = readGridEyeData.GridEyeData(sys.argv[2])

while objects.readFrame():
  frames = objects.frames
  imgs = []
  heat_points = []

  # Subtracts background and scale the value.
  for i in range(4):
    frame = np.array(frames[i].data) - background_frames[i]
    frame[frame < 0] = 0.0
    frame *= TEMP_SCALE
    frame[frame > 255] = 255
    imgs.append(frame.astype(np.uint8))
    x = int(sum(sum(frame * Horizon_array)) / sum(sum(frame)) / (SIZE - 1) *
        (visible_size - 1))
    y = int(sum(sum(frame * Vertical_array)) / sum(sum(frame)) / (SIZE - 1) *
        (visible_size - 1))
    heat_points.append((x,y))
    print (x, y)
  all_img = np.concatenate((np.concatenate((imgs[0], imgs[1]), axis = 1),
                            np.concatenate((imgs[2], imgs[3]), axis = 1)),
                           axis = 0)
  all_img = cv2.cvtColor(all_img, cv2.COLOR_GRAY2RGB)
  all_img = cv2.resize(all_img, (visible_size*2, visible_size*2),
      interpolation = cv2.INTER_NEAREST)
  cv2.circle(all_img, heat_points[0], 5, RED, -1)
  cv2.circle(all_img, (heat_points[1][0]+visible_size, heat_points[1][1]), 5, RED, -1)
  
  cv2.imshow('sample', all_img)


  while cv2.getWindowProperty('sample', 0) >= 0:
    key = cv2.waitKey(50)
    if key >= 0:
      break