#include <stdio.h>          // Funções de entrada/saída (printf).
#include <math.h>           // Funções matemáticas (sqrt, atan2, M_PI).
#include <string.h>         // Funções para manipulação de strings.
#include "pico/stdlib.h"    // Funções padrão do Raspberry Pi Pico.
#include "hardware/i2c.h"   // Biblioteca para comunicação I2C.
#include "ili9341.h"        // Biblioteca para controle do display ILI9341.

// Configurações do MPU6050
#define MPU6050_ADDR 0x68   // Endereço I2C do MPU6050.
#define I2C_PORT i2c0       // Define a porta I2C (i2c0).
#define SDA_MPU6050 4       // Pino GPIO para SDA do MPU6050.
#define SCL_MPU6050 5       // Pino GPIO para SCL do MPU6050.

// Configurações do Servo Motor
#define PIN_SERVO 14        // Pino GPIO para o servo motor.
#define PERIODO_SERVO 20    // Período de um ciclo do servo (ms).
#define DUTY_CONSTANT 10    // Constante para cálculo do duty cycle.

// Configurações do Display
ILI9341_t display = { // Estrutura de configuração do display.
    .sck = 18,        // Pino SCK (clock serial).
    .mosi = 19,       // Pino MOSI (Master Out Slave In).
    .cs = 17,         // Pino CS (Chip Select).
    .rst = 20,        // Pino RST (Reset).
    .dc = 21,         // Pino DC (Data/Command).
    .bl = 22,         // Pino BL (Backlight).
    .orientation = 0x60 // Orientação do display.
};

// Constante de conversão para 'g'
#define conv_g 16384.0      // Conversão de valor bruto para 'g' (para ±2g).

void send_pulse(uint duty) // Envia um pulso para o servo motor.
{
    gpio_put(PIN_SERVO, 1); // Coloca pino do servo em nível ALTO.
    sleep_us(duty);         // Mantém pino ALTO por 'duty' microssegundos.
    gpio_put(PIN_SERVO, 0); // Coloca pino do servo em nível BAIXO.
    sleep_ms(PERIODO_SERVO - (duty / 1000)); // Aguarda para completar o período.
}

void adjust_servo_motor(float incline, uint16_t color) // Ajusta servo e exibe mensagem.
{
    uint duty_us; // Variável para o duty cycle em microssegundos.
    if(incline > 30.0) // Se inclinação maior que 30 graus.
    {
        duty_us = incline * DUTY_CONSTANT * 2; // Calcula duty cycle.
        ILI9341_drawText(&display, 18, 56, "Inclinação maior que 30 graus", color, 1, 1); // Exibe mensagem.
    } else if(incline < -30.0){ // Se inclinação menor que -30 graus.
        duty_us = (incline * (-1)) * DUTY_CONSTANT * 2; // Calcula duty cycle (valor absoluto).
        ILI9341_drawText(&display, 18, 56, "Inclinação menor que -30 graus", color, 1, 1); // Exibe mensagem.
    } else { // Inclinação entre -30 e 30 graus.
        duty_us = incline * DUTY_CONSTANT * 2; // Calcula duty cycle.
        ILI9341_drawText(&display, 18, 56, "Inclinação neutra", color, 1, 1); // Exibe mensagem.
    }

    for(int i = 0; i < 50; i++) // Envia pulso várias vezes para estabilizar.
    {
        send_pulse(duty_us); // Chama função de envio de pulso.
    }
}

void read_mpu6050(int16_t *x, int16_t *y, int16_t *z) // Lê dados do acelerômetro MPU6050.
{
    uint8_t buffer[6]; // Buffer para armazenar os 6 bytes de dados.
    // Lê registrador do acelerômetro (0x3B).
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, (uint8_t[]){0x3B}, 1, true);
    i2c_read_blocking(I2C_PORT, MPU6050_ADDR, buffer, 6, false); // Lê 6 bytes.

    *x = (buffer[0] << 8) | buffer[1]; // Combina bytes para X.
    *y = (buffer[2] << 8) | buffer[3]; // Combina bytes para Y.
    *z = (buffer[4] << 8) | buffer[5]; // Combina bytes para Z.
}

void init_pins() // Inicializa pinos I2C, MPU6050, display e servo.
{
    i2c_init(I2C_PORT, 100000); // Inicializa I2C com 100 kHz.
    gpio_set_function(SDA_MPU6050, GPIO_FUNC_I2C); // Configura SDA para I2C.
    gpio_set_function(SCL_MPU6050, GPIO_FUNC_I2C); // Configura SCL para I2C.
    // Inicialização do MPU6050 (sair do modo sleep).
    uint8_t reset[2] = {0x6B, 0x00}; // Registrador Power Management 1.
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, reset, 2, false); // Envia comando de reset.
    ILI9341_init(&display); // Inicializa o display ILI9341.
    gpio_init(PIN_SERVO); // Inicializa pino do servo motor.
    gpio_set_dir(PIN_SERVO, GPIO_OUT); // Define pino do servo como SAÍDA.
}

int main() { // Função principal do programa.
    stdio_init_all(); // Inicializa console serial.
    init_pins(); // Inicializa pinos e periféricos.

    int16_t x, y, z; // Variáveis para valores brutos do acelerômetro.
    float xg, yg, zg, incline; // Variáveis para valores em 'g' e inclinação.

    uint16_t color = RGB_to_RGB565(255, 255, 255); // Define cor branca para texto.
    char texto_xyz[31]; // Buffer para string de coordenadas XYZ.
    char texto_incline[19]; // Buffer para string da inclinação.

    while (true) { // Loop infinito de operação.
        read_mpu6050(&x, &y, &z); // Lê dados do MPU6050.

        xg = x / conv_g; // Converte X para 'g'.
        yg = y / conv_g; // Converte Y para 'g'.
        zg = z / conv_g; // Converte Z para 'g'.

        // Calcula inclinação usando atan2, eixo X versus plano YZ.
        incline = atan2(xg, sqrt(yg * yg + zg * zg)) * (180.0 / M_PI);

        // Formata strings para exibição.
        sprintf(texto_xyz, "X: %d  Y: %d  Z: %d", x, y, z);
        sprintf(texto_incline, "Incline: %.2f", incline);

        // Desenha textos no display.
        ILI9341_drawText(&display, 18, 80, texto_xyz, color, 1, 1);
        ILI9341_drawText(&display, 18, 68, texto_incline, color, 1, 1);
        
        adjust_servo_motor(incline, color); // Ajusta servo e atualiza display.
        sleep_ms(1000); // Pausa por 1 segundo.
        ILI9341_clear(&display); // ATENÇÃO: Limpa o display a cada loop.
        // Isso faz o display piscar e o texto sumir.
        sleep_ms(2000); // Pausa por 2 segundos.
    }
}