#define BLYNK_TEMPLATE_ID "TMPL2HOCEpKFn"
#define BLYNK_TEMPLATE_NAME "Final project FAB26"
#define BLYNK_AUTH_TOKEN "2KxwbJzsfgYgP-iGgJzATmubULcozdKy"

#include <WiFi.h>
#include <WiFiClient.h>
#include <BlynkSimpleEsp32.h>

// WiFi credentials
char ssid[] = "Red abierta UCUENCA";
char pass[] = "";

// HC-SR04 sensor pins
#define TRIG1 D3
#define ECHO1 D4                                    

#define TRIG2 D8
#define ECHO2 D9

// Status LEDs
#define RED_LED D0       // Inventory available
#define YELLOW_LED D1    // Part in process
#define GREEN_LED D10    // Finished part detected

// Reset button
#define RESET_BTN D2

// Configuration
const float DETECTION_DISTANCE = 10.0;
const unsigned long READING_INTERVAL = 300;

// Queue for active production cycles
const int MAX_CYCLES = 10;
unsigned long cycleStartTimes[MAX_CYCLES];
int pendingCycles = 0;

// Cycle time variables
float cycleTime = 0;
float totalCycleTime = 0;
float averageCycleTime = 0;

// Total operation timer
unsigned long operationStartTime = 0;
unsigned long currentOperationTime = 0;
bool operationStarted = false;

// Counters
int inventoryCount = 0;
int finishedParts = 0;

// Sensor states
bool sensor1Detects = false;
bool sensor2Detects = false;
bool previousSensor1 = false;
bool previousSensor2 = false;

// Productivity
float productivity = 0;

BlynkTimer timer;

float measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);

  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);

  if (duration == 0) {
    return 999;
  }

  return duration * 0.0343 / 2.0;
}

void updateLEDs() {
  // Red LED indicates that there is a part in the inventory area
  digitalWrite(RED_LED, sensor1Detects ? HIGH : LOW);

  // Yellow LED indicates that at least one part is currently in process
  digitalWrite(YELLOW_LED, pendingCycles > 0 ? HIGH : LOW);

  // Green LED only indicates physical presence at Sensor 2
  // Removing the part from Sensor 2 only turns off the LED.
  // It does not affect cycle calculations.
  digitalWrite(GREEN_LED, sensor2Detects ? HIGH : LOW);
}

void resetSystem() {
  pendingCycles = 0;

  cycleTime = 0;
  totalCycleTime = 0;
  averageCycleTime = 0;

  operationStartTime = 0;
  currentOperationTime = 0;
  operationStarted = false;

  inventoryCount = 0;
  finishedParts = 0;

  productivity = 0;

  previousSensor1 = false;
  previousSensor2 = false;

  digitalWrite(RED_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(GREEN_LED, LOW);

  Blynk.virtualWrite(V0, 0);
  Blynk.virtualWrite(V1, 0);
  Blynk.virtualWrite(V2, 0);
  Blynk.virtualWrite(V3, 0);
  Blynk.virtualWrite(V4, 0);
  Blynk.virtualWrite(V5, 0);
  Blynk.virtualWrite(V6, "System reset");
  Blynk.virtualWrite(V7, 0);
  Blynk.virtualWrite(V8, 0);
  Blynk.virtualWrite(V10, "00:00:00");
  Blynk.virtualWrite(V11, 0);
  Blynk.virtualWrite(V12, 0);

  Serial.println("System reset");
}

void updateOperationTimer() {
  if (operationStarted) {
    currentOperationTime = (millis() - operationStartTime) / 1000;

    int hours = currentOperationTime / 3600;
    int minutes = (currentOperationTime % 3600) / 60;
    int seconds = currentOperationTime % 60;

    char timeText[20];
    sprintf(timeText, "%02d:%02d:%02d", hours, minutes, seconds);

    Blynk.virtualWrite(V10, timeText);

    if (currentOperationTime > 0) {
      productivity = finishedParts / (currentOperationTime / 3600.0);
      Blynk.virtualWrite(V11, productivity);
    }
  }
}

void startNewCycle() {
  if (!operationStarted) {
    operationStartTime = millis();
    operationStarted = true;
  }

  if (pendingCycles < MAX_CYCLES) {
    cycleStartTimes[pendingCycles] = millis();
    pendingCycles++;

    Blynk.virtualWrite(V6, "Part in process");
    Blynk.virtualWrite(V12, pendingCycles);

    Serial.print("New part in process. Pending cycles: ");
    Serial.println(pendingCycles);
  } else {
    Blynk.virtualWrite(V6, "Error: cycle queue full");
    Serial.println("Error: cycle queue full");
  }
}

void finishCycle() {
  if (pendingCycles > 0) {
    unsigned long startTime = cycleStartTimes[0];

    cycleTime = (millis() - startTime) / 1000.0;

    // Move queue forward
    for (int i = 0; i < pendingCycles - 1; i++) {
      cycleStartTimes[i] = cycleStartTimes[i + 1];
    }

    pendingCycles--;

    finishedParts++;
    totalCycleTime += cycleTime;
    averageCycleTime = totalCycleTime / finishedParts;

    Blynk.virtualWrite(V2, cycleTime);
    Blynk.virtualWrite(V3, averageCycleTime);
    Blynk.virtualWrite(V5, finishedParts);
    Blynk.virtualWrite(V6, "Finished part");
    Blynk.virtualWrite(V7, averageCycleTime);
    Blynk.virtualWrite(V8, finishedParts);
    Blynk.virtualWrite(V12, pendingCycles);

    if (currentOperationTime > 0) {
      productivity = finishedParts / (currentOperationTime / 3600.0);
      Blynk.virtualWrite(V11, productivity);
    }

    Serial.print("Finished part | Cycle time: ");
    Serial.print(cycleTime);
    Serial.println(" s");

    Serial.print("Average cycle time: ");
    Serial.print(averageCycleTime);
    Serial.println(" s");

    Serial.print("Pending cycles: ");
    Serial.println(pendingCycles);
  } else {
    Blynk.virtualWrite(V6, "Finished part without active cycle");
    Serial.println("Sensor 2 detected a part, but there is no pending cycle");
  }
}

void readSensors() {
  float distance1 = measureDistance(TRIG1, ECHO1);
  delay(60);
  float distance2 = measureDistance(TRIG2, ECHO2);

  sensor1Detects = distance1 <= DETECTION_DISTANCE;
  sensor2Detects = distance2 <= DETECTION_DISTANCE;

  Blynk.virtualWrite(V0, distance1);
  Blynk.virtualWrite(V1, distance2);

  Serial.print("Sensor 1: ");
  Serial.print(distance1);
  Serial.print(" cm | Sensor 2: ");
  Serial.print(distance2);
  Serial.println(" cm");

  // Sensor 1 detects a part: inventory available
  if (sensor1Detects && !previousSensor1) {
    inventoryCount++;

    Blynk.virtualWrite(V4, inventoryCount);
    Blynk.virtualWrite(V6, "Part in inventory");

    Serial.println("Part in inventory");
  }

  // Sensor 1 stops detecting: a new part enters the assembly process
  if (!sensor1Detects && previousSensor1) {
    startNewCycle();
  }

  // Sensor 2 detects a part: finish the oldest pending cycle
  // Removing the part from Sensor 2 will not recalculate anything.
  if (sensor2Detects && !previousSensor2) {
    finishCycle();
  }

  previousSensor1 = sensor1Detects;
  previousSensor2 = sensor2Detects;

  updateLEDs();
}

void checkResetButton() {
  if (digitalRead(RESET_BTN) == LOW) {
    delay(300);
    resetSystem();
  }
}

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

  pinMode(TRIG1, OUTPUT);
  pinMode(ECHO1, INPUT);

  pinMode(TRIG2, OUTPUT);
  pinMode(ECHO2, INPUT);

  pinMode(RED_LED, OUTPUT);
  pinMode(YELLOW_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);

  pinMode(RESET_BTN, INPUT_PULLUP);

  digitalWrite(RED_LED, LOW);
  digitalWrite(YELLOW_LED, LOW);
  digitalWrite(GREEN_LED, LOW);

  Blynk.begin(BLYNK_AUTH_TOKEN, ssid, pass);

  timer.setInterval(READING_INTERVAL, readSensors);
  timer.setInterval(200L, checkResetButton);
  timer.setInterval(1000L, updateOperationTimer);
  timer.setInterval(100L, updateLEDs);

  Serial.println("Kaizen Kart started");
}

void loop() {
  Blynk.run();
  timer.run();
}