########## RASP PI IMPORTS ##########
from machine import I2C, Pin, PWM

########## PYTHON IMPORTS ##########
import time
import random
import math

########## IMPORTS ##########
from Vector3 import Vector3
from AsyncTasks import AsyncTasks
from ComponentsRW import InitI2C, IMU, Barometer,Servo


time.sleep(0.1) # Wait for USB to become ready

allPins = list(map(lambda x: Pin(x, Pin.OUT), range(28)))

class Debug:
    Frames: int = 0
    AccFrames: int = 0
    AccTime: int = 0

    @staticmethod
    def PrintFPS(intervalMCRS: int, __async_task: bool = False):
        if __async_task:
            currentTime = intervalMCRS / 1000000

            Debug.AccFrames += Debug.Frames
            Debug.AccTime += currentTime

            print("CURRENT FPS:", Debug.Frames / currentTime, end="\t\t\t")
            print("AVERAGE FPS:", Debug.AccFrames / Debug.AccTime)

            Debug.Frames = 0

        AsyncTasks.Create(Debug.PrintFPS, intervalMCRS, intervalMCRS, __async_task=True)

class Ares:
    Orientation = Vector3(0.0,0.0,0.0) # Rotation
    Height = 0

    @staticmethod
    def ignite():
        allPins[6].value(1)
        AsyncTasks.Create(allPins[6].value, 100 * 1000, 0)

    @staticmethod
    def Run():
        InitI2C(SDA=2, SCL=3)

        imu1 = IMU(0)
        imu2 = IMU(1)
        bar = Barometer()
        ServoX = Servo(PIN=11)
        ServoY = Servo(PIN=12)

        ServoX.setAngle(90)
        ServoY.setAngle(90)

        AsyncTasks.Create(Ares.ignite, 500000)
        allPins[11].value(1)

        Debug.PrintFPS(1000000)
        while True:
            Debug.Frames += 1

            # imu.getReadingAccel()
            # print(">", Ares.Orientation)
            # Ares.Orientation += imu1.getReadingAccel()
            # Ares.Orientation += imu2.getReadingAccel()
            # accel = imu.getReadingAccel()
            # print(imu1.getReadingAccel(), imu2.getReadingAccel())
            print(bar.getReadingPressure())

            TIME = (time.time_ns() // 10000000) % 1000
            ServoX.setAngle(math.sin(TIME*0.1 * 3.1415 / 180) * 90 + 90)
            ServoY.setAngle(math.sin((TIME+300)*0.1 * 3.1415 / 180) * 90 + 90)

            AsyncTasks.Process()
            time.sleep(0.01)

if __name__ == "__main__":
    Ares.Run()