#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"
#include "font.h"
#include "hardware/pio.h"
#include "hardware/i2c.h"
#include "ssd1306.h"

#define POTENTIOMETER_PIN 27  // Alterado para outro pino GPIO27 (ADC1)
#define SERVO_PIN 28          // GPIO15 (PWM)
#define PWM_FREQUENCY 50      // 50Hz para controle do servo
#define PWM_WRAP 20000        // Período de 20ms (50Hz) com clock de 1MHz
#define MIN_DUTY 1000         // 1ms de largura de pulso (mínimo)
#define MAX_DUTY 4800         // Aprox. 2ms de largura de pulso (máximo)
// Definições dos pinos para comunicação I2C
#define I2C_PORT i2c1         // Porta I2C utilizada
#define I2C_SDA 14            // Pino para SDA (Serial Data Line)
#define I2C_SCL 15            // Pino para SCL (Serial Clock Line)
#define ENDERECO 0x3C         // Endereço I2C do display SSD1306

ssd1306_t ssd;  // Estrutura global para controle do display
bool cor = true; // Declarado corretamente antes de seu uso
// Inicializa o I2C, o display OLED SSD1306 e o ADC do joystick
void display() {
    i2c_init(I2C_PORT, 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);

    ssd1306_init(&ssd, WIDTH, HEIGHT, true, ENDERECO, I2C_PORT);
    ssd1306_config(&ssd);
    ssd1306_fill(&ssd, false); // Limpa o display (fundo preto)
    ssd1306_send_data(&ssd);
}

void escrever(ssd1306_t *display, const char *texto, uint8_t x, uint8_t y, bool cor) {
    ssd1306_draw_string(display, texto, x, y);
    ssd1306_send_data(display);
}

void init_pwm(uint slice_num, uint channel) {
    pwm_config config = pwm_get_default_config();
    pwm_config_set_clkdiv(&config, 64.0f);  // Reduz a frequência do clock
    pwm_config_set_wrap(&config, PWM_WRAP); // Define período do PWM (20ms)
    pwm_init(slice_num, &config, true);
    gpio_set_function(SERVO_PIN, GPIO_FUNC_PWM);
}

int main() {
    stdio_init_all();
    adc_init();
    adc_gpio_init(POTENTIOMETER_PIN);
    adc_select_input(1);  // Atualizado para ADC1
    display();

    uint slice_num = pwm_gpio_to_slice_num(SERVO_PIN);
    uint channel = pwm_gpio_to_channel(SERVO_PIN);
    init_pwm(slice_num, channel);

    while (1) {
        uint16_t raw_value = adc_read();
        float angle = (raw_value / 4095.0f) * 180.0f; // Converte ADC (0-4095) para ângulo (0-180°)
        uint16_t duty = MIN_DUTY + (angle / 180.0f) * (MAX_DUTY - MIN_DUTY); // Mapeia PWM de 1ms a 2ms

        pwm_set_gpio_level(SERVO_PIN, duty);
        printf("ADC: %u, Angle: %.2f, Duty: %u\n", raw_value, angle, duty);
        
        ssd1306_fill(&ssd, !cor);
        
        // Converte valores para strings antes de exibir no display
        char raw_value_str[20];
        char angle_str[20];
        char duty_str[20];
        
        snprintf(raw_value_str, sizeof(raw_value_str), "ADC: %u", raw_value);
        snprintf(angle_str, sizeof(angle_str), "Ang: %.2f", angle);
        snprintf(duty_str, sizeof(duty_str), "DC:  %u", duty);

        // Exibe cada valor em uma linha separada no display
        escrever(&ssd, raw_value_str, 2, 10, cor);
        escrever(&ssd, angle_str, 2, 25, cor);
        escrever(&ssd, duty_str, 2, 40, cor);

        sleep_ms(50);
    }
}