import math
import time
from machine import Pin, PWM
import esp32_ssd1306

# Define pin assignments
SERVO_PIN = 19
I2C_SDA = 21
I2C_SCL = 22

# Create a PWM object for the servo
servo = PWM(Pin(SERVO_PIN))
servo.freq(50)

# Create an SSD1306 OLED display object
display = esp32_ssd1306.SSD1306_I2C(128, 64, I2C_SDA, I2C_SCL)

# Define some constants
X_ORIGIN = 64
Y_ORIGIN = 60
INCREMENT = 5  # Degrees
DELAY = 100  # Milliseconds
LENGTH = 40  # Pixels

# Declare some variables
angle = 0
delta_x = 0
delta_y = 0
final_x = 0
final_y = 0

def setup():
    # Initialize the serial port
    print("Setting up...")

    # Attach the servo to the specified pin
    servo.duty_ns(int(angle * 1000000 / 180))

    # Initialize the OLED display
    display.init()

    # Clear the display
    display.fill(0)

    # Display text
    display.text("Brazo robotico 1 GDL!", 0, 28, 1, 1)
    display.show()

    # Clear the display again
    display.fill(0)

    # Wait for one second
    time.sleep(1)

def loop():
    # Sweep the servo from 0 to 180 degrees
    for angle in range(0, 181, INCREMENT):
        draw()
        time.sleep_ms(DELAY)

    # Then sweep the servo back from 180 to 0 degrees
    for angle in range(180, -1, -INCREMENT):
        draw()
        time.sleep_ms(DELAY)

def draw():
    # Set the servo angle
    servo.duty_ns(int(angle * 1000000 / 180))

    # Calculate the delta x and y values
    delta_x = -int(LENGTH * math.cos(angle * math.pi / 180))
    delta_y = int(LENGTH * math.sin(angle * math.pi / 180))

    # Calculate the final x and y values
    final_x = X_ORIGIN + delta_x
    final_y = Y_ORIGIN - delta_y

    # Draw a line from the origin to the final x and y coordinates
    display.line(X_ORIGIN, Y_ORIGIN, final_x, final_y, 1)

    # Draw a circle at the final x and y coordinates
    display.circle(final_x, final_y, 2, 1)

    # Draw a rectangle at the origin
    display.rect(X_ORIGIN - 4, Y_ORIGIN, 8, 4, 1)

    # Display the updated display
    display.show()

if __name__ == "__main__":
    setup()
    loop()