#include <Wire.h>
#include <RTClib.h>
#include <Ultrasonic.h>

// Define new I2C pins for RTC
#define SDA_PIN 16
#define SCL_PIN 17

// Define pins for lift height sensor
const int trigPinA = 5;   // Trig pin for ultrasonic sensor A (lift height)
const int echoPinA = 18;  // Echo pin for ultrasonic sensor A (lift height)

// Define pins for fork extension sensor
const int trigPinB = 19;   // Trig pin for ultrasonic sensor B (fork extension)
const int echoPinB = 21;   // Echo pin for ultrasonic sensor B (fork extension)

// Variables to store sensor readings
long durationA, durationB;
int distanceA, distanceB;

// Variables for tracking fork extension and lift height
int maxForkExtension = 0;      // Store the maximum fork extension in a cycle
int maxForkExtensionHeight = 0; // Store the lift height when max extension occurred
String forkStatus = "";
bool isTiming = false;

// Time tracking variables for the lift
unsigned long startTime = 0;
unsigned long stopTime = 0;
unsigned long liftTime = 0;

// Variables for checking conditions every second
unsigned long previousMillis = 0;
const long interval = 1000;  // 1 second interval
int prevDistanceA = 0;
int prevDistanceB = 0;

// RTC and Ultrasonic sensor objects
RTC_DS3231 rtc;
Ultrasonic ultrasonicA(trigPinA, echoPinA);
Ultrasonic ultrasonicB(trigPinB, echoPinB);

void setup() {
  Serial.begin(115200);
  
  // Initialize I2C with new SDA and SCL pins
  Wire.begin(SDA_PIN, SCL_PIN);

  // Initialize RTC
  if (!rtc.begin()) {
    Serial.println("Couldn't find RTC");
    while (1);
  }

  if (rtc.lostPower()) {
    Serial.println("RTC lost power, setting the time!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  // Initialize ultrasonic sensor pins
  pinMode(trigPinA, OUTPUT);
  pinMode(echoPinA, INPUT);
  pinMode(trigPinB, OUTPUT);
  pinMode(echoPinB, INPUT);
}

void loop() {
  // Get the current time
  unsigned long currentMillis = millis();

  // Check the condition every second
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    // Read the lift height and fork extension
    distanceA = ultrasonicA.read();
    distanceB = ultrasonicB.read();

    // Get the current date and time
    DateTime now = rtc.now();
    Serial.print(now.timestamp(DateTime::TIMESTAMP_FULL));
    Serial.print(" - ");

    // Determine the machine condition
    if (distanceA == prevDistanceA && distanceB == prevDistanceB) {
      Serial.println("Machine Condition: Idle");
    } else {
      Serial.println("Machine Condition: Working");
    }

    // Update previous distances for the next check
    prevDistanceA = distanceA;
    prevDistanceB = distanceB;
  }

  // Start the timer when the lift goes above 10 cm
  if (distanceA > 10 && !isTiming) {
    startTime = millis();
    maxForkExtension = 0;  // Reset maximum fork extension for the new cycle
    maxForkExtensionHeight = 0;  // Reset height when max extension occurred
    isTiming = true;

    // Get current date and time
    DateTime now = rtc.now();
    Serial.print("Lift go up: ");
    Serial.print(now.timestamp(DateTime::TIMESTAMP_FULL));
    Serial.println();
  }

  // Monitor fork extension during the lift
  if (isTiming) {
    // Update the maximum fork extension and corresponding lift height
    if (distanceB > maxForkExtension) {
      maxForkExtension = distanceB;
      maxForkExtensionHeight = distanceA;  // Store lift height when max extension occurs
    }
  }

  // Stop the timer when the lift goes back below 10 cm
  if (distanceA <= 10 && isTiming) {
    stopTime = millis();
    liftTime = stopTime - startTime;
    isTiming = false;

    // Convert lift time to seconds
    float liftTimeSec = liftTime / 1000.0;

    // Determine fork status based on maximum extension
    if (maxForkExtension < 100) {
      forkStatus = "Single Deep";
    } else {
      forkStatus = "Double Deep";
    }

    // Get current date and time
    DateTime now = rtc.now();
    Serial.print("Lift go down: ");
    Serial.print(now.timestamp(DateTime::TIMESTAMP_FULL));
    Serial.println();

    // Output the lift time, maximum fork extension, and height at maximum extension
    Serial.print("Lift Time: ");
    Serial.print(liftTimeSec, 2);
    Serial.println(" s");
    Serial.print("Fork Extension: ");
    Serial.print(maxForkExtension);
    Serial.print(" cm | Fork Status: ");
    Serial.print(forkStatus);
    Serial.print("| Lift Height: ");
    Serial.print(maxForkExtensionHeight);
    Serial.println(" cm");


    

  }

  delay(50);  // Adjust the delay as needed
}