#import library
from machine import Pin
from machine import PWM
from machine import I2C
import utime
from lcd_api import LcdApi
from pico_i2c_lcd import I2cLcd

# creating an I2C object, specifying the data (SDA) and clock (SCL) pins used in the Raspberry Pi Pico
# any SDA and SCL pins in the Raspberry Pi Pico can be used (check documentation for SDA and SCL pins)
i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000)

# getting I2C address
I2C_ADDR = i2c.scan()[0]
# creating an LCD object using the I2C address and specifying number of rows and columns in the LCD
# LCD number of rows = 2, number of columns = 16
lcd = I2cLcd(i2c, I2C_ADDR, 2, 16)

class Servo():
    MAX_DUTY_CYCLE = 2**16 - 1 #largest unsigned int (65535)
    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  #Set duty to 0.5 milliseconds
    SERVO_GO_STRAIGHT_DUTY = 1.5 #Set duty to 1.5 milliseconds
    SERVO_LEFT_TURN_DUTY = 2.48   #Set duty to 2.5 milliseconds
    SERVO_NORTH_EAST_DUTY = 1.0  #Set duty to 1.0 milliseconds
    SERVO_NORTH_WEST_DUTY = 2.0  #Set duty to 1.0 milliseconds
    
    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):
        # I know that 2.5 miliseconds of frequency 50 is 180 degree 
        # and .5 milisec is 0 degree
        # so the angle input makes: 
        self.duty_cycle_period = ((angle/180)*(2.5 -0.5))+0.5
        #print(self.duty_cycle_period)
        self.duty_cycle_percentage = (self.duty_cycle_period/Servo.SERVO_PERIOD_IN_MILLISECONDS) * 100
        #print(self.duty_cycle_percentage) 
        self.duty_cycle = Servo.MAX_DUTY_CYCLE * (self.duty_cycle_percentage/100)
        #print(self.duty_cycle)
    
        self.pwm_pin.duty_u16(int(self.duty_cycle))
    
    
    def print_settings(self):
        print("Servo PMW 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)

#END_CLASS     

#Need a 20 millisecond period
#1000 Cycles/Second = 1 Millisecond/Cycle (1/1000)
#50 Cycles/Second = 20 Milliseconds/Cycle (1/50)
servo = Servo(Pin(16), Servo.SERVO_FREQUENCY)

#while True:
    
    #print("Servo is going straight...")   
    #servo.set_position_by_duty_cycle(Servo.SERVO_GO_STRAIGHT_DUTY)
    #servo.print_settings()

    #print("Servo is going right...")
    #servo.set_position_by_duty_cycle(Servo.SERVO_RIGHT_TURN_DUTY)
    #servo.print_settings()

    #print("Servo is going left...")
    #servo.set_position_by_duty_cycle(Servo.SERVO_LEFT_TURN_DUTY)
    #servo.print_settings()
    #angle = 30
    #print("Servo is moved to:")
    #print(angle)
    #servo.set_position_by_angle(angle)
    #servo.print_settings()
#open file and split into tuple by line
with open('file.txt', 'r') as f:
    file_content = [tuple(map(int, line.split(','))) for line in f]
#for each line, do the operation
for record in file_content:
    angle, duration = record #indexing the value to angle and time
    message = f"Moving servo to {angle} degrees for {duration} seconds.  \t\t\t "
    print(message)
    lcd.putstr(message)
    if angle == 180:
        angle = 178
    servo.set_position_by_angle(angle)#use the functions
    utime.sleep(duration)
    lcd.clear()