from machine import Pin, PWM
import time

# Define constants
LSBFIRST = 0
MSBFIRST = 1

# Pin definitions
LED1_PIN = 2
LED2_PIN = 3
SHCP_PIN = 4
STCP_PIN = 5
DS_PIN = 6
SERVO1_PIN = 7
SERVO2_PIN = 8

# Set up LEDs
led1 = Pin(LED1_PIN, Pin.OUT)
led2 = Pin(LED2_PIN, Pin.OUT)

# Set up shift register pins
shcp = Pin(SHCP_PIN, Pin.OUT)
stcp = Pin(STCP_PIN, Pin.OUT)
ds = Pin(DS_PIN, Pin.OUT)

# Set up servos
servo1 = PWM(Pin(SERVO1_PIN))
servo2 = PWM(Pin(SERVO2_PIN))
servo1.freq(50)  # Standard servo frequency
servo2.freq(50)

# Segment codes for 0-9 digits (common anode)
segment_codes = [
    0b11000000,  # 0
    0b11111001,  # 1
    0b10100100,  # 2
    0b10110000,  # 3
    0b10011001,  # 4
    0b10010010,  # 5
    0b10000010,  # 6
    0b11111000,  # 7
    0b10000000,  # 8
    0b10010000   # 9
]

# Initialize variables
current_number = 0
previous_millis_led = 0
previous_millis_display = 0
previous_millis_servo = 0
is_servo_paused = False
pause_start_time = 0
led1_state = False  # Equivalent to Arduino's LOW
led2_state = True   # Equivalent to Arduino's HIGH

# Function to convert servo angle to duty cycle
def angle_to_duty(angle):
    # Map angle from 0-180 to duty cycle for servos
    # For most servos, a pulse width of 1ms (duty 5%) to 2ms (duty 10%) is used
    # With 16-bit resolution, this corresponds to values between 3277 and 6554
    min_duty = 3277  # 1ms pulse at 50Hz (5% duty)
    max_duty = 6554  # 2ms pulse at 50Hz (10% duty)
    return int(min_duty + (max_duty - min_duty) * angle / 180)

# Function to shift out data to shift register
def shift_out(data_pin, clock_pin, bit_order, value):
    for i in range(8):
        if bit_order == MSBFIRST:
            data_pin.value((value & (1 << (7 - i))) > 0)
        else:  # LSBFIRST
            data_pin.value((value & (1 << i)) > 0)
        clock_pin.value(1)
        clock_pin.value(0)

# Function to display a number on the 7-segment display
def display_number(number):
    code = segment_codes[number]
    shift_out(ds, shcp, MSBFIRST, code)
    stcp.value(1)
    stcp.value(0)

# Initial setup
led1.value(led1_state)
led2.value(led2_state)
display_number(current_number)

# Initialize servo positions
pos1 = 0
pos2 = 180
dir1 = 1
dir2 = -1
servo1.duty_u16(angle_to_duty(pos1))
servo2.duty_u16(angle_to_duty(pos2))

# Main loop
while True:
    current_millis = time.ticks_ms()
    
    # LED blink logic
    if time.ticks_diff(current_millis, previous_millis_led) >= 2000:
        previous_millis_led = current_millis
        led1_state = not led1_state
        led2_state = not led2_state
        led1.value(led1_state)
        led2.value(led2_state)
    
    # 7-segment display counter logic
    if time.ticks_diff(current_millis, previous_millis_display) >= 2000:
        previous_millis_display = current_millis
        current_number = (current_number + 1) % 10
        display_number(current_number)
    
    # Servo movement logic
    if not is_servo_paused:
        if time.ticks_diff(current_millis, previous_millis_servo) >= 20:
            previous_millis_servo = current_millis
            
            pos1 += dir1
            pos2 += dir2
            
            if pos1 >= 180 or pos1 <= 0:
                dir1 = -dir1
            
            if pos2 >= 180 or pos2 <= 0:
                dir2 = -dir2
                is_servo_paused = True
                pause_start_time = current_millis
            
            servo1.duty_u16(angle_to_duty(pos1))
            servo2.duty_u16(angle_to_duty(pos2))
    
    elif time.ticks_diff(current_millis, pause_start_time) >= 2000:
        is_servo_paused = False