#include <Servo.h>
#include <PID_v1.h>

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

// --- Variáveis para o PID Horizontal ---
double InputH, OutputH, SetpointH = 0;
// *** CONSTANTES PID HORIZONTAL: REQUIERE SINTONIA (TUNING) ***
// Kp: Proporcional (Determina a força da correção)
// Ki: Integral (Elimina erro de longo prazo)
// Kd: Derivativo (Amortece o movimento)
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;

// --- Função setup() ---
void setup() {
    // Inicia a comunicação serial para monitoramento (tuning)
    Serial.begin(9600);

    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 ---
    // Define os limites de saída (o movimento máximo que o servo pode fazer em um ciclo)
    pidHorizontal.SetOutputLimits(-10, 10); // Permite um ajuste máximo de +/- 10 graus por ciclo
    pidHorizontal.SetMode(AUTOMATIC);

    // --- Configuração do PID Vertical ---
    pidVertical.SetOutputLimits(-10, 10); // Permite um ajuste máximo de +/- 10 graus por ciclo
    pidVertical.SetMode(AUTOMATIC);

    Serial.println("Rastreador Solar PID Inicializado.");
    delay(3000); 
}

// --- 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)
    // O erro é a diferença de luminosidade entre os lados opostos
    // O Setpoint é 0, então o Input do PID é o próprio erro.
    int diferencaDirEsq = valorDireita - valorEsquerda;
    int diferencaSupInf = valorSuperior - valorInferior;

    // --- 4. CÁLCULO E MOVIMENTO HORIZONTAL ---
    InputH = diferencaDirEsq;
    pidHorizontal.Compute();
    
    // O OutputH é o ajuste de ângulo recomendado pelo PID
    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();
    
    // O OutputV é o ajuste de ângulo recomendado pelo PID
    // Subtraímos pois um erro positivo (mais luz no topo) significa que o painel deve descer (diminuir o ângulo do servo)
    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);
    
    // --- MONITORAMENTO SERIAL (NOVO) ---
    // Imprime os valores para sintonização do PID
    Serial.print("H_Input(Erro): "); Serial.print(InputH);
    Serial.print(" | H_Output(Ajuste): "); Serial.print(OutputH);
    Serial.print(" | H_Pos: "); Serial.print(posicaoServoHorizontal);
    Serial.print(" || V_Input(Erro): "); Serial.print(InputV);
    Serial.print(" | V_Output(Ajuste): "); Serial.print(OutputV);
    Serial.print(" | V_Pos: "); Serial.println(posicaoServoVertical);
    // ------------------------------------
    
    // Pequena pausa para estabilização
    delay(DELAY_MOVIMENTO_MS);
}