import queue
import time
<<<<<<< HEAD
import threading
import random
=======
>>>>>>> de61b5b54174156bc78823188e70ac7b94fdde66

from sensors.connection import globalArduinoSlave
import logHandler

conn = globalArduinoSlave()


class MagneticSensor:
  def __init__(self, conf):
    self.conf = conf
    self.queue = queue.Queue()
<<<<<<< HEAD
    self.calibration_state  = CalibrationStateMashine()
    self.log_handler = logHandler.get_log_handler() # neu
    self.success = False

    self.field_height = float(conf["field"]["y"])
    self.field_width = float(conf["field"]["x"])

    self.n = 0
  
    #pass
=======
    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 #
>>>>>>> de61b5b54174156bc78823188e70ac7b94fdde66

  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)
<<<<<<< HEAD
    self.dummyActive = True
    dummyThread = threading.Thread(target=self._readCb_dummy)
    dummyThread.start()

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

  def _readCb_dummy(self):
    self.log_handler.log_and_print("acoustic sensor: generating test values")
    while self.dummyActive:
      dummyValue = 250
      position = 20

      self.pass_to_gui(position + dummyValue)

  def calibrate(self, x, y):
    pass
=======
    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
>>>>>>> de61b5b54174156bc78823188e70ac7b94fdde66

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

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

  def pass_to_gui(self, data):
    self.queue.put(("data", data))