import queue
import statistics
from struct import calcsize
import numpy as np
import time
import threading # ?
import random # ?
from configparser import ConfigParser

from sensors.calibration import CalibrationStateMashine
from sensors.connection import globalArduinoSlave
import logHandler

conn = globalArduinoSlave()


class MagneticSensor:
  def __init__(self, conf):
    self.conf = conf
    self.queue = queue.Queue()
    self.calibration_state = CalibrationStateMashine()
    self.success = False
    self.mag_offset_x = float(conf["mag_sensor"]["mag_offset_x"])
    self.mag_offset_y = float(conf["mag_sensor"]["mag_offset_y"])
    self.log_handler = logHandler.get_log_handler()
  
  def start(self):
    if not conn.isConnected():
      conn.open()
    conn.addRecvCallback(self._readCb)

  def _readCb(self, raw):
    mag_values = conn.getMagneticField()
    print("Magnetic Offsets:", conn.getMagneticOffsets())
    if mag_values[0] >= 0 and mag_values[1] >= 0:
      pass
      #position = self.calculate_position(mag_values) ### MUSS AUF MAG. SENSOR ANGEPASST WERDEN!!!
      #if position != None:
      #  self.pass_to_gui(position + value)


  def setOffsets(self):
    # Read config file
    config_object = ConfigParser()
    config_object("config.ini")
    # Get mag_sensor section
    mag_sensor = config_object["mag_sensor"]
    # Update Offsets
    mag_sensor["offset_x"] = conn.getMagneticOffsets(0)
    mag_sensor["offset_y"] = conn.getMagneticOffsets(1)
     

  def start_calibration(self): ###
    self.calibration_state.reset_state()
    self.time_vals = [[],[]]
    self.calibration_state.next_state()
  
  def calibrate(self, value): ### öffnet erstmal popup :)
    #pass
     if self.calibration_state.get_state() == self.calibration_state.ACCUMULATING_1:
      self.time_vals[0].append(value[0])
      self.time_vals[1].append(value[1])
      self.calibration_state.progress = len(self.time_vals[0]) / 2
      if len(self.time_vals[0]) >= 100:
        self.cal_values["front"][0]  = statistics.mean(self.time_vals[0])
        self.cal_values["front"][1] = statistics.mean(self.time_vals[1])
        self.time_vals = [[],[]]
        self.calibration_state.next_state() # signal gui to get next position


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