#include <hardware/structs/clocks.h>
#include <stdint.h>
#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/clocks.h"

#define ROTATE_180 2500
#define ROTATE_0 500


void setup_servo(uint pin) {
    // Initialize the GPIO pin for PWM output
    gpio_set_function(pin, GPIO_FUNC_PWM);
    
    // Get the PWM slice for the given pin
    uint slice_num = pwm_gpio_to_slice_num(pin);

    // Set the frequency and wrap (period)
    uint32_t clk  = clock_get_hz(clk_sys);
    uint32_t div  = clk / 20000/ 50;
    pwm_set_clkdiv_int_frac4(slice_num,div,1);  // Set the clock divider
    pwm_set_wrap(slice_num, 20000);

    // Enable the PWM
    pwm_set_enabled(slice_num, true);
}

void set_degree(uint pin, float deg) {

    int duty = (((float)(ROTATE_180 - ROTATE_0) / 180.0) * deg) + ROTATE_0;
    pwm_set_gpio_level(pin, duty);
}

int main()
{
    stdio_init_all();

    setup_servo(2);
    setup_servo(3);
    setup_servo(4);
    setup_servo(5);
    setup_servo(6);
    setup_servo(7);
    setup_servo(8);
    setup_servo(9);
    setup_servo(10);
    setup_servo(11);
    setup_servo(12);
    while (true) {
        printf("Hello, world!\n");
        sleep_ms(100);
        set_degree(2, 0);
        set_degree(3, 20);
        set_degree(4, 30);
        set_degree(5, 40);
        set_degree(6, 50);
        set_degree(7, 60);
        set_degree(8, 70);
        set_degree(9, 80);
        set_degree(10, 90);
        set_degree(11, 100);
        set_degree(12, 180);
    }
}