import threading
import time

import serial
import serial.tools.list_ports
import logHandler


class SerialConnection:
  def __init__(self):
    self._ser = None
    self.port = None
    self.log_handler = logHandler.get_log_handler()
  
  # try connecting to an available port
  def open(self, port = None, baudrate = 1000000):
    if port != None:
      self.port = port

    if self.port:
      try:
        self._ser = serial.Serial(self.port, baudrate)
        self.log_handler.log_and_print("SERIAL: connected to " + self.port)

      except serial.SerialException as e:
        self.log_handler.log_and_print("SERIAL:", e)

    else:
      for port, name, _ in serial.tools.list_ports.comports():
        try:
          self._ser = serial.Serial(port, baudrate)
          self.log_handler.log_and_print("SERIAL: connected to " + port +  ", name: " + name)
          self.port = port
          break
        except serial.SerialException as e:
          self.log_handler.log_and_print("SERIAL:", e)

    if not self._ser:
        self.log_handler.log_and_print("SERIAL: no device available")
    
  def send(self, bytes):
    if self._ser:
      try:
        return self._ser.write(bytes)
      except serial.SerialException:
        self._ser = None
    else:
      time.sleep(10)
      self.open()
      return None
          
  def read(self, n=64):
    if self._ser:
      try:
        return self._ser.read(n)
      except AttributeError as e:
        self.log_handler.log_and_print("SERIAL:", e)
        return None
      except serial.SerialException as e:
        self.log_handler.log_and_print("SERIAL:", e)
        if self._ser.isOpen():
          self._ser.close()
        self._ser = None
        return None
    else:
      time.sleep(10)
      self.open()
      return None

  def readline(self):
    if self._ser:
      try:
        return self._ser.readline()
      except AttributeError as e:
        self.log_handler.log_and_print("SERIAL:", e)
        return None
      except serial.SerialException as e:
        self.log_handler.log_and_print("SERIAL:", e)
        if self._ser.isOpen():
          self._ser.close()
        self._ser = None
        return None
    else:
      time.sleep(10)
      self.open()
      return None

  def isConnected(self):
    return self._ser != None

  def close(self):
    if self._ser:
      self._ser.close()
      self._ser = None

class ArduinoSlave(SerialConnection):
  def __init__(self):
    super().__init__()
    self.sensorData = [0] * 13
    self._recvCbs = []
    self._t = threading.Thread(target=self._readSensors, args=())
    self._t.daemon = True  # thread dies when main thread (only non-daemon thread) exits.

  def open(self, port = None, baudrate = 1000000):
    super().open(port, baudrate)
    if not self._t.is_alive():
      self._t.start()

  def close(self):
    super().close()

  def getAcousticRTTs(self): # in microseconds
    return (
      int(self.sensorData[0]) / 16,
      int(self.sensorData[1]) / 16
    )

  def getMagneticField(self): # in mT
    return (
      int(self.sensorData[2]) / 1000,
      int(self.sensorData[3]) / 1000,
      int(self.sensorData[4]) / 1000
    )
  
  def getMagneticOffsets(self):
    return (
      int(self.sensorData[5]) / 1000,
      int(self.sensorData[6]) / 1000,
      int(self.sensorData[7]) / 1000,
    )
  
  def getAccelValues(self):
    return (
      int(self.sensorData[8]) / 1000,
      int(self.sensorData[9]) / 1000,
      int(self.sensorData[10]) / 1000,
    )
  
  def getGyroValues(self):
    return (
      int(self.sensorData[11])  / 1000, #
      int(self.sensorData[12])  / 1000, #
      int(self.sensorData[13]) / 1000  #
    )
  
  def getTemperature(self): # in °C
    return int(self.sensorData[14]) / 1000

  def addRecvCallback(self, cb):
    self._recvCbs.append(cb)

  def _readSensors(self):
    while True:
      data = super().readline()
      if data and len(data) > 2:
        data = str(data, encoding="ASCII", errors="ignore")
        vals = data[:-2].split('\t')
        if vals[0] == "DATA:" and len(vals) >= 13:
          self.sensorData = vals[1:]
          for cb in self._recvCbs:
            cb(self.sensorData)
        elif data.startswith("I2C ERROR"):
          pass
        else:
          self.log_handler.log_and_print("SERIAL: ", data[:-2])

# this allows the usage of a single instance of ArduinoSlave
_conn = None
def globalArduinoSlave():
  global _conn
  if not _conn:
    _conn = ArduinoSlave()
  return _conn

if __name__ == "__main__":
  arduino = ArduinoSlave()
  def cb(x):
    print(arduino.getAcousticRTTs(), arduino.getMagneticField(), arduino.getTemperature())

  arduino.addRecvCallback(cb)
  arduino.open()
  while True:
    time.sleep(1)