#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)

typedef struct servo_config {
    uint32_t slice_num;
    uint32_t channel;
    float current_angle;
} ServoConfig;

ServoConfig servo_config;

// Define a configuração para o servomotor
void set_config_servo(uint32_t slice_num, uint32_t channel){
    servo_config.slice_num = slice_num;
    servo_config.channel = channel;
}

// Inicializa o PWM para o servomotor
void servo_init() {
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
    uint channel = pwm_gpio_to_channel(SERVO_PIN);
    set_config_servo(slice_num, channel);
    

    // Configura o divisor de clock para 125MHz para obter 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);
}

// Define o ângulo do servomotor
void servo_set_angle(uint slice_num, uint channel, float angle) {
    // Mapeia o ângulo (0-180) para a largura de pulso (500-2400µs)
    uint32_t pulse = MIN_PULSE + (uint32_t)((angle / 180.0f) * (MAX_PULSE - MIN_PULSE));
    // Converte a largura de pulso para o nível do PWM (25000 wrap = 20ms)
    uint32_t level = (pulse * 25000) / 20000;
    pwm_set_chan_level(slice_num, channel, level);
}

// Movimentação suave do ângulo inicial para o ângulo final
void servo_sweep(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(servo_config.slice_num, servo_config.channel, angle);
        sleep_ms(STEP_DELAY);
    }
}

void servo_move(uint slice_num, uint channel, float angle){

}

int main() {
    
    servo_init();

        servo_sweep(0, 90);
    while (true) {
        // Movimentação suave de 0° a 180°

        // Movimentação suave de 180° a 0°
        //servo_sweep(180, 0);
        //sleep_ms(PAUSE_MS); // Pausa breve em 0°
    }
}