from machine import Pin, I2C
from machine import PWM
import utime
from time import sleep
from pico_i2c_lcd import I2cLcd

class Servo():
    MAX_DUTY_CYCLE = 2**16 - 1
    SLEEP_PERIOD_IN_SECONDS = 2
    MILLISECONDS_PER_SECOND = 1000
    SERVO_PERIOD_IN_MILLISECONDS = 20
    SERVO_FREQUENCY = int(MILLISECONDS_PER_SECOND / SERVO_PERIOD_IN_MILLISECONDS)
    SERVO_RIGHT_TURN_DUTY = 0.5
    SERVO_GO_STRAIGHT_DUTY = 1.5
    SERVO_LEFT_TURN_DUTY = 2.4
    SERVO_NORTH_EAST_DUTY = 1.0 # 45 degrees
    SERVO_NORTH_WEST_DUTY = 2.0 # 135 degrees

    def __init__(self, pin, frequency=50):
        self.pwm_pin = PWM(pin)
        self.pwm_pin.freq(frequency)
        self.duty_cycle = 0

    def set_position_by_duty_cycle(self, duty_cycle_period):
        self.duty_cycle_period = duty_cycle_period
        self.duty_cycle_percentage = (duty_cycle_period / Servo.SERVO_PERIOD_IN_MILLISECONDS) * 100
        self.duty_cycle = Servo.MAX_DUTY_CYCLE * (self.duty_cycle_percentage / 100)
        self.pwm_pin.duty_u16(int(self.duty_cycle))

    def set_position_by_angle(self, angle):
        if angle == 90:
            self.set_position_by_duty_cycle(self.SERVO_GO_STRAIGHT_DUTY)
        elif angle == 180:
            self.set_position_by_duty_cycle(self.SERVO_RIGHT_TURN_DUTY)
        elif angle == 0:
            self.set_position_by_duty_cycle(self.SERVO_LEFT_TURN_DUTY)
        elif angle == 45:
            self.set_position_by_duty_cycle(self.SERVO_NORTH_WEST_DUTY)
        elif angle == 135:
            self.set_position_by_duty_cycle(self.SERVO_NORTH_EAST_DUTY)

    def print_settings(self):
        print("Servo PWM Frequency: " + str(Servo.SERVO_FREQUENCY))
        print("Servo PWM Period (milliseconds): " + str(Servo.SERVO_PERIOD_IN_MILLISECONDS))
        print("Duty (milliseconds): " + str(self.duty_cycle_period))
        print("Duty/Period percentage: " + str(self.duty_cycle_percentage))
        print("Duty value (1:65535): " + str(self.duty_cycle))
        print("\n")
        utime.sleep(Servo.SLEEP_PERIOD_IN_SECONDS)

i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)
I2C_ADDR = i2c.scan()[0]
lcd = I2cLcd(i2c, I2C_ADDR, 4, 20)

#Example usage:
servo = Servo(Pin(16), Servo.SERVO_FREQUENCY)
with open('servo_records.txt', 'r') as file:
    for line in file:
        parts = line.strip().split(',')
        if len(parts) != 2:
            continue
        
        angle = int(parts[0])
        duration = float(parts[1])

        lcd.clear()
        #print(f"Moving servo to {angle} degrees for {duration} seconds...")
        lcd.putstr(f"Moving servo to {angle}\n degrees for {duration}\n seconds ")
        servo.set_position_by_angle(angle)
        servo.print_settings()
    
        utime.sleep(duration)