#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;

// --- Constantes para Cálculo de Lux (Luminosidade) ---
const float GAMMA = 0.7;
// RL10 é a resistência do LDR a 10 lux (em kOhms)
const float RL10 = 50.0; 
// Resistência fixa (em Ohms) usada no divisor de tensão. O valor 2000.0 é baseado na sua fórmula.
const float R_FIXED = 2000.0;

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

// --- Configuração do LCD I2C (Endereço 0x27, 16 colunas, 2 linhas) ---
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;  
double Ki_H = 0.001; 
double Kd_H = 0.2;  
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;  
double Ki_V = 0.001; 
double Kd_V = 0.2;  
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;

// --- Variável Global para o Lux Médio ---
float averageLux = 0.0;

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

// --- Função setup() ---
void setup() {
    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 ---
    pidHorizontal.SetOutputLimits(-10, 10);
    pidHorizontal.SetMode(AUTOMATIC);
    pidVertical.SetOutputLimits(-10, 10);
    pidVertical.SetMode(AUTOMATIC);

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

// --- 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 (em valor Analógico, usado para o PID)
    int valorSuperior = (leituraLDRDC + leituraLDREC) / 2;
    int valorInferior = (leituraLDRDB + leituraLEB) / 2;
    int valorDireita = (leituraLDRDC + leituraLDRDB) / 2;
    int valorEsquerda = (leituraLDREC + leituraLEB) / 2;

    // 3. Conversão para Lux e Média
    float luxDC = calculateLux(leituraLDRDC);
    float luxEC = calculateLux(leituraLDREC);
    float luxDB = calculateLux(leituraLDRDB);
    float luxEB = calculateLux(leituraLEB);
    
    // Armazena o Lux médio para exibição no LCD
    averageLux = (luxDC + luxEC + luxDB + luxEB) / 4.0;
    
    // 4. Cálculo das Diferenças (ERRO) em valor analógico
    int diferencaDirEsq = valorDireita - valorEsquerda;
    int diferencaSupInf = valorSuperior - valorInferior;

    // --- 5. 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);

    // --- 6. 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 ---
    updateLCD();
    
    // Pequena pausa para estabilização
    delay(DELAY_MOVIMENTO_MS);
}

// --- Funções Auxiliares ---

// Converte o valor analógico em valor lux usando as constantes fornecidas
float calculateLux(int analogValue) {
    // Conversão do valor analógico (0-1023) para voltagem (0-5V)
    float voltage = analogValue / 1024.0 * 5.0;
    
    // Cálculo da resistência do LDR (em Ohms)
    // Fórmula original do usuário: resistance = 2000 * voltage / (1 - voltage / 5)
    float resistance = R_FIXED * voltage / (5.0 - voltage); 

    // Cálculo do Lux
    float lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1.0 / GAMMA));
    
    return lux;
}

void updateLCD() {
    // Buffer para formatar o valor double do Lux
    char bufferLux[8]; // Suficiente para "XXXX.X"

    // --- Linha 1: Posições dos Servos ---
    // Ex: H Pos: 180 V Pos: 45
    lcd.setCursor(0, 0);
    lcd.print("Pos: ");
    lcd.print(posicaoServoHorizontal);
    lcd.print(char(223));
    lcd.print("H ");
    lcd.print(posicaoServoVertical);
    lcd.print(char(223));
    lcd.print("V   ");

    // --- Linha 2: Lux Médio ---
    // Ex: Lux: 1234.5 lux
    dtostrf(averageLux, 5, 1, bufferLux); // Formata Lux com 1 casa decimal
    
    lcd.setCursor(0, 1);
    lcd.print("Lux: ");
    lcd.print(bufferLux);
    lcd.print(" lux "); // Espaços para limpar a linha
}