#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
// =====================================================
#define SERVO_VERTICAL_PIN   5  // Pino PWM que controla o servo vertical

#define LDR_TOP_RIGHT    A0     // Sensor Superior Direito
#define LDR_BOTTOM_RIGHT A1     // Sensor Inferior Direito
#define LDR_TOP_LEFT     A2     // Sensor Superior Esquerdo
#define LDR_BOTTOM_LEFT  A3     // Sensor Inferior Esquerdo

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

// =====================================================
// LIMITES MECÂNICOS (AGORA 180 GRAUS)
// =====================================================
const int V_MAX = 180;          // Limite máximo ajustado para 180°
const int V_MIN = 0;            // Limite mínimo ajustado para 0°

// =====================================================
// POSIÇÃO INICIAL
// =====================================================
int posV = 90;                  // Inicia o servo exatamente no meio (90°)

// =====================================================
// CONFIGURAÇÕES
// =====================================================
const int DEADBAND = 15;        // Zona morta para evitar trepidação do motor
const int NIGHT_THRESHOLD = 15; // Limite de leitura para ativar o modo noturno

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

// =====================================================
// SERVO & PID
// =====================================================
Servo servoV;                   // Objeto de controle do único servo ativo

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

// Constantes de Sintonia do PID Vertical
double KpV = 0.40;
double KiV = 0.002;
double KdV = 0.20;

// Inicializa o motor do PID Vertical
PID pidV(&inputV, &outputV, &setpointV, KpV, KiV, KdV, DIRECT);

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

float filteredTR = 0;
float filteredTL = 0;
float filteredBR = 0;
float filteredBL = 0;

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

// Filtro passa-baixas para suavizar a leitura dos sensores
int readLDRFiltered(int pin, float &filtered)
{
    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();
}

// Move o painel de volta para a posição central de espera 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 1 Eixo");
    lcd.setCursor(0, 1);
    lcd.print("Inicializando...");

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

    // Limites de alteração angular por ciclo (+/- 5 graus)
    pidV.SetOutputLimits(-5, 5);
    pidV.SetSampleTime(250);
    pidV.SetMode(AUTOMATIC);

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

// =====================================================
// LOOP PRINCIPAL
// =====================================================
void loop()
{
    // Temporização não-bloqueante (250ms)
    if (millis() - lastLoop < LOOP_INTERVAL)
        return;

    lastLoop = millis();

    // 1. LEITURA DOS SENSORES FILTRADOS
    int tr = readLDRFiltered(LDR_TOP_RIGHT, filteredTR);
    int tl = readLDRFiltered(LDR_TOP_LEFT, filteredTL);
    int br = readLDRFiltered(LDR_BOTTOM_RIGHT, filteredBR);
    int bl = readLDRFiltered(LDR_BOTTOM_LEFT, filteredBL);

    // 2. CÁLCULO DA MÉDIA DE CIMA VS. BAIXO
    int top    = (tr + tl) / 2; 
    int bottom = (br + bl) / 2; 

    int diffV = top - bottom;   // Erro vertical que o PID tentará corrigir
    float average = (tr + tl + br + bl) / 4.0; // Média global de luz

    // 3. MODO NOTURNO
    if (average < NIGHT_THRESHOLD)
    {
        returnToCenter(); // Retorna o motor para 90° gradualmente
        lcd.setCursor(0, 1);
        lcd.print("Modo Noturno    ");        
        return;
        detachServo();    // Desliga o servo para economizar energia 
    }

    attachServo(); // Garante que o servo está ligado durante o dia

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

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

    // Inversão proposital para corrigir o sentido de rotação física
    posV -= (int)outputV;

    // Garante que o motor respeite a nova faixa de 0° a 180°
    posV = constrain(posV, V_MIN, V_MAX);

    // Envia o comando de movimento para o servo
    servoV.write(posV);

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

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