#include "Lpf2Hub.h"

// =====================
// LDR SETTINGS
// =====================
const int ldr1Pin = 39;
const int ldr2Pin = 34;
const int ldr3Pin = 36;
const int threshold = 2300;

// =====================
// HUB 1 (GREEN TRAIN)
// =====================
Lpf2Hub trainHub1;
char hub1Address[] = "90:84:2b:cd:a5:89";
byte motorPort1 = (byte)PoweredUpHubPort::A;
byte lightPort1 = (byte)PoweredUpHubPort::B;

// =====================
// HUB 2 (ORANGE TRAIN)
// =====================
Lpf2Hub trainHub2;
char hub2Address[] = "9c:9a:c0:03:e4:47";
byte motorPort2 = (byte)PoweredUpHubPort::A;
byte lightPort2 = (byte)PoweredUpHubPort::B;

// =====================
// L298N PINS
// =====================
#define IN1 27
#define IN2 16
#define ENA 14
#define IN3 17
#define IN4 25
#define ENB 26

// =====================
// STATE VARIABLES
// =====================
int ldr1HitCount = 0;
bool ldr1Ready = true;
bool ldr2Ready = true;
bool ldr2Triggered = false;
bool ldr3Ready = true;
bool ldr3Triggered = false;
bool orangeStarted = false;
bool io36Hit = false;
bool stopOrangeNext = true;

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

    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);
    pinMode(ENA, OUTPUT);
    pinMode(ENB, OUTPUT);
}

// =====================
// LOOP
// =====================
void loop() {

    // CONNECT HUBS
    if (!trainHub1.isConnected() && !trainHub1.isConnecting())
        trainHub1.init(hub1Address);

    if (trainHub1.isConnecting()) {
        trainHub1.connectHub();
        if (trainHub1.isConnected()) {
            trainHub1.setLedColor(GREEN);
            trainHub1.setBasicMotorSpeed(lightPort1, 100);
        }
    }

    if (!trainHub2.isConnected() && !trainHub2.isConnecting())
        trainHub2.init(hub2Address);

    if (trainHub2.isConnecting()) {
        trainHub2.connectHub();
        if (trainHub2.isConnected()) {
            trainHub2.setLedColor(ORANGE);
            trainHub2.setBasicMotorSpeed(lightPort2, 100);
        }
    }

    if (!trainHub1.isConnected() || !trainHub2.isConnected()) {
        delay(200);
        return;
    }

    // START ORANGE TRAIN
    if (!orangeStarted) {
        trainHub2.setBasicMotorSpeed(motorPort2, -45);
        orangeStarted = true;
    }

    // READ LDRS
    int ldr1 = analogRead(ldr1Pin);
    int ldr2 = analogRead(ldr2Pin);
    int ldr3 = analogRead(ldr3Pin);

    // ==================================================
    // IO36 SENSOR CHECK + ALTERNATING STOP
    // ==================================================
    if (ldr3 > threshold && ldr3Ready) {

        ldr3Ready = false;
        io36Hit = true;
        Serial.println("IO36 HIT → cycle allowed");

        if (ldr2Triggered && !ldr3Triggered) {

            ldr3Triggered = true;

            if (stopOrangeNext) {
                Serial.println("Stopping ORANGE train");
                trainHub2.setBasicMotorSpeed(motorPort2, 0);
            } else {
                Serial.println("Stopping GREEN train");
                trainHub1.setBasicMotorSpeed(motorPort1, 0);
            }

            stopOrangeNext = !stopOrangeNext;
        }
    }

    if (ldr3 < threshold - 200) {
        ldr3Ready = true;
        ldr3Triggered = false;
    }

    // =====================
    // LDR1 LOGIC (IO39)
    // =====================
    if (ldr1 > threshold && ldr1Ready && io36Hit) {

        ldr1HitCount++;
        ldr1Ready = false;

        Serial.print("IO39 HIT #: ");
        Serial.println(ldr1HitCount);

        if (ldr1HitCount == 1) {

            if (stopOrangeNext) {
                Serial.println("Step 1 → Start GREEN train");
                trainHub1.setBasicMotorSpeed(motorPort1, 45);;
            } else {
                Serial.println("Step 1 → Start ORANGE train");
                trainHub2.setBasicMotorSpeed(motorPort2, -45);
            }

            digitalWrite(IN3, HIGH);
            digitalWrite(IN4, LOW);
            analogWrite(ENB, 135);
            delay(300);
            analogWrite(ENB, 0);
        }

        else if (ldr1HitCount == 2) {
            Serial.println("Step 2 → Green ENB back");

            digitalWrite(IN3, LOW);
            digitalWrite(IN4, HIGH);
            analogWrite(ENB, 135);
            delay(300);
            analogWrite(ENB, 0);
        }

        else if (ldr1HitCount == 3) {
            Serial.println("Step 3 → Orange ENA forward");

            digitalWrite(IN1, LOW);
            digitalWrite(IN2, HIGH);
            analogWrite(ENA, 120);
            delay(300);
            analogWrite(ENA, 0);

            ldr2Triggered = false;
            io36Hit = false;

            trainHub2.setBasicMotorSpeed(motorPort2, -45);
        }

        if (ldr1HitCount >= 3)
            ldr1HitCount = 0;
    }

    if (ldr1 < threshold - 200)
        ldr1Ready = true;

    // =====================
    // LDR2 LOGIC
    // =====================
    if (ldr2 > threshold && ldr2Ready && !ldr2Triggered) {

        ldr2Ready = false;
        ldr2Triggered = true;

        Serial.println("LDR2 → ENA back");

        digitalWrite(IN1, HIGH);
        digitalWrite(IN2, LOW);
        analogWrite(ENA, 120);
        delay(300);
        analogWrite(ENA, 0);
    }

    if (ldr2 < threshold - 200)
        ldr2Ready = true;

    delay(20);
}