from machine import Pin, PWM, I2C
import utime

class LCD:
    def __init__(self, addr=39, blen=1):
        sda = Pin(0)
        scl = Pin(1)
        self.bus = I2C(0, sda=sda, scl=scl, freq=400000)
        self.addr = addr
        self.blen = blen
        self.send_command(0x33)  # Must initialize to 8-line mode at first
        utime.sleep(0.005)
        self.send_command(0x32)  # Then initialize to 4-line mode
        utime.sleep(0.005)
        self.send_command(0x28)  # 2 Lines & 5*7 dots
        utime.sleep(0.005)
        self.send_command(0x0C)  # Enable display without cursor
        utime.sleep(0.005)
        self.send_command(0x01)  # Clear Screen
        self.bus.writeto(self.addr, bytearray([0x08]))
    
    def write_word(self, data):
        temp = data
        if self.blen == 1:
            temp |= 0x08
        else:
            temp &= 0xF7
        self.bus.writeto(self.addr, bytearray([temp]))
    
    def send_command(self, cmd):
        # Send bit7-4 firstly
        buf = cmd & 0xF0
        buf |= 0x04  # RS = 0, RW = 0, EN = 1
        self.write_word(buf)
        utime.sleep(0.002)
        buf &= 0xFB  # Make EN = 0
        self.write_word(buf)
        # Send bit3-0 secondly
        buf = (cmd & 0x0F) << 4
        buf |= 0x04  # RS = 0, RW = 0, EN = 1
        self.write_word(buf)
        utime.sleep(0.002)
        buf &= 0xFB  # Make EN = 0
        self.write_word(buf)
    
    def send_data(self, data):
        # Send bit7-4 firstly
        buf = data & 0xF0
        buf |= 0x05  # RS = 1, RW = 0, EN = 1
        self.write_word(buf)
        utime.sleep(0.002)
        buf &= 0xFB  # Make EN = 0
        self.write_word(buf)
        # Send bit3-0 secondly
        buf = (data & 0x0F) << 4
        buf |= 0x05  # RS = 1, RW = 0, EN = 1
        self.write_word(buf)
        utime.sleep(0.002)
        buf &= 0xFB  # Make EN = 0
        self.write_word(buf)
    
    def clear(self):
        self.send_command(0x01)  # Clear Screen
    
    def openlight(self):  # Enable the backlight
        self.bus.writeto(self.addr, bytearray([0x08]))
    
    def write(self, x, y, str):
        if x < 0:
            x = 0
        if x > 15:
            x = 15
        if y < 0:
            y = 0
        if y > 1:
            y = 1
        # Move cursor
        addr = 0x80 + 0x40 * y + x
        self.send_command(addr)
        for chr in str:
            self.send_data(ord(chr))
    
    def message(self, text):
        for char in text:
            if char == '\n':
                self.send_command(0xC0)  # next line
            else:
                self.send_data(ord(char))

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.47  # Set duty to 2.5 milliseconds
    
    def __init__(self, pin, frequency=50, lcd=None):
        self.pwm_pin = PWM(pin)
        self.pwm_pin.freq(frequency)
        self.duty_cycle = 0
        self.lcd = lcd
        
    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):
        duty_cycle_period = (angle / 90) + 0.47
        self.set_position_by_duty_cycle(duty_cycle_period)
    
    def print_settings(self):
        output = (
            "Frequency: {}\n"
            "Period(ms): {}\n"
            "Duty-P(ms): {:.2f}\n"
            "Duty(%): {:.2f}\n"
            "Duty-value: {}\n"
        ).format(
            Servo.SERVO_FREQUENCY, # modifies output with our frequency value.
            Servo.SERVO_PERIOD_IN_MILLISECONDS, # modifies output with period value.
            self.duty_cycle_period, # modifies output with duty cycle period.
            self.duty_cycle_percentage,# modifies output with duty cycle percentage.
            int(self.duty_cycle)# modifies output with duty cycle value.
        )
        lines = output.split('\n') # splits lines in output by '\n'
        
        for line in lines:
            print(line) # print line to console.
            if self.lcd:
                self.lcd.clear() # clear screen.
                self.lcd.message(line) # print line to screen.
            utime.sleep(1) # delay to see output.

        utime.sleep(Servo.SLEEP_PERIOD_IN_SECONDS) # overall delay between positional changes.

# Create an LCD object
lcd = LCD(addr=39, blen=1)

# Create a Servo object with LCD
servo = Servo(Pin(16), Servo.SERVO_FREQUENCY, lcd)

# Read from file and control servo
with open("text.txt", "r") as file:  # Open and read text.txt
    for line in file:  # Continue looping while there are still lines to read
        angle = float(line.strip().split(',')[0])  # Get angle from the current line.
        direction = float(line.strip().split(',')[1])  # Get direction from the current line.
        servo.set_position_by_angle(angle)  # Function to change position based on angle.
        servo.print_settings()
file.close()  # Close file.