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

// =====================================================
// PINOS
// =====================================================

#define SERVO_HORIZONTAL_PIN 3
#define SERVO_VERTICAL_PIN   5

#define LDR_TOP_RIGHT    A0
#define LDR_BOTTOM_RIGHT A1
#define LDR_TOP_LEFT     A2
#define LDR_BOTTOM_LEFT  A3

// =====================================================
// LCD
// =====================================================

LiquidCrystal_I2C lcd(0x27, 16, 2);

// =====================================================
// LIMITES MECÂNICOS
// =====================================================

const int H_MAX = 180;
const int H_MIN = 60;

const int V_MAX = 120;
const int V_MIN = 20;

// =====================================================
// POSIÇÕES INICIAIS
// =====================================================

int posH = 120;
int posV = 60;

// =====================================================
// CONFIGURAÇÕES
// =====================================================

const int DEADBAND = 15;

const unsigned long LOOP_INTERVAL = 250;
const unsigned long LCD_INTERVAL  = 500;

const int NIGHT_THRESHOLD = 15;

// =====================================================
// SERVOS
// =====================================================

Servo servoH;
Servo servoV;

// =====================================================
// PID
// =====================================================

double inputH;
double outputH;
double setpointH = 0;

double inputV;
double outputV;
double setpointV = 0;

// PID Horizontal
double KpH = 0.45;
double KiH = 0.002;
double KdH = 0.25;

// PID Vertical
double KpV = 0.40;
double KiV = 0.002;
double KdV = 0.20;

PID pidH(&inputH, &outputH, &setpointH,
         KpH, KiH, KdH, DIRECT);

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

// =====================================================
// CONTROLE DE TEMPO
// =====================================================

unsigned long lastLoop = 0;
unsigned long lastLCD  = 0;

// =====================================================
// FILTROS
// =====================================================

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

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

int readLDRFiltered(int pin, float &filtered)
{
    int raw = analogRead(pin);

    filtered =
        (filtered * 0.85) +
        (raw * 0.15);

    return (int)filtered;
}

void attachServos()
{
    if (!servoH.attached())
        servoH.attach(SERVO_HORIZONTAL_PIN);

    if (!servoV.attached())
        servoV.attach(SERVO_VERTICAL_PIN);
}

void detachServos()
{
    servoH.detach();
    servoV.detach();
}

void moveServos()
{
    servoH.write(posH);
    servoV.write(posV);
}

bool isNight(float average)
{
    return average < NIGHT_THRESHOLD;
}

void returnEast()
{
    if (posH > 120)
        posH--;

    if (posH < 120)
        posH++;

    if (posV > 60)
        posV--;

    if (posV < 60)
        posV++;

    attachServos();
    moveServos();
}

void updateLCD(
    int diffH,
    int diffV,
    float avgLight)
{
    lcd.setCursor(0, 0);

    lcd.print("H:");
    lcd.print(posH);

    lcd.print(" V:");
    lcd.print(posV);
    lcd.print("   ");

    lcd.setCursor(0, 1);

    lcd.print("L:");
    lcd.print((int)avgLight);

    lcd.print(" D:");
    lcd.print(diffH);

    lcd.print("   ");
}

// =====================================================
// SETUP
// =====================================================

void setup()
{
    Serial.begin(9600);

    lcd.init();
    lcd.backlight();

    lcd.setCursor(0, 0);
    lcd.print("Solar Tracker");

    lcd.setCursor(0, 1);
    lcd.print("Inicializando");

    servoH.attach(SERVO_HORIZONTAL_PIN);
    servoV.attach(SERVO_VERTICAL_PIN);

    moveServos();

    pidH.SetOutputLimits(-5, 5);
    pidV.SetOutputLimits(-5, 5);

    pidH.SetSampleTime(250);
    pidV.SetSampleTime(250);

    pidH.SetMode(AUTOMATIC);
    pidV.SetMode(AUTOMATIC);

    delay(2000);

    lcd.clear();
}

// =====================================================
// LOOP
// =====================================================

void loop()
{
    if (millis() - lastLoop < LOOP_INTERVAL)
        return;

    lastLoop = millis();

    // =================================================
    // LEITURA DOS LDRs
    // =================================================

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

    // =================================================
    // MÉDIAS
    // =================================================

    int top    = (tr + tl) / 2;
    int bottom = (br + bl) / 2;

    int right  = (tr + br) / 2;
    int left   = (tl + bl) / 2;

    int diffH = right - left;
    int diffV = top - bottom;

    float average =
        (tr + tl + br + bl) / 4.0;

    // =================================================
    // MODO NOTURNO
    // =================================================

    if (isNight(average))
    {
        returnEast();

        detachServos();

        lcd.setCursor(0, 1);
        lcd.print("Modo Noturno ");

        delay(500);

        return;
    }

    attachServos();

    // =================================================
    // DEADBAND
    // =================================================

    if (abs(diffH) < DEADBAND)
        diffH = 0;

    if (abs(diffV) < DEADBAND)
        diffV = 0;

    // =================================================
    // PID
    // =================================================

    inputH = diffH;
    inputV = diffV;

    pidH.Compute();
    pidV.Compute();

    posH += (int)outputH;

    // Invertido propositalmente
    posV -= (int)outputV;

    // =================================================
    // LIMITES
    // =================================================

    posH = constrain(posH, H_MIN, H_MAX);
    posV = constrain(posV, V_MIN, V_MAX);

    // =================================================
    // MOVIMENTO
    // =================================================

    moveServos();

    // =================================================
    // LCD
    // =================================================

    if (millis() - lastLCD > LCD_INTERVAL)
    {
        lastLCD = millis();

        updateLCD(diffH, diffV, average);
    }

    // =================================================
    // SERIAL DEBUG
    // =================================================

    Serial.print("TR:");
    Serial.print(tr);

    Serial.print(" TL:");
    Serial.print(tl);

    Serial.print(" BR:");
    Serial.print(br);

    Serial.print(" BL:");
    Serial.print(bl);

    Serial.print(" H:");
    Serial.print(posH);

    Serial.print(" V:");
    Serial.print(posV);

    Serial.print(" DiffH:");
    Serial.print(diffH);

    Serial.print(" DiffV:");
    Serial.println(diffV);
}