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

// Convert angle (0–180) to PWM duty cycle in microseconds
uint16_t angle_to_pulse(float angle) {
    float min_us = 500;   // 0 degrees
    float max_us = 2400;  // 180 degrees
    return (uint16_t)(min_us + (angle / 180.0f) * (max_us - min_us));
}

int main() {
    const uint servo_pin = 15;

    stdio_init_all();

    // Set pin function to PWM
    gpio_set_function(servo_pin, GPIO_FUNC_PWM);

    // Get PWM slice number
    uint slice = pwm_gpio_to_slice_num(servo_pin);

    // Configure PWM: 50Hz (20ms period)
    pwm_config config = pwm_get_default_config();
    pwm_config_set_clkdiv(&config, 64.0f);        // slow down clock
    pwm_config_set_wrap(&config, 39062);          // gives 50Hz with div=64
    pwm_init(slice, &config, true);

    while (true) {
        // Move to 0 degrees
        pwm_set_gpio_level(servo_pin, angle_to_pulse(0) * 16);
        sleep_ms(1000);

        // Move to 90 degrees
        pwm_set_gpio_level(servo_pin, angle_to_pulse(90) * 16);
        sleep_ms(1000);

        // Move to 180 degrees
        pwm_set_gpio_level(servo_pin, angle_to_pulse(180) * 16);
        sleep_ms(1000);
    }
}