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

#define led_esq 0
#define led_dir 1

#define key_lant 2
#define key_break 3

#define GPIO_HIGH 1
#define GPIO_LOW 0

#define FREQ 1000.0f
#define WRAP 4095

void system_init(void) {
    gpio_init(key_lant);
    gpio_set_dir(key_lant, GPIO_IN);
    gpio_pull_up(key_lant);

    gpio_init(key_break);
    gpio_set_dir(key_break, GPIO_IN);
    gpio_pull_up(key_break);
}

int main() {
    stdio_init_all();
    system_init();

    uint16_t val = 2048;
    uint16_t val100 = 4095;
    uint16_t val0 = 0;
    uint sts_key_lant = 0;
    uint sts_key_brake = 0;
    uint slice_esq = pwm_gpio_to_slice_num(led_esq);
    uint chan_esq = pwm_gpio_to_channel(led_esq);

    uint slice_dir = pwm_gpio_to_slice_num(led_dir);
    uint chan_dir = pwm_gpio_to_channel(led_dir);

    float fsys = frequency_count_khz(CLOCKS_FC0_SRC_VALUE_CLK_SYS) * 1000.0f;
    float div = fsys / (FREQ * (WRAP + 1));
    printf("Teste\n");
    sleep_ms(50);
    pwm_config config = pwm_get_default_config();
    pwm_config_set_wrap(&config, WRAP);
    pwm_config_set_clkdiv(&config, div);
    pwm_config_set_phase_correct(&config, false);
    pwm_config_set_clkdiv_mode(&config, PWM_DIV_FREE_RUNNING);

    // Inicializa e configura PWM para os LEDs
    pwm_init(slice_esq, &config, false);
    gpio_set_function(led_esq, GPIO_FUNC_PWM);
    pwm_set_chan_level(slice_esq, chan_esq, val0);
    pwm_set_enabled(slice_esq, true);

    pwm_init(slice_dir, &config, false);
    gpio_set_function(led_dir, GPIO_FUNC_PWM);
    pwm_set_chan_level(slice_dir, chan_dir, val0);
    pwm_set_enabled(slice_dir, true);

    while (true) {
        sts_key_lant = gpio_get(key_lant);
        sts_key_brake = gpio_get(key_break);
        if (sts_key_lant == 0 && sts_key_brake == 1) {
            pwm_set_chan_level(slice_esq, chan_esq, val);
            pwm_set_chan_level(slice_dir, chan_dir, val);
        } else if ((sts_key_brake == 0 && sts_key_lant == 1) || (sts_key_lant == 1 && sts_key_brake == 1)) {
            pwm_set_chan_level(slice_esq, chan_esq, val100);
            pwm_set_chan_level(slice_dir, chan_dir, val100);
        } else {
            pwm_set_chan_level(slice_esq, chan_esq, val0);
            pwm_set_chan_level(slice_dir, chan_dir, val0);
        }
        sleep_ms(10);
    }
}