# Motor GPIO's (not pin number, GPIO number) ###
gpio_motor1 = 2 # front left, clockwise

# i2c pins used for MPU-6050
gpio_i2c_sda = 12
gpio_i2c_scl = 13

import machine
import time

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

print("Hello, Pi Pico!")

# THE FLIGHT CONTROL LOOP
def run() -> None:

    print("Hello from Scout!")

    # flash the LED a few times to show the microcontroller has received power and the program is active
    led = machine.Pin(25, machine.Pin.OUT) # the onboard LED of the Raspberry Pi Pico
    for x in range(8):
        led.on()
        time.sleep(0.1)
        led.off()
        time.sleep(0.1)

    # overclock
    machine.freq(250000000)
    print("Overclocked to 250,000,000")

    # Set up IMU (MPU-6050)
    i2c = machine.I2C(0, sda = machine.Pin(gpio_i2c_sda), scl = machine.Pin(gpio_i2c_scl))
    mpu6050_address:int = 0x68
    i2c.writeto_mem(mpu6050_address, 0x6B, bytes([0x01])) # wake it up
    i2c.writeto_mem(mpu6050_address, 0x1A, bytes([0x05])) # set low pass filter to 5 (0-6)
    i2c.writeto_mem(mpu6050_address, 0x1B, bytes([0x08])) # set gyro scale to 1 (0-3)

    # confirm IMU is set up
    whoami:int = i2c.readfrom_mem(mpu6050_address, 0x75, 1)[0]

    # did who am I work?
    if whoami == 104: #0x68
        print("MPU-6050 WHOAMI validated!")
    else:
        FATAL_ERROR("ERROR! MPU-6050 WHOAMI failed! '" + str(whoami) + "' returned.")

    # measure gyro bias
    print("Measuring gyro bias...")
    gxs:list[float] = []
    gys:list[float] = []
    gzs:list[float] = []
    started_at_ticks_ms:int = time.ticks_ms()
    while ((time.ticks_ms() - started_at_ticks_ms) / 1000) < 3.0:
        gyro_data = i2c.readfrom_mem(mpu6050_address, 0x43, 6) # read 6 bytes (2 for each axis)
        gyro_x = (translate_pair(gyro_data[0], gyro_data[1]) / 65.5)
        gyro_y = (translate_pair(gyro_data[2], gyro_data[3]) / 65.5)
        gyro_z = (translate_pair(gyro_data[4], gyro_data[5]) / 65.5) * -1 # multiply by -1 because of the way I have it mounted on the quadcopter - it may be upside down. I want a "yaw to the right" to be positive and a "yaw to the left" to be negative.
        gxs.append(gyro_x)
        gys.append(gyro_y)
        gzs.append(gyro_z)
        time.sleep(0.025)
    gyro_bias_x = sum(gxs) / len(gxs)
    gyro_bias_y = sum(gys) / len(gys)
    gyro_bias_z = sum(gzs) / len(gzs)
    print("Gyro bias: " + str((gyro_bias_x, gyro_bias_y, gyro_bias_z)))

    # Set up PWM's
    M1:machine.PWM = machine.PWM(machine.Pin(gpio_motor1))
    M1.freq(250)
    print("Motor PWM's set up @ 250 hz")

def translate_pair(high:int, low:int) -> int:
        """Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
        value = (high << 8) + low
        if value >= 0x8000:
            value = -((65535 - value) + 1)
        return value 

########### RUN THE SCOUT FLIGHT CONTROLLER PROGRAM ###########
run()