from machine import Pin, PWM
import math
import time

# Servo setup
servo = PWM(Pin(15))
servo.freq(50)

# Target point
target_x = 2
target_y = 2

# Initial direction vector (pointing right)
current_x = 1
current_y = 0

# Normalize vector
def normalize(x, y):
    length = math.sqrt(x**2 + y**2)
    return (x / length, y / length)

# Dot product
def dot(x1, y1, x2, y2):
    return x1 * x2 + y1 * y2

# Convert angle to servo PWM
def set_servo_angle(angle):
    min_duty = 1638    # ~0.5ms
    max_duty = 8192    # ~2.5ms
    duty_u16 = int(min_duty + (angle / 180) * (max_duty - min_duty))
    servo.duty_u16(duty_u16)

# Main logic
cx, cy = normalize(current_x, current_y)
tx, ty = normalize(target_x, target_y)

dp = dot(cx, cy, tx, ty)
angle_rad = math.acos(dp)
angle_deg = math.degrees(angle_rad)

print("Rotating servo to", round(angle_deg, 1), "degrees")
set_servo_angle(angle_deg)
time.sleep(3)