#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/bootrom.h"
#include "ssd1306.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"

#define ADC0_NUM 0
#define ADC0_PIN (26 + ADC0_NUM)
#define PWM_PIN 14

#define SW1 20
#define SW2 21

// Buffer do display
uint8_t display_buffer[128 * SSD1306_HEIGHT / 8] = {0};

// Variáveis globais
uint16_t adc0_valor;
uint slice_num;
char texto[50];

int main() {

  stdio_init_all();
  SSD1306_init();

  // ===== ADC =====
  adc_init();
  adc_gpio_init(ADC0_PIN);

  // ===== PWM =====
  gpio_set_function(PWM_PIN, GPIO_FUNC_PWM);
  slice_num = pwm_gpio_to_slice_num(PWM_PIN);

  pwm_set_clkdiv(slice_num, 125.0f);
  pwm_set_wrap(slice_num, 1000);
  pwm_set_enabled(slice_num, true);

  // ===== SWITCHES =====
  gpio_init(SW1);
  gpio_set_dir(SW1, GPIO_IN);
  gpio_pull_up(SW1);

  gpio_init(SW2);
  gpio_set_dir(SW2, GPIO_IN);
  gpio_pull_up(SW2);

  float tensao;
  float percentual;
  float ganho;
  uint16_t duty;

  while (true)
  {
    // ===== Leitura dos switches =====
    bool s1 = gpio_get(SW1);
    bool s2 = gpio_get(SW2);

    int modo;

    // NORMAL = 00
    if (s1 == 0 && s2 == 0) modo = 1;       // NORMAL
    else if (s1 == 0 && s2 == 1) modo = 0;  // ECO
    else if (s1 == 1 && s2 == 0) modo = 2;  // SPORT
    else modo = 1; // fallback seguro

    // ===== Ganhos =====
    if (modo == 0) ganho = 0.7;   // ECO
    if (modo == 1) ganho = 1.0;   // NORMAL
    if (modo == 2) ganho = 1.3;   // SPORT

    // ===== Leitura ADC =====
    adc_select_input(ADC0_NUM);
    adc0_valor = adc_read();

    tensao = (adc0_valor * 3.3f) / 4095.0f;
    percentual = (tensao / 3.3f) * 100.0f;

    // ===== Aplica ganho =====
    float valor_mod = adc0_valor * ganho;

    // Limites de segurança
    if (valor_mod > 4095) valor_mod = 4095;
    if (valor_mod < 0) valor_mod = 0;

    // ===== PWM =====
    duty = (valor_mod * 1000) / 4095;
    pwm_set_chan_level(slice_num, PWM_CHAN_A, duty);

    // ===== Display =====
    ssd1306_clear_buffer(display_buffer);

    sprintf(texto, "ADC: %d", adc0_valor);
    ssd1306_write_text(display_buffer, 0, 0, texto);

    sprintf(texto, "Tensao: %.2fV", tensao);
    ssd1306_write_text(display_buffer, 0, 12, texto);

    sprintf(texto, "Pedal: %.1f%%", percentual);
    ssd1306_write_text(display_buffer, 0, 24, texto);

    if (modo == 0) sprintf(texto, "Modo: ECO");
    if (modo == 1) sprintf(texto, "Modo: NORMAL");
    if (modo == 2) sprintf(texto, "Modo: SPORT");

    ssd1306_write_text(display_buffer, 0, 36, texto);

    sprintf(texto, "PWM: %d", duty);
    ssd1306_write_text(display_buffer, 0, 48, texto);

    ssd1306_send_data(display_buffer, sizeof(display_buffer));

    sleep_ms(50);
  }
}