import machine
import math
import time

# Define PWM pins
PWM1 = machine.PWM(machine.Pin(0))
PWM2 = machine.PWM(machine.Pin(1))
PWM3 = machine.PWM(machine.Pin(2))
PWM4 = machine.PWM(machine.Pin(3))
PWM5 = machine.PWM(machine.Pin(4))
PWM6 = machine.PWM(machine.Pin(5))

# Set PWM frequency to 10 kHz for all PWM pins
PWM1.freq(10000)
PWM2.freq(10000)
PWM3.freq(10000)
PWM4.freq(10000)
PWM5.freq(10000)
PWM6.freq(10000)

# Motor parameters
rated_speed_rpm = 4000

# Function to calculate duty cycle
def calculate_duty_cycle(angle, d_ref, q_ref):
    sector = int(angle / (2 * math.pi / 6)) % 6  # Ensure sector index stays within range 0-5
    sin_values = [math.sin(sector * (2 * math.pi / 6) + i * (2 * math.pi / 6) / 100) for i in range(101)]
    print("Sin values:", sin_values)  # Print the sine values
    t1 = sin_values[(sector + 1) % 6]
    t2 = sin_values[sector]
    t3 = (sin_values[(sector + 2) % 6] - t2) / math.sqrt(3)
    duty_u = (t1 + t2 + t3) / 2.0
    duty_v = (t1 - t2 - t3) / 2.0
    duty_w = -duty_u - duty_v
    
    # Inverse Park-Clark transformation
    alpha = duty_u
    beta = (duty_v + duty_w) * math.sqrt(3) / 2
    phase_voltage_d = d_ref * math.cos(angle) - q_ref * math.sin(angle)
    phase_voltage_q = d_ref * math.sin(angle) + q_ref * math.cos(angle)
    duty_u, duty_v, duty_w = alpha * phase_voltage_d + beta * phase_voltage_q, (2/3) * beta * phase_voltage_q, -(1/3) * alpha * phase_voltage_d - (1/3) * beta * phase_voltage_q
    
    return duty_u, duty_v, duty_w

# Main loop
while True:
    # Read speed from serial input
    speed = input("Enter speed in RPM: ")
    try:
        speed = int(speed)
    except ValueError:
        print("Invalid input. Please enter a valid integer.")
        continue
    
    # Calculate reference d-q voltages based on input speed
    d_ref = speed / rated_speed_rpm
    q_ref = 0  # For simplicity, we keep q_ref constant
    
    # Calculate angle increment based on input speed
    angle_increment = (2 * math.pi * speed) / 60
    
    # Main control loop
    start_time = time.ticks_ms()
    while time.ticks_diff(time.ticks_ms(), start_time) < 10000:  # Run for 10 seconds
        angle = 0
        while True:
            # Calculate duty cycles for each phase
            duty_u, duty_v, duty_w = calculate_duty_cycle(angle, d_ref, q_ref)
        
            # Set duty cycles for PWM outputs
            PWM1.duty_ns(int(duty_u * (1 / PWM1.freq()) * 1e9))
            PWM2.duty_ns(int(duty_v * (1 / PWM2.freq()) * 1e9))
            PWM3.duty_ns(int(duty_w * (1 / PWM3.freq()) * 1e9))
            PWM4.duty_ns(int(duty_w * (1 / PWM4.freq()) * 1e9))  # In a three-phase inverter, PWM4 is complementary to PWM3
            PWM5.duty_ns(int(duty_v * (1 / PWM5.freq()) * 1e9))  # In a three-phase inverter, PWM5 is complementary to PWM2
            PWM6.duty_ns(int(duty_u * (1 / PWM6.freq()) * 1e9))  # In a three-phase inverter, PWM6 is complementary to PWM1
            
            # Increment angle
            angle += angle_increment
            if angle >= 2 * math.pi:
                angle -= 2 * math.pi
            
            # Check if 10 seconds have elapsed
            if time.ticks_diff(time.ticks_ms(), start_time) >= 10000:
                break  # Break out of the inner loop after 10 seconds
    
    print("Motor stopped. Enter a new speed.")