import cv2
import math
import numpy as np

kBACKGROUND_NUM = 10
kSIZE = 128
kEXPONENTAL_VALUE = 0.7

gSENSOR_FOV = 60.0 / 180.0 * math.pi


def exponential(img, value):
  tmp = cv2.pow(img.astype(np.double), value)*(255.0/(255.0**value))
  return tmp.astype(np.uint8)


def mergeFrames(imgs, SIZE, overlap):
  tmp = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
  tmp[:, :SIZE] = imgs[0]
  tmp[:, -SIZE:] += imgs[1]
  tmp[:, (SIZE-overlap): SIZE] = tmp[:, (SIZE-overlap): SIZE]/2
  
  tmp2 = np.zeros((SIZE, SIZE*2-overlap), dtype=np.uint16)
  tmp2[:, :SIZE] = imgs[2]
  tmp2[:, -SIZE:] += imgs[3]
  tmp2[:, (SIZE-overlap): SIZE] = tmp2[:, (SIZE-overlap): SIZE]/2
  
  merge = np.zeros((SIZE*2-overlap, SIZE*2-overlap), dtype=np.uint16)
  merge[:SIZE, :] = tmp
  merge[-SIZE:, :] += tmp2
  merge[(SIZE-overlap):SIZE, :] = merge[(SIZE-overlap):SIZE, :]/2
  #merge = exponential(merge, kEXPONENTAL_VALUE)
  
  return merge.astype(np.uint8)


class DataFuser(object):
  def __init__(self, sensor_dist):
    self.background_cnt = 0
    self.background_frame = np.zeros((4, 64))
    self.sensor_dist = sensor_dist

  def mergeFrame(self, frame, dist = None):
    if self.background_cnt < kBACKGROUND_NUM:
      self.background_frame += frame / kBACKGROUND_NUM
      self.background_cnt += 1
      return False

    frame = exponential(cv2.subtract(exponential(frame, kEXPONENTAL_VALUE),
                         exponential(self.background_frame, kEXPONENTAL_VALUE)),
                         0.3)
    print (([max(x) for x in frame]))
    imgs = [np.reshape(img, (8, 8)) for img in frame]
    imgs = [cv2.resize(img.astype(np.uint8), (kSIZE, kSIZE),
        interpolation = cv2.INTER_LINEAR) for img in imgs]
    try:
      overlap = int(kSIZE - 
          kSIZE * (self.sensor_dist / (2 * dist * math.tan(gSENSOR_FOV / 2))))
    except:
      overlap = 0
    if overlap < 0:
      overlap = 0
    overlap = 0
    
    return mergeFrames(imgs, kSIZE, overlap)

    if not dist:
      pass