import queue
from struct import calcsize
import numpy
#import time
import threading # ?
import random # ?

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() # neu
    self.success = False
    self.mpu_array = []
    self.mpu_gyro_offsets = []
    self.log_handler = logHandler.get_log_handler()
  
  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)
    #self.dummyActive = True
    #dummyThread = threading.Thread(target=self._readCb_dummy)
    #dummyThread.start()

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

  def calibrate(self, conf):
    # Gyroscope Calibration
    gyro_x, gyro_y, gyro_z = conn.getGyroValues()
    self.mpu_array.append([gyro_x,gyro_y,gyro_z])
    
    # Accelerometer Calibration
    accel_x, accel_y, accel_z = conn.getAccelValues()

    # Magnetometer Calibration
    magneto_x, magneto_y, magneto_z = conn.getMagneticField()

  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))