from machine import Pin, PWM, SoftI2C
import mpu6050
import time


def main():
    led_gre = Pin(4, Pin.OUT, value = False)
    led_gre.value

    sld_main = Pin(17, Pin.IN, Pin.PULL_UP) # 1 Off --- 0 ON

    btn_gre = Pin(18, Pin.IN, Pin.PULL_UP) # 1 NOT PRESSED --- 0 PRESSED
    btn_blu = Pin(5, Pin.IN, Pin.PULL_UP)  # 1 NOT PRESSED --- 0 PRESSED

    i2c  = SoftI2C(scl=Pin(22), sda=Pin(21))
    acc  = mpu6050.accel(i2c)
    
    g_X, g_Y, g_Z = 0.0, 0.0, 0.0
    Flight_Data_X, Flight_Data_Y, Flight_Data_Z = [], [], []
    bool_writeCSV = False
    
    servo = PWM(Pin(32), freq = 50)
    servo_value = 75 # 25 (Horizitnal / Locked) --- 75 (Vertikal / Unlocked)
    servo.duty(servo_value)
    time.sleep(1)
    
    print(' STANDBY')
    print(' ')

    while sld_main.value() == 1:
        print("True")
        time.sleep(1)

    print(' ACTIVE')
    print(' ')
    print(' Testing Flight Computer:')
    time.sleep(2)
    g_X, g_Y, g_Z = fct_acc_val(acc.get_values(), Flight_Data_X, Flight_Data_Y, Flight_Data_Z)
    if isinstance(g_X, (int, float)):
        print(' X-Value: VALID')
    else:
        print(' X-Value: INVALID')
        abort = True

    time.sleep(1)
    
    if isinstance(g_Y, (int, float)):
        print(' Y-Value: VALID')
    else:
        print(' Y-Value: INVALID')
        abort = True
        
    time.sleep(1)
    
    if isinstance(g_Z, (int, float)):
        print(' Z-Value: VALID')
    else:
        print(' Z-Value: INVALID')
        abort = True
    
    print(' ')
    time.sleep(2)

    print(' Testing Parachute Actuator:')
    time.sleep(2)
    print(' LOCKING...')
    time.sleep(1)
    for duty in range(75,24,-1):
            servo.duty(duty)
            time.sleep_ms(5)
    time.sleep(2)
    print(' UNLOCKING...')
    time.sleep(1)
    for duty in range(25,76,1):
            servo.duty(duty)
            time.sleep_ms(5)
    time.sleep(2)
    print(' ')

    while True:
        g_X, g_Y, g_Z = fct_acc_val(acc.get_values(), Flight_Data_X, Flight_Data_Y, Flight_Data_Z)
        
        print(str(g_X)+' g, '+str(g_Y)+' g, '+str(g_Z)+' g')
        time.sleep(1)
        servo.duty(servo_value)
        time.sleep(1)
        for i in range(0, 2, 1):
            led_gre.value(led_gre.value() ^ 1)
            time.sleep(0.5)

        for duty in range(25,76,1):
            servo.duty(duty)
            time.sleep_ms(5)


def fct_acc_val(acc_values, Flight_Data_X, Flight_Data_Y, Flight_Data_Z):
    a_X = acc_values['AcX']
    a_Y = acc_values['AcY']
    a_Z = acc_values['AcZ']
    g_X = a_X / 16384
    g_Y = a_Y / 16384
    g_Z = a_Z / 16384
    Flight_Data_X.append(f'{g_X:.f} g')
    Flight_Data_Y.append(f'{g_Y:.f} g')
    Flight_Data_Z.append(f'{g_Z:.f} g')
    #print("x: "+str(round(g_X, 2))+"g || y: "+str(round(g_Y, 2))+"g || z: "+str(round(g_Z,2))+"g")
    
    return g_X, g_Y, g_Z



if __name__ == "__main__":
    main()