from connection import ArduinoSlave
import time
import statistics
import math

conn = ArduinoSlave()

class AcusticSensor:
  def __init__(self):
    self.sonic_speed_left = 0
    self.sonic_speed_rigth = 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 rigth corner")
    input()
    speed = list()
    for i in range(100):
        time_val = [0,1.312]
        speed.append(distance/time_val[1])
    self.sonic_speed_rigth = statistics.mean(speed)
    print("sonic speed rigth:",self.sonic_speed_rigth)

  def read(self):
    return (952, 660)

  def calculate_position(self):
        if not self.sonic_speed_rigth 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_rigth = time_val[1] * self.sonic_speed_rigth / 1000
            print(distance_left,distance_rigth)
            x = (self.sensor_distance**2 - distance_rigth**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 (0, 0)

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