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

// Define os pinos de conexão
#define POT_PIN 26  // Pino de entrada do potenciômetro (ADC)
#define SERVO_PIN 15  // Pino de controle do servo (PWM)

// Função para configurar o PWM para o servo motor
void servo_init() {
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);

    // Configuração inicial do PWM
    pwm_set_wrap(slice_num, 1000);  // Tamanho do ciclo PWM (valor máximo)
    pwm_set_clkdiv(slice_num, 64);  // Divisor de clock (ajusta a frequência do PWM)
    pwm_set_enabled(slice_num, true);
}

// Função para mover o servo para um valor de 0 a 180 graus
// A velocidade de rotação será ajustada pela frequência do PWM
void move_servo(uint angle, uint speed) {
    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);

    // Converte o ângulo para um valor proporcional ao ciclo de PWM (largura de pulso)
    uint pulse_width = 1000 + (angle * 1000 / 180);  // Mapeia o ângulo de 0-180 para 1000-2000 microssegundos

    // Ajusta a frequência do PWM para controlar a velocidade de rotação
    uint pwm_freq = 100 + speed;  // Aumenta a frequência com o valor de "speed" (quanto maior, mais rápido)
    pwm_set_clkdiv(slice_num, 64.0 / pwm_freq);  // Ajusta a frequência (quanto maior a frequência, mais rápido o servo se move)

    // Define o valor do ciclo de trabalho (duty cycle) no pino de PWM
    pwm_set_gpio_level(SERVO_PIN, pulse_width);
}

int main() {
    // Inicializa a biblioteca de I/O e o ADC
    stdio_init_all();
    adc_init();
    adc_gpio_init(POT_PIN);
    adc_select_input(0);  // Seleciona o canal ADC correspondente ao pino do potenciômetro

    // Inicializa o servo motor
    servo_init();

    while (true) {
        // Lê o valor do potenciômetro (0-4095)
        uint16_t pot_value = adc_read();

        printf("pot_value: %d\n", pot_value);

        // Mapeia o valor lido (0-4095) para um ângulo de 0 a 180 graus
        uint angle = (pot_value * 180) / 4095;

        // Mapeia o valor lido (0-4095) para uma velocidade de 1 a 50
        uint speed = (pot_value * 50) / 4095;

        // print("speed: %d\n", speed);

        // Move o servo para a posição correspondente ao ângulo e ajusta a velocidade
        move_servo(angle, speed);

        // Espera um pouco antes de ler novamente
        sleep_ms(20);
    }

    return 0;
}