import queue
import time

from sensors.connection import globalArduinoSlave
import logHandler

conn = globalArduinoSlave()


class MagneticSensor:
  def __init__(self, conf):
    self.conf = conf
    self.queue = queue.Queue()
    self.log_handler = logHandler.get_log_handler()
    self.gyro_x = []
    self.gyro_y = []
    self.gyro_z = []
    self.offset_x = 0 #
    self.offset_y = 0 #
    self.offset_z = 0 #

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

  def _readCb(self, raw):
    #print("mag: ", conn.getMagneticField())
    #print("accel: ", conn.getAccelValues())
    print("gyro: ", conn.getGyroValues())

  def calibrate(self):
    # Gyroscope Calibration
    i = 0
    while i < 500:
      self.gyro_x.append(conn.getGyroValues()) #
      self.gyro_y.append(conn.getGyroValues()) #
      self.gyro_z.append(conn.getGyroValues()) #
      print("gyro_x: %d", self.gyro_x)
      i += 1
      if i == 500:
        self.offset_x = sum(self.gyro_x) / len(self.gyro_x)
        self.offset_y = sum(self.gyro_y) / len(self.gyro_y) 
        self.offset_z = sum(self.gyro_z) / len(self.gyro_z) 
        #pass
    # Accelerometer Calibration
    # Magnetometer Calibration

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

  def stop(self):
    self.log_handler.log_and_print("stop magnetic sensor")
    conn.close