#include <stdio.h> //Biblioteca padrão de C que permite entrada e saída de dados
#include "pico/stdlib.h" //Biblioteca fundamental do SDK
#include "hardware/pwm.h" //Biblioteca para manipular o pwm
#include "hardware/adc.h" 

#define ledG 11
#define ledB 12
#define ledR 13

#define Button_Joy 22
#define ButtonA 5

#define VX_JOY 26
#define VY_JOY 27

// Conversão de microssegundos para nível de PWM

//Variáveis para configurar o pwm
#define DIVIDER_PWM 100 //Estabelecendo divisor de clock
#define WRAP_PERIOD 24999 //Com o divisor do clock em 100, o wrap tem valor 24999 
uint16_t levelX = 0; //Nível inicial baixo
uint16_t levelY=0;

static volatile bool estado_leds = true;;

void led_config(int led_pin){
  gpio_init(led_pin);
  gpio_set_dir(led_pin, GPIO_OUT);
  gpio_put(led_pin, 0);
};

//Função para inicializar configurações do pwm
void config_pwm(int pino){  //Função pode ser usada para o led e o servo 
    uint slice;
    gpio_set_function(pino, GPIO_FUNC_PWM); //Configura pino do led como pwm
    slice = pwm_gpio_to_slice_num(pino); //Adiquire o slice do pino
    pwm_set_clkdiv(slice, DIVIDER_PWM); 
    pwm_set_wrap(slice, WRAP_PERIOD);
    pwm_set_gpio_level(pino, 0); //Determina com o level desejado
    pwm_set_enabled(slice, true);
};


int main()
{
    
    uint16_t valor_anteriorX=2048;
    uint16_t valor_anteriorY=2048;
    uint16_t valor_atualX;
    uint16_t valor_atualY;

    stdio_init_all(); //Inicia comunicação padrão
    adc_init();

    

    //Configurando leds como saída PWM
    led_config(ledB);
    led_config(ledR);
    led_config(ledG);
    config_pwm(ledB);
    config_pwm(ledR);
    config_pwm(ledG);

    //Configurando botão do joystick
    gpio_init(Button_Joy);
    gpio_set_dir(Button_Joy, GPIO_IN);
    gpio_pull_up(Button_Joy);

    //Configurando entradas analógicas
    adc_gpio_init(VX_JOY); 
    adc_gpio_init(VY_JOY);

    //Configurando interrupções para os botões

    while (true) {
        adc_select_input(0);
        valor_atualX = adc_read();
        adc_select_input(1);
        valor_atualY = adc_read();

        if (valor_atualX ==0 || valor_atualX == 2048){
            levelX = 24999;
        }else if(valor_atualX < 2048) {
          levelX = ((2048 - valor_atualX) * WRAP_PERIOD) / 2048; 
        } else if (valor_atualX > 2048) {
            levelX = ((valor_atualX - 2048) * WRAP_PERIOD) / 2048; 
        }else {
            levelX = 0; // Apaga quando estiver em 2048
        };

        if (valor_atualY ==0 || valor_atualY == 2048){
            levelY = 24999;
        }else if(valor_atualY < 2048) {
          levelY = ((2048 - valor_atualY) * WRAP_PERIOD) / 2048; 
        } else if (valor_atualY > 2048) {
            levelY = ((valor_atualY - 2048) * WRAP_PERIOD) / 2048; 
        }else {
            levelY = 0; // Apaga quando estiver em 2048
        };

        printf("Level: %d; VX: %d\n", levelX, valor_atualX);
        pwm_set_gpio_level(ledB, levelX); //Atualiza o level
        printf("Level: %d; VY: %d\n", levelY, valor_atualY);
        pwm_set_gpio_level(ledR, levelY); //Atualiza o level

        sleep_ms(10); //Pausa para atualizar e suavizar o movimento
    };
};