#include <Servo.h>              // Biblioteca para controle do servomotor
#include <PID_v1.h>             // Biblioteca oficial para o algoritmo PID
#include <Wire.h>               // Biblioteca para comunicação I2C
#include <LiquidCrystal_I2C.h>  // Biblioteca para controle do display LCD

// =====================================================
// PINOS (AGORA APENAS 2 LDRs)
// =====================================================
#define SERVO_VERTICAL_PIN   5  // Pino PWM que controla o servo vertical

#define LDR_TOP              A0     // Sensor Superior (Cima)
#define LDR_BOTTOM           A1     // Sensor Inferior (Baixo)

// =====================================================
// LCD
// =====================================================
LiquidCrystal_I2C lcd(0x27, 16, 2);

// =====================================================
// LIMITES MECÂNICOS
// =====================================================
const int V_MAX = 180;          // Limite máximo de 180°
const int V_MIN = 0;            // Limite mínimo de 0°

// =====================================================
// POSIÇÃO INICIAL
// =====================================================
int posV = 90;                  // Inicia o servo no meio (90°) mirando o zênite

// =====================================================
// CONFIGURAÇÕES
// =====================================================
const int DEADBAND = 15;        // Zona morta para evitar trepidação do motor
const int NIGHT_THRESHOLD = 15; // Limite corrigido: abaixo de 15 é escuro (noite)

const unsigned long LOOP_INTERVAL = 250; // Intervalo do PID (250ms)
const unsigned long LCD_INTERVAL  = 500; // Intervalo do LCD (500ms)

// =====================================================
// SERVO & PID
// =====================================================
Servo servoV;                   

double inputV;                  // Diferença de luz (Erro Cima/Baixo)
double outputV;                 // Ajuste angular calculado pelo PID
double setpointV = 0;           // Objetivo: zerar a diferença de luz

// Constantes de Sintonia do PID
double KpV = 0.40;
double KiV = 0.000;             // Ki zerado para evitar travamentos no simulador
double KdV = 0.20;

PID pidV(&inputV, &outputV, &setpointV, KpV, KiV, KdV, DIRECT);

// =====================================================
// CONTROLE DE TEMPO & FILTROS
// =====================================================
unsigned long lastLoop = 0;
unsigned long lastLCD  = 0;

float filteredTop = 0;
float filteredBottom = 0;

// =====================================================
// FUNÇÕES
// =====================================================

// Filtro passa-baixas com correção de inversão para o simulador
int readLDRFiltered(int pin, float &filtered)
{
    // CORREÇÃO: "1023 -" inverte o sinal elétrico do simulador.
    // Agora: Luz alta = Valor alto (~1023), Escuro = Valor baixo (~0)
    int raw = 1023 - analogRead(pin); 
    
    filtered = (filtered * 0.85) + (raw * 0.15);
    return (int)filtered;
}

void attachServo()
{
    if (!servoV.attached())
        servoV.attach(SERVO_VERTICAL_PIN);
}

void detachServo()
{
    servoV.detach();
}

// Retorna o motor para 90° de forma suave ao anoitecer
void returnToCenter()
{
    if (posV > 90)  posV--;
    if (posV < 90)  posV++;

    attachServo();
    servoV.write(posV);
}

void updateLCD(int diffV, float avgLight)
{
    lcd.setCursor(0, 0);
    lcd.print("Angulo V: ");
    lcd.print(posV);
    lcd.print("deg   ");

    lcd.setCursor(0, 1);
    lcd.print("Luz:");
    lcd.print((int)avgLight);
    lcd.print(" DifV:");
    lcd.print(diffV);
    lcd.print("   ");
}

// =====================================================
// SETUP
// =====================================================
void setup()
{
    Serial.begin(9600);

    lcd.init();
    lcd.backlight();
    lcd.setCursor(0, 0);
    lcd.print("Tracker 2 LDRs");
    lcd.setCursor(0, 1);
    lcd.print("Inicializando...");

    servoV.attach(SERVO_VERTICAL_PIN);
    servoV.write(posV);

    pidV.SetOutputLimits(-5, 5); // Passos máximos de 5° por ciclo
    pidV.SetSampleTime(250);
    pidV.SetMode(AUTOMATIC);

    delay(2000);
    lcd.clear();
}

// =====================================================
// LOOP PRINCIPAL
// =====================================================
void loop()
{
    // Temporização estável de 250ms
    if (millis() - lastLoop < LOOP_INTERVAL)
        return;

    lastLoop = millis();

    // 1. LEITURA DIRETA DOS DOIS LDRS
    int top    = readLDRFiltered(LDR_TOP, filteredTop);
    int bottom = readLDRFiltered(LDR_BOTTOM, filteredBottom);

    // 2. CÁLCULO DO ERRO E DA MÉDIA GLOBAL
    int diffV = top - bottom;                 // Diferença direta (Cima menos Baixo)
    float average = (top + bottom) / 2.0;     // Média real de luz atual

    // 3. VERIFICAÇÃO DE MODO NOTURNO
    if (average < NIGHT_THRESHOLD)
    {
        returnToCenter(); 
        detachServo();    
        
        lcd.setCursor(0, 1);
        lcd.print("Modo Noturno    ");
        return; 
    }

    attachServo(); 

    // 4. APLICAÇÃO DA ZONA MORTA
    if (abs(diffV) < DEADBAND)
        diffV = 0;

    // 5. PROCESSAMENTO DO PID E ATUALIZAÇÃO DA POSIÇÃO
    inputV = diffV;
    pidV.Compute();

    // NOTA: Se o motor se mover na direção OPONENTE à luz no simulador,
    // mude o sinal de "-=" para "+=" na linha abaixo:
    posV -= (int)outputV;

    // Restringe o movimento dentro da calha física de 0 a 180 graus
    posV = constrain(posV, V_MIN, V_MAX);

    // Atualiza fisicamente o Servo
    servoV.write(posV);

    // 6. ATUALIZAÇÃO DO LCD
    if (millis() - lastLCD > LCD_INTERVAL)
    {
        lastLCD = millis();
        updateLCD(diffV, average);
    }

    // 7. TELEMETRIA SERIAL (MONITOR)
    Serial.print("Cima:");    Serial.print(top);
    Serial.print(" Baixo:");   Serial.print(bottom);
    Serial.print(" M_Luz:");   Serial.print(average);
    Serial.print(" AnguloV:"); Serial.print(posV);
    Serial.print(" ErroV:");   Serial.println(diffV);
}