#include <Servo.h>
#include <PID_v1.h>
#include <Wire.h> // Biblioteca para comunicação I2C
#include <LiquidCrystal_I2C.h> // Biblioteca para LCD I2C

// --- Definições de Pinos e Constantes ---
// Pinos dos Servos
const int PINO_SERVO_HORIZONTAL = 3;
const int PINO_SERVO_VERTICAL = 5;

// Pinos dos LDRs (Entradas Analógicas)
const int PINO_LDR_DIREITA_CIMA = A0;
const int PINO_LDR_DIREITA_BAIXO = A1;
const int PINO_LDR_ESQUERDA_CIMA = A2;
const int PINO_LDR_ESQUERDA_BAIXO = A3;

// Limites de Rotação dos Servos (Proteção Mecânica)
const int HORIZONTAL_MAX = 180;
const int HORIZONTAL_MIN = 65;
const int VERTICAL_MAX = 120;
const int VERTICAL_MIN = 15;

// Posições Iniciais dos Servos
const int POS_INICIAL_HORIZONTAL = 180;
const int POS_INICIAL_VERTICAL = 45;

// O PID elimina a necessidade de TOLERANCIA, mas mantemos o DELAY_MOVIMENTO
const int DELAY_MOVIMENTO_MS = 100;

// --- Objetos Servo ---
Servo servoHorizontal;
Servo servoVertical;

// --- Configuração do LCD I2C (Endereço 0x27, 16 colunas, 2 linhas) ---
// Se o seu display não funcionar, tente 0x3F ou outro endereço.
LiquidCrystal_I2C lcd(0x27, 16, 2); 

// --- Variáveis para o PID Horizontal ---
double InputH, OutputH, SetpointH = 0;
// *** CONSTANTES PID HORIZONTAL: REQUIERE SINTONIA (TUNING) ***
double Kp_H = 0.4; // Valor inicial
double Ki_H = 0.001; // Valor inicial
double Kd_H = 0.2; // Valor inicial
PID pidHorizontal(&InputH, &OutputH, &SetpointH, Kp_H, Ki_H, Kd_H, DIRECT);

// --- Variáveis para o PID Vertical ---
double InputV, OutputV, SetpointV = 0;
// *** CONSTANTES PID VERTICAL: REQUIERE SINTONIA (TUNING) ***
double Kp_V = 0.4; // Valor inicial
double Ki_V = 0.001; // Valor inicial
double Kd_V = 0.2; // Valor inicial
PID pidVertical(&InputV, &OutputV, &SetpointV, Kp_V, Ki_V, Kd_V, DIRECT);

// --- Variáveis de Posição Atual dos Servos ---
int posicaoServoHorizontal = POS_INICIAL_HORIZONTAL;
int posicaoServoVertical = POS_INICIAL_VERTICAL;

// --- Protótipo da Função Auxiliar ---
void updateLCD();

// --- Função setup() ---
void setup() {
    // Inicia a comunicação serial (mantida para debug, mas sem prints de status)
    Serial.begin(9600);

    // Inicialização do LCD
    lcd.init();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("Rastreador Solar");
    lcd.setCursor(0, 1);
    lcd.print("Inicializando...");
    
    servoHorizontal.attach(PINO_SERVO_HORIZONTAL);
    servoVertical.attach(PINO_SERVO_VERTICAL);

    // Define as posições iniciais dos servos
    servoHorizontal.write(posicaoServoHorizontal);
    servoVertical.write(posicaoServoVertical);

    // --- Configuração do PID Horizontal ---
    pidHorizontal.SetOutputLimits(-10, 10);
    pidHorizontal.SetMode(AUTOMATIC);

    // --- Configuração do PID Vertical ---
    pidVertical.SetOutputLimits(-10, 10);
    pidVertical.SetMode(AUTOMATIC);

    delay(3000); 
    lcd.clear(); // Limpa o LCD para iniciar o monitoramento PID
}

// --- Função loop() ---
void loop() {
    // 1. Leitura Analógica dos LDRs
    int leituraLDRDC = analogRead(PINO_LDR_DIREITA_CIMA);
    int leituraLDREC = analogRead(PINO_LDR_ESQUERDA_CIMA);
    int leituraLDRDB = analogRead(PINO_LDR_DIREITA_BAIXO);
    int leituraLEB = analogRead(PINO_LDR_ESQUERDA_BAIXO);

    // 2. Cálculo das Médias
    int valorSuperior = (leituraLDRDC + leituraLDREC) / 2;
    int valorInferior = (leituraLDRDB + leituraLEB) / 2;
    int valorDireita = (leituraLDRDC + leituraLDRDB) / 2;
    int valorEsquerda = (leituraLDREC + leituraLEB) / 2;

    // 3. Cálculo das Diferenças (ERRO)
    int diferencaDirEsq = valorDireita - valorEsquerda;
    int diferencaSupInf = valorSuperior - valorInferior;

    // --- 4. CÁLCULO E MOVIMENTO HORIZONTAL ---
    InputH = diferencaDirEsq;
    pidHorizontal.Compute();
    posicaoServoHorizontal += OutputH; 

    // Limites de proteção do servo Horizontal
    if (posicaoServoHorizontal > HORIZONTAL_MAX) {
        posicaoServoHorizontal = HORIZONTAL_MAX;
    } else if (posicaoServoHorizontal < HORIZONTAL_MIN) {
        posicaoServoHorizontal = HORIZONTAL_MIN;
    }
    servoHorizontal.write(posicaoServoHorizontal);

    // --- 5. CÁLCULO E MOVIMENTO VERTICAL ---
    InputV = diferencaSupInf;
    pidVertical.Compute();
    
    // Subtraímos pois um erro positivo (mais luz no topo) significa que o painel deve descer
    posicaoServoVertical -= OutputV; 

    // Limites de proteção do servo Vertical
    if (posicaoServoVertical > VERTICAL_MAX) {
        posicaoServoVertical = VERTICAL_MAX;
    } else if (posicaoServoVertical < VERTICAL_MIN) {
        posicaoServoVertical = VERTICAL_MIN;
    }
    servoVertical.write(posicaoServoVertical);
    
    // --- ATUALIZAÇÃO DO DISPLAY LCD (NOVO) ---
    updateLCD();
    
    // Pequena pausa para estabilização
    delay(DELAY_MOVIMENTO_MS);
}

// --- Funções Auxiliares ---
void updateLCD() {
    // Buffer para formatar os valores double em string (dtostrf)
    char bufferH[6]; // Suficiente para "XXX.X"
    char bufferV[6]; 

    // Formata o erro horizontal e a posição (Linha 1)
    // Ex: H E:XXX.X P:XXX
    dtostrf(InputH, 4, 1, bufferH); // Formata InputH com 1 casa decimal
    
    lcd.setCursor(0, 0);
    lcd.print("H E:");
    lcd.print(bufferH);
    lcd.print(" P:");
    lcd.print(posicaoServoHorizontal);
    lcd.print(" "); // Espaço para garantir a limpeza de dígitos anteriores

    // Formata o erro vertical e a posição (Linha 2)
    // Ex: V E:XXX.X P:XXX
    dtostrf(InputV, 4, 1, bufferV); // Formata InputV com 1 casa decimal
    
    lcd.setCursor(0, 1);
    lcd.print("V E:");
    lcd.print(bufferV);
    lcd.print(" P:");
    lcd.print(posicaoServoVertical);
    lcd.print(" "); // Espaço para garantir a limpeza de dígitos anteriores
}