#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include "hardware/i2c.h"
#include "ss_oled.h" // Biblioteca do OLED ss_oled

// Definições dos pinos e constantes
#define LDR_ESQUERDO 27   // ADC0
#define LDR_DIREITO 26    // ADC1
#define PINO_SERVO   9    // Pino de controle do servo
#define I2C_SDA      4    // Pino SDA do OLED
#define I2C_SCL      5    // Pino SCL do OLED

int posicaoAtual = 90; // Posição inicial do servo (em graus)
SSOLED oled;         // Instância do display OLED

// Aloca o backbuffer para um display 128x64 (128 x 64 / 8 = 1024 bytes)
static uint8_t backBuffer[1024];

// Configuração do PWM para o servo motor
void setup_servo(uint gpio) {
    gpio_set_function(gpio, GPIO_FUNC_PWM);
    uint slice = pwm_gpio_to_slice_num(gpio);
    pwm_config config = pwm_get_default_config();
    pwm_config_set_clkdiv(&config, 128.0f);
    pwm_config_set_wrap(&config, 3096); // Configurado para ~50Hz (período de 20ms)
    pwm_init(slice, &config, true);
}

// Função para mapear um valor de um intervalo para outro
int mapear(int valor, int in_min, int in_max, int out_min, int out_max) {
    return (valor - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// Função para mover o servo para um ângulo desejado (entre 0 e 180°)
void moverServo(int angulo) {
    if (angulo < 0) angulo = 0;
    if (angulo > 180) angulo = 180;
    
    // Calcula o valor de pulso (duty cycle) correspondente.
    // Aqui o cálculo foi ajustado para um pulso entre aproximadamente 500us e 2500us.
    uint slice = pwm_gpio_to_slice_num(PINO_SERVO);
    uint duty = (uint)(11.0167 * angulo + 446);  
    pwm_set_gpio_level(PINO_SERVO, duty);
}

// Calcula a posição desejada do servo com base nas leituras dos LDRs
int calcularPosicao(int leituraEsquerda, int leituraDireita) {
    int diferenca = leituraDireita - leituraEsquerda;
    return mapear(diferenca, -4095, 4095, 0, 180);
}

// Atualiza o conteúdo do OLED: desenha um "sol", uma linha representando o servo e exibe o valor
void atualizarTela(int posicao) {
    // Limpa a tela preenchendo com 0 (tudo apagado)
    oledFill(&oled, 0x00, 1);
    
    // Calcula a posição horizontal do servo a partir do ângulo
    int x_servo = mapear(posicao, 0, 180, 20, 108);
    
    // Exibe o valor do servo (texto)
    char buffer[20];
    snprintf(buffer, sizeof(buffer), "Placas ajustadas ", posicao);
    oledSetCursor(&oled, 5, 5);
    oledWriteString(&oled, 0, 0, 2, buffer, FONT_8x8, 0, 1);
    snprintf(buffer, sizeof(buffer), "a %d graus", posicao);
    oledSetCursor(&oled, 5, 5);
    oledWriteString(&oled, 0, 30, 4, buffer, FONT_8x8, 0, 1);
}

int main() {
    stdio_init_all();
    
    // Inicializa o ADC e configura os pinos dos LDRs
    adc_init();
    adc_gpio_init(LDR_ESQUERDO);
    adc_gpio_init(LDR_DIREITO);
    
    // Inicializa o pino do servo
    gpio_init(PINO_SERVO);
    
    // Inicializa o I2C (usando o i2c_default, normalmente i2c0) para o OLED
    i2c_init(i2c_default, 400 * 1000);
    gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SDA);
    gpio_pull_up(I2C_SCL);
    
    // Inicializa o display OLED:
    // Parâmetros: 
    // - Tipo do display: OLED_128x64
    // - Endereço I2C: 0x3C
    // - bFlip = 0, bInvert = 0, usa Wire (bWire = 1)
    // - Pinos SDA e SCL conforme definido
    // - Pino de reset = -1 (não usado) e velocidade de 400 kHz
    oledInit(&oled, OLED_128x64, 0x3C, 0, 0, 1, I2C_SDA, I2C_SCL, -1, 400000);
    
    // Define o backbuffer para o OLED
    oledSetBackBuffer(&oled, backBuffer);
    
    printf("Inicializando setup\n");
    setup_servo(PINO_SERVO);
    
    while (1) {
        // Leitura dos LDRs
        adc_select_input(0);
        int leituraDireita = adc_read();
        adc_select_input(1);
        int leituraEsquerda = adc_read();
        printf("Leitura LDR Esquerdo: %d, LDR Direito: %d\n", leituraEsquerda, leituraDireita);
        
        // Calcula a posição desejada do servo com base na diferença entre as leituras
        int posicaoDesejada = calcularPosicao(leituraEsquerda, leituraDireita);
        
        // Se a posição atual for diferente da desejada, movimente o servo gradualmente
        if (posicaoAtual != posicaoDesejada) {
            if (posicaoAtual < posicaoDesejada) {
                for (int pos = posicaoAtual; pos <= posicaoDesejada; pos++) {
                    moverServo(pos);
                    //atualizarTela(pos);
                    sleep_ms(10);
                }
            } else {
                for (int pos = posicaoAtual; pos >= posicaoDesejada; pos--) {
                    moverServo(pos);
                    //atualizarTela(pos);
                    sleep_ms(10);
                }
            }
            posicaoAtual = posicaoDesejada;
            printf("\n---------------------\nNova posição do servo: %d\n---------------------\n\n", posicaoAtual);
        }
        atualizarTela(posicaoAtual);
        
        sleep_ms(50);  // Tempo de espera para maior responsividade
    }
    
    return 0;
}