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

// Mapeamento dos pinos no Raspberry Pi Pico
#define ENA 15  // PWM motor A
#define IN1 14
#define IN2 13

#define ENB 12  // PWM motor B
#define IN3 10
#define IN4 11

#define POT_X 26  // ADC0 (A0 no Arduino)
#define POT_Y 27  // ADC1 (A1 no Arduino)

// Função para inicializar um pino PWM e retornar o slice
uint pwm_init_pin(uint gpio) {
    gpio_set_function(gpio, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(gpio);
    pwm_set_wrap(slice, 255);  // Resolução de 8 bits (0-255)
    pwm_set_enabled(slice, true);
    return slice;
}

void writeA(bool a1, bool a2) {
    gpio_put(IN1, a1);
    gpio_put(IN2, a2);
}

void writeB(bool a3, bool a4) {
    gpio_put(IN3, a3);
    gpio_put(IN4, a4);
}

int main() {
    //stdio_init_all();

    // Configuração GPIO
    gpio_init(IN1); gpio_set_dir(IN1, GPIO_OUT);
    gpio_init(IN2); gpio_set_dir(IN2, GPIO_OUT);
    gpio_init(IN3); gpio_set_dir(IN3, GPIO_OUT);
    gpio_init(IN4); gpio_set_dir(IN4, GPIO_OUT);

    // Inicializa PWM
    uint sliceA = pwm_init_pin(ENA);
    uint sliceB = pwm_init_pin(ENB);

    // Inicializa ADC
    adc_init();
    adc_gpio_init(POT_X);
    adc_gpio_init(POT_Y);

    while (1) {
        // Leitura potenciômetro X
        adc_select_input(0); // POT_X
        int raw_x = adc_read(); // 0..4095
        // Leitura potenciômetro Y
        adc_select_input(1); // POT_Y
        int raw_y = adc_read();

        int xx = (raw_x - 2048) / 8;  // equivalente a (0-4095) centrado
        int yy = (raw_y - 2048) / 2;

        int speedA = yy - xx;
        int speedB = yy + xx;

        // Limita valores a -255..255
        if (speedA > 255) speedA = 255;
        if (speedA < -255) speedA = -255;
        if (speedB > 255) speedB = 255;
        if (speedB < -255) speedB = -255;

        // Motor A
        if (speedA >= 0) {
            writeA(1, 0);
            pwm_set_gpio_level(ENA, speedA);
        } else {
            writeA(0, 1);
            pwm_set_gpio_level(ENA, -speedA);
        }

        // Motor B
        if (speedB >= 0) {
            writeB(1, 0);
            pwm_set_gpio_level(ENB, speedB);
        } else {
            writeB(0, 1);
            pwm_set_gpio_level(ENB, -speedB);
        }

        sleep_ms(20); // delay de estabilidade
    }
}