from machine import Pin, PWM
import time

# Pin definitions
LED_PIN = 2
RGB1_RED_PIN = 3
RGB1_GREEN_PIN = 4
RGB1_BLUE_PIN = 5
RGB2_RED_PIN = 6
RGB2_GREEN_PIN = 7
RGB2_BLUE_PIN = 8
SERVO1_PIN = 9
SERVO2_PIN = 10
SERVO3_PIN = 11

# Initialize pins
led = Pin(LED_PIN, Pin.OUT)

# Initialize RGB LEDs
rgb1_red = PWM(Pin(RGB1_RED_PIN))
rgb1_green = PWM(Pin(RGB1_GREEN_PIN))
rgb1_blue = PWM(Pin(RGB1_BLUE_PIN))
rgb2_red = PWM(Pin(RGB2_RED_PIN))
rgb2_green = PWM(Pin(RGB2_GREEN_PIN))
rgb2_blue = PWM(Pin(RGB2_BLUE_PIN))

# Set PWM frequency for RGB LEDs
rgb1_red.freq(1000)
rgb1_green.freq(1000)
rgb1_blue.freq(1000)
rgb2_red.freq(1000)
rgb2_green.freq(1000)
rgb2_blue.freq(1000)

# Initialize servo motors
servo1 = PWM(Pin(SERVO1_PIN))
servo2 = PWM(Pin(SERVO2_PIN))
servo3 = PWM(Pin(SERVO3_PIN))

# Set PWM frequency for servo motors
servo1.freq(50)  # 50Hz is standard for servos
servo2.freq(50)
servo3.freq(50)

# Define rainbow colors
RAINBOW_COLORS = [
    (255, 0, 0),    # Red
    (0, 255, 0),    # Green
    (0, 0, 255),    # Blue
    (255, 255, 0),  # Yellow
    (0, 255, 255),  # Cyan
    (255, 0, 255),  # Magenta
    (255, 255, 255) # White
]

# Helper function to map values (similar to Arduino's map function)
def map_value(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) // (in_max - in_min) + out_min

# Helper function to set servo angle (0-180 degrees)
def set_servo_angle(servo_pwm, angle):
    # Map angle 0-180 to pulse width 1000-2000 microseconds
    pulse_width = map_value(angle, 0, 180, 1000, 2000)
    # Convert to duty cycle (0-65535)
    duty = map_value(pulse_width, 0, 20000, 0, 65535)
    servo_pwm.duty_u16(duty)

# Helper function to set RGB LED colors
def set_rgb_color(red, green, blue):
    # Map 0-255 to 0-65535 for PWM duty cycle
    r_duty = map_value(red, 0, 255, 0, 65535)
    g_duty = map_value(green, 0, 255, 0, 65535)
    b_duty = map_value(blue, 0, 255, 0, 65535)
    
    rgb1_red.duty_u16(r_duty)
    rgb1_green.duty_u16(g_duty)
    rgb1_blue.duty_u16(b_duty)
    rgb2_red.duty_u16(r_duty)
    rgb2_green.duty_u16(g_duty)
    rgb2_blue.duty_u16(b_duty)

# Initialize global variables
current_color_index = 0
led_state = False
servo_direction = 1

# Set initial positions
set_servo_angle(servo1, 0)
set_servo_angle(servo2, 180)
set_servo_angle(servo3, 90)

# Set initial RGB color
set_rgb_color(RAINBOW_COLORS[current_color_index][0], 
              RAINBOW_COLORS[current_color_index][1], 
              RAINBOW_COLORS[current_color_index][2])
current_color_index = (current_color_index + 1) % 7

# Initialize time tracking
led_millis = time.ticks_ms()
rgb_millis = time.ticks_ms()
servo_millis = time.ticks_ms()
interval = 2000  # 2 seconds

# Main loop
try:
    while True:
        current_time = time.ticks_ms()
        
        # Check if it's time to toggle the LED
        if time.ticks_diff(current_time, led_millis) >= interval:
            led_millis = current_time
            led_state = not led_state
            led.value(led_state)
        
        # Check if it's time to update RGB LEDs
        if time.ticks_diff(current_time, rgb_millis) >= interval:
            rgb_millis = current_time
            set_rgb_color(RAINBOW_COLORS[current_color_index][0], 
                         RAINBOW_COLORS[current_color_index][1], 
                         RAINBOW_COLORS[current_color_index][2])
            current_color_index = (current_color_index + 1) % 7
        
        # Check if it's time to update servo positions
        if time.ticks_diff(current_time, servo_millis) >= interval:
            servo_millis = current_time
            if servo_direction == 1:
                set_servo_angle(servo1, 180)
                set_servo_angle(servo2, 0)
                set_servo_angle(servo3, 135)
            else:
                set_servo_angle(servo1, 0)
                set_servo_angle(servo2, 180)
                set_servo_angle(servo3, 90)
            servo_direction *= -1
            
        time.sleep(0.01)  # Small delay to prevent busy-waiting
except KeyboardInterrupt:
    # Clean up on exit
    pass