import time
import board
import busio
import adafruit_bno055
import adafruit_gps
import digitalio
from car_os.network_handler import NetworkHandler
import math
from car_os.pps import PPS
from car_os.gps import GPS
from car_os.imu import IMU
from car_os.timer import Timer
from car_os.enums import SensorClass

def wait(start):
    delta = time.monotonic_ns() - start
    print("delta:", delta)
    while 0 <= delta < 1000:
        delta = time.monotonic_ns() - start
        print("delta:", delta)
        continue
    return

def main():
    heartbeat_timer = Timer(15)
    
    network = NetworkHandler(SensorClass.VEHICLE, 256, 0.1, 5000, 0.001, 5001)
    network.initialize()
    network_timer = Timer(1)
    network_reset_timer = Timer(600)
    
    pps = PPS(board.GP22)
    imu = IMU(board.GP21, board.GP20, 0x0C, (44, 66, -109), (-1, 3, -1), (-7, -16, -39))
    gps = GPS(board.GP27, board.GP26, 0x42)
    
    while True:
        delta = pps.update()
        if delta > 60:
            continue
        heartbeat_timer.update(delta)
        network_timer.update(delta)
        network_reset_timer.update(delta)
        gps.update(pps.utc)
        imu.update(pps.utc)
            
        if not gps.has_fix:
            print(time.time())
            print("waiting for gps fix")
            print()
            continue
        
        if gps.time_updated:
            pps.update_from_gps(gps)
        
        if network_timer.check_timer():
            network.update(pps.utc, network_reset_timer)
        
        #if heartbeat_timer.check_timer():
        #    message = bytearray(b'\xd4\x53\x6e\x4e\x00\x00\x00')
        #    message[4] = pps.utc[0]
        #    message[5] = pps.utc[1]
        #    message[6] = pps.utc[2]
        #    message += b'\x00\x00'
        #    network.send(message)
        
        if not gps.ready:
            continue
        try:
            message = gps.get_position_message()
            if message is not None:
                network.send(message)
        except:
            pass
        try:
            message = gps.get_sat_message()
            if message is not None:
                network.send(message)
        except:
            pass
        try:
            message = imu.get_accel_message()
            if message is not None:
                network.send(message)
        except:
            pass
        try:
            message = imu.get_mag_message()
            if message is not None:
                network.send(message)
        except:
            pass
            
main()