from machine import Pin, I2C, ADC, Timer, PWM
from i2c_lcd import I2cLcd
from time import sleep
import random
import utime

flag = 0

# GPIO pins for 7-segment display segments (a-g)
segments = [
    Pin(7, Pin.OUT),
    Pin(8, Pin.OUT),
    Pin(2, Pin.OUT),
    Pin(3, Pin.OUT),
    Pin(4, Pin.OUT),
    Pin(5, Pin.OUT),
    Pin(6, Pin.OUT)
]

# Pin states for each digit to display numbers 0-9
number_map = [
    [1, 1, 1, 1, 1, 1, 0],  # 0
    [0, 1, 1, 0, 0, 0, 0],  # 1
    [1, 1, 0, 1, 1, 0, 1],  # 2
    [1, 1, 1, 1, 0, 0, 1],  # 3
    [0, 1, 1, 0, 0, 1, 1],  # 4
    [1, 0, 1, 1, 0, 1, 1],  # 5
    [1, 0, 1, 1, 1, 1, 1],  # 6
    [1, 1, 1, 0, 0, 0, 0],  # 7
    [1, 1, 1, 1, 1, 1, 1],  # 8
    [1, 1, 1, 1, 0, 1, 1]   # 9
]

# Function to display a specific number on the 7-segment display
def display_number(number):
    segments_values = number_map[number]
    for i in range(len(segments)):
        segments[i].value(segments_values[i])

# Servo pins and PWM setup
servo_pins = [Pin(15), Pin(14), Pin(13), Pin(12)]  # Four servos connected to these pins
servo_pwm = [PWM(servo_pin) for servo_pin in servo_pins]  # Create PWM instances for each servo

# Set the frequency for each servo (50Hz is standard for most servos)
for pwm in servo_pwm:
    pwm.freq(50)

# Function to move all servos
def remove_rafter(i):
    servo_pwm[i].duty_u16(3227)  # Move each rafter to its "open" position
    print(f"Rafter {i + 1} removed!")
    sleep(0.5)  # Wait before moving the next rafter
    servo_pwm[i].duty_u16(6553)

# LCD setup
sda = Pin(0)
scl = Pin(1)
i2c = I2C(0, sda=sda, scl=scl, freq=400000)
devices = i2c.scan()
idcaddr = devices[0]
lcd = I2cLcd(i2c, idcaddr, 2, 16)

# Timer callback function to update LCD
def update_lcd(timer):
    lcd.clear()
    lcd.putstr("TAKE OFF")

# Main loop
i = 0  # Initialize the servo index here

while True:
    for number in range(10):
        display_number(9 - number)
        if(number==9):
            break
        if ((9 - number) % 2 == 0):  # Condition to open the rafter
            remove_rafter(i)  # Pass the correct index to the remove_rafter function
            sleep(0.5)
            i += 1  # Increment the rafter index
        utime.sleep(1)  # Delay between numbers

    flag = 1
    break

# Start the timer once the flag is set
if flag == 1:
    timer = Timer()
    timer.init(period=1000, mode=Timer.PERIODIC, callback=update_lcd)