from connection import ArduinoSlave
import markerDetection
import time
import statistics
import math
import threading, cv2

conn = ArduinoSlave()

class AcusticSensor:
  def __init__(self):
    self.sonic_speed_left = 0
    self.sonic_speed_right = 0
    self.sensor_distance = 450 #in mm

  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)

  def _readCb(self, raw):
    print("acc: ", conn.getAcusticRTTs())

  def calibrate(self, distance):
    print("Move gondel to far left corner")
    input()
    speed = list()
    for i in range(100):
        time_val = [1.312,0]
        speed.append(distance/time_val[0])
    self.sonic_speed_left = statistics.mean(speed)
    print("sonic speed left:",self.sonic_speed_left)

    print("Move gondel to far right corner")
    input()
    speed = list()
    for i in range(100):
        time_val = [0,1.312]
        speed.append(distance/time_val[1])
    self.sonic_speed_right = statistics.mean(speed)
    print("sonic speed right:",self.sonic_speed_right)

  def read(self):
    return conn.getAcusticRTTs()

  def calculate_position(self):
        if not self.sonic_speed_right or not self.sonic_speed_left:
            print("sensor not calibrated! calibrate now!")
            return (0,0)
        else:
            time_val = self.read()
            distance_left = time_val[0] * self.sonic_speed_left / 1000 # seconds -> mseconds
            distance_right = time_val[1] * self.sonic_speed_right / 1000
            print(distance_left,distance_right)
            x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance)
            y = math.sqrt(distance_left**2 - x**2)
            return (x,y)

class MagneticSensor:
  def __init__(self):
    pass

  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)

  def _readCb(self, raw):
    print("mag: ", conn.getMagneticField())

  def calibrate(self, x, y):
    pass

  def read(self):
    return conn.getMagneticField()

class OpticalSensor():
  def __init__(self):
    self.cap = cv2.VideoCapture(0)
    self._t  = None
    self.values = None

  def start(self):
    if not self._t:
      self._t = threading.Thread(target=self._getFrames, args=())
      self._t.daemon = True  # thread dies when main thread (only non-daemon thread) exits.
      self._t.start()

  def _getFrames(self):
    while True:
      success, image = self.cap.read()
      if success:
        self.values = markerDetection.measureDistances(image)
        print("opt:", self.values)

  def calibrate(self, x, y):
    pass

  def read(self):
    return self.values

if __name__ == "__main__":
  acc = AcusticSensor()
  acc.start()
  mag = MagneticSensor()
  mag.start()
  opt = OpticalSensor()
  opt.start()
  while True:
    time.sleep(1)