import sensors
import time
import threading
import queue
import gui.mainWindow as mainWindow

conf = {
    "field":{
        "x":450,
        "y":450
    },
    "ac_sensor":{
        "sensor_distance":450,
        "y_offset":10,
        "left_x_offset":0,
        "rigth_x_offset":0,
        "sonicspeed":0.343,
        "overhead":150
    }
}

class CalibrationStateMashine():

    def __init__(self):
        self.state = 0
        self.value_count = 0

        self.NOT_CALIBRATED = 0
        self.READY_ON_POS_1 = 1
        self.ACCUMULATING_1 = 2
        self.FINISHED_POS_1 = 3
        self.READY_ON_POS_2 = 4
        self.ACCUMULATING_2 = 5
        self.FINISHED_POS_2 = 6
        self.CALIBRATION_DONE = 7

    def state_clearname(self):
        if self.state == self.NOT_CALIBRATED:
            return "not calibrated"
        elif self.state == self.READY_ON_POS_1:
            return "ready on position one"
        elif self.state == self.ACCUMULATING_1:
            return "gathering values on position one"
        elif self.state == self.FINISHED_POS_1:
            return "finished gathering values from position one"
        elif self.state == self.READY_ON_POS_2:
            return "ready on position two"
        elif self.state == self.ACCUMULATING_2:
            return "gathering values on position two"
        elif self.state == self.FINISHED_POS_2:
            return "calculating calibration values"
        elif self.state == self.CALIBRATION_DONE:
            return "calibration done"
    
    def next_state(self):
        if self.state < 7:
            self.state += 1
            print(self.state_clearname())

    def add_value(self):
        self.value_count += 1
    
    def return_state(self):
        return self.state

    def return_value_count(self):
        return self.value_count

    def reset_state(self):
        self.state = 0
    
    def reset_value_count(self):
        self.value_count = 0


def main():
    up_queue = queue.Queue()
    down_queue = queue.Queue()
    calibration_sate = CalibrationStateMashine()
    ac_sensor = sensors.AcusticSensor(conf,up_queue,down_queue,calibration_sate)
    sensor_thread = threading.Thread(target=ac_sensor.start)

    ui_thread = threading.Thread(target=mainWindow.start_mainwindow, args=(up_queue,down_queue,calibration_sate))

    sensor_thread.start()
    ui_thread.start()
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print("stop")
    except Exception as e:
        print("Error: ",e)
    finally:
        down_queue.put("stop")

main()