from math import sqrt
import machine
from time import sleep_ms, ticks_ms
import pca9685
import motor
from MPU6050 import MPU6050

def motor_move(motors, m1_speed=0, m2_speed=0, m3_speed=0, m4_speed=0):
    sc = 0.5
    motors.speed(index=0, value= int(sc*m1_speed))
    motors.speed(index=1, value= int(sc*m2_speed))
    motors.speed(index=2, value= int(sc*m3_speed))
    motors.speed(index=3, value= int(sc*m4_speed))
    
def accel_calibration(mpu):
    # Initialize variables
    sample_count = 30  # sample to average
    sample_interval = 20   # time between samples
    axc = 0
    ayc = 0
    azc = 0
    gzc = 0
    axsd = 0
    aysd = 0
    azsd = 0
    
    for n in range(sample_count):
    # Accelerometer Data
        accel = mpu.read_accel_data() # read the accelerometer [ms^-2]
        aX = accel["x"]
        aY = accel["y"]
        aZ = accel["z"] - 9.8   # account for gravity
    #     print("ax: " + str(aX) + " ay: " + str(aY) + " az: " + str(aZ))
        gyro = mpu.read_gyro_data()   # read the gyro [deg/s]
        gZ = gyro["z"]
        axc += aX
        ayc += aY
        azc += aZ
        axsd += aX**2
        aysd += aY**2
        azsd += aZ**2
        gzc += gZ
        
    # find mean
    axc = axc / sample_count
    ayc = ayc / sample_count
    azc = (azc / sample_count)
    gzc = (gzc / sample_count)
    
    # find standard deviation
    axsd = sqrt(axsd/sample_count - axc**2)
    aysd = sqrt(aysd/sample_count - ayc**2)
    azsd = sqrt(azsd/sample_count - azc**2)
    
    print(f"Final error aX: {axc:.2f}  +/- {axsd}")
    print(f"Final error aY: {ayc:.2f}  +/- {aysd}")
    print(f"Final error aZ: {azc:.2f}  +/- {azsd}")
    print(f"Final error gZ: {gzc:.2f}")
    
    return axc, ayc, azc, gzc