#!/usr/bin/env python3
import smbus2
import time
import math

# I2C bus (use 7 for I2C7)
bus = smbus2.SMBus(7)
address = 0x40  # default PCA9685 address

# Registers
MODE1 = 0x00
PRESCALE = 0xFE
LED0_ON_L = 0x06

# Initialize PCA9685
def set_pwm_freq(freq_hz):
    """Set the PWM frequency in Hz"""
    prescale_val = 25000000.0    # 25 MHz oscillator
    prescale_val /= 4096.0       # 12-bit
    prescale_val /= float(freq_hz)
    prescale_val -= 1.0
    prescale = math.floor(prescale_val + 0.5)

    oldmode = bus.read_byte_data(address, MODE1)
    newmode = (oldmode & 0x7F) | 0x10  # sleep
    bus.write_byte_data(address, MODE1, newmode)       # go to sleep
    bus.write_byte_data(address, PRESCALE, prescale)   # set prescaler
    bus.write_byte_data(address, MODE1, oldmode)
    time.sleep(0.005)
    bus.write_byte_data(address, MODE1, oldmode | 0x80)  # restart

def set_pwm(channel, on, off):
    """Set PWM pulse for one channel"""
    reg = LED0_ON_L + 4 * channel
    bus.write_byte_data(address, reg, on & 0xFF)
    bus.write_byte_data(address, reg+1, (on >> 8) & 0xFF)
    bus.write_byte_data(address, reg+2, off & 0xFF)
    bus.write_byte_data(address, reg+3, (off >> 8) & 0xFF)

def set_servo_angle(channel, angle):
    """Move servo to angle (0-180)"""
    pulse_min = 1000  # µs
    pulse_max = 2000  # µs
    pulse_len = 1000000.0 / 50.0 / 4096.0  # µs per bit at 50Hz
    pulse = pulse_min + (pulse_max - pulse_min) * (angle / 180.0)
    ticks = int(pulse / pulse_len)
    set_pwm(channel, 0, ticks)

# ---- MAIN TEST ----
set_pwm_freq(50)  # 50 Hz for servos

channel = 1  # the channel you are actually using
print(f"Sweeping servo on channel {channel}...")

try:
    while True:
        for angle in range(10, 181, 10):
            set_servo_angle(channel, angle)
            time.sleep(0.05)
        for angle in range(180, -1, -10):
            set_servo_angle(channel, angle)
            time.sleep(0.05)
except KeyboardInterrupt:
    print("\nStopped by user")
finally:
    # Optionally set channel off
    set_pwm(channel, 0, 0)
    bus.close()