# from umqtt.robust import MQTTClient
from machine import Pin, Timer
from orientation import *
import time

time.sleep(0.1)

last_interrupt_time = 0
debounce_time = 200

target = 0
target_azimuth = 0
target_latitude = 0

current_azimuth = 0
current_latitude = 0
heading_angle = 0
distance_angle = 0

filtered_acc = [ 0, 0, 0 ]
filtered_mag = [ 0, 0, 0 ]

mode = 0

CLK_PIN = 16
DT_PIN = 17
SW_PIN = 18
SDA_PIN = 20
SCL_PIN = 21

leds = [ Pin(i, Pin.OUT) for i in range(4, 12) ]
clk = Pin(CLK_PIN, Pin.IN)
dt = Pin(DT_PIN, Pin.IN)
sw = Pin(SW_PIN, Pin.IN)
i2c = machine.I2C(0, sda=Pin(SDA_PIN), scl=Pin(SCL_PIN), freq=400000)


update_sensor = Timer()

def rotary_encoder_handler(t):
    global target, last_interrupt_time
 
    current_time = time.ticks_ms()
    if time.ticks_diff(current_time, last_interrupt_time) > debounce_time:
        last_interrupt_time = current_time
        previous = target

        if dt.value():
            if target < 7:
                target += 1
        else:
            if target > 0:
                target -= 1

        print(target)
        leds[previous].off()
        leds[target].on()

def click_handler(t):
    global target, mode, last_interrupt_time
 
    current_time = time.ticks_ms()
    if time.ticks_diff(current_time, last_interrupt_time) > debounce_time:
        last_interrupt_time = current_time
        # mqtt_conn.publish(b'edinhg/response',str(target))
        # mqtt_conn.wait_msg()
        mode = not mode
        if (mode):
            update_sensor.init(period=500, callback=read_sensor)
        else:
            update_sensor.deinit()


def msg_recieved(topic, msg):
    global target_azimuth, target_latitude

    target_azimuth, target_latitude = msg.split(" ")


clk.irq(handler=rotary_encoder_handler, trigger=Pin.IRQ_FALLING)
sw.irq(handler=click_handler, trigger=Pin.IRQ_FALLING)


# ### Spajanje sa internetom ###

# nic = network.WLAN(network.STA_IF)
# nic.active(True)
# nic.connect('Lab220', 'lab220lozinka')

# print("Povezivanje")
# while not nic.isconnected():
#     print(".")
#     time.sleep(1)
# print("Povezano")



# ### Spajanje sa brokerom ###

# mqtt_conn = MQTTClient(client_id='EDINHG', server='broker.hivemq.com', user='', password='', port=1883)
# mqtt_conn.connect()
# mgtt.set_callback(sub)
# mqtt_conn.subscribe(b"edinhg/#")
# print("Konekcija sa MQTT brokerom uspostavljena")

leds[target].on()

def read_sensor(t):
    global target_azimuth, target_latitude, current_azimuth, current_latitude
    global heading_angle, distance_angle, filtered_acc, filtered_mag

    acc = normalize((0, 5, 4))
    mag = normalize((0, 40, 0))

    # filtered_acc = low_pass_filter(filtered_acc, acc, 0.1)
    # filtered_mag = low_pass_filter(filtered_mag, mag, 0.1)

    current_azimuth = get_azimuth(mag[0], mag[1])
    current_latitude = get_latitude(acc[1])

    heading_angle, distance_angle = upute(
        target_azimuth, target_latitude, current_azimuth, current_latitude
    )



while True:
    if mode == 1:
        print("Azimuth: ", current_azimuth)
        print("Latitude: ", current_latitude)
        print("Target azimuth: ", target_azimuth)
        print("Target latitude: ", target_latitude)
        print("Heading angle: ", heading_angle)
        print("Distance angle: ", distance_angle)
        print()
        print()

    time.sleep(0.1)