#include "pico/stdlib.h"
#include "hardware/pwm.h"

// Servo pin and PWM settings
#define SERVO_PIN 22        // Servo motor pin
#define PWM_FREQ 50         // 50Hz (20ms period)
#define MIN_PULSE 500       // 0° (500µs)
#define MAX_PULSE 2400      // 180° (2400µs)
#define STEP_ANGLE 1        // Angle step for smooth movement
#define STEP_DELAY 15       // Delay between steps (ms)
#define PAUSE_MS 1000       // Pause at 0° and 180° (ms)

// Initialize PWM for servo
void servo_init(uint slice_num) {
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
    // Set clock divider for 125MHz system clock to achieve 50Hz
    pwm_set_clkdiv(slice_num, 100.0f); // 125MHz / 100 = 1.25MHz
    pwm_set_wrap(slice_num, 25000);    // 1.25MHz / 50Hz = 25000 cycles
    pwm_set_enabled(slice_num, true);
}

// Set servo angle (0° to 180°)
void servo_set_angle(uint slice_num, uint channel, float angle) {
    // Map angle (0-180) to pulse width (500-2400µs)
    uint32_t pulse = MIN_PULSE + (uint32_t)((angle / 180.0f) * (MAX_PULSE - MIN_PULSE));
    // Convert pulse width to PWM level (25000 wrap = 20ms)
    uint32_t level = (pulse * 25000) / 20000;
    pwm_set_chan_level(slice_num, channel, level);
}

// Smooth sweep from start_angle to end_angle
void servo_sweep(uint slice_num, uint channel, float start_angle, float end_angle) {
    float step = (start_angle < end_angle) ? STEP_ANGLE : -STEP_ANGLE;
    for (float angle = start_angle; 
         (step > 0) ? (angle <= end_angle) : (angle >= end_angle); 
         angle += step) {
        servo_set_angle(slice_num, channel, angle);
        sleep_ms(STEP_DELAY);
    }
}

int main() {
    stdio_init_all();
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
    uint channel = pwm_gpio_to_channel(SERVO_PIN);
    servo_init(slice_num);

    while (true) {
        // Sweep from 0° to 180°
        servo_sweep(slice_num, channel, 0, 180);
        sleep_ms(PAUSE_MS); // Brief pause at 180°

        // Sweep from 180° to 0°
        servo_sweep(slice_num, channel, 180, 0);
        sleep_ms(PAUSE_MS); // Brief pause at 0°
    }
}