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

#define SERVO_PIN 15

// Speed: -100 = full CCW, 0 = stop, 100 = full CW
void set_servo_speed(uint slice, uint channel, float speed) {
    float min_pulse = 1000.0f; // 1ms pulse = full CCW
    float max_pulse = 2000.0f; // 2ms pulse = full CW
    float stop_pulse = 1500.0f; // 1.5ms = stop

    float pulse;
    if (speed > 0)
        pulse = stop_pulse + (speed / 100.0f) * (max_pulse - stop_pulse);
    else
        pulse = stop_pulse + (speed / 100.0f) * (stop_pulse - min_pulse);

    uint16_t level = (uint16_t)((pulse / 20000.0f) * 65535);
    pwm_set_chan_level(slice, channel, level);
}

int main() {
    stdio_init_all();

    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(SERVO_PIN);
    uint channel = pwm_gpio_to_channel(SERVO_PIN);

    // Standard servo PWM: 50Hz (20ms)
    pwm_set_clkdiv(slice, 125.0f);
    pwm_set_wrap(slice, 20000);
    pwm_set_enabled(slice, true);

    while (true) {
        set_servo_speed(slice, channel, 100);  // Full CW
        sleep_ms(3000);
        set_servo_speed(slice, channel, -100); // Full CCW
        sleep_ms(3000);
        set_servo_speed(slice, channel, 0);    // Stop
        sleep_ms(2000);
    }
}