#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <math.h>

#define IR_Sensor1 2 // Change these to the actual pin numbers
#define IR_Sensor2 3
#define Ledred 13
#define Ledgreen 12

LiquidCrystal_I2C lcd(0x27, 16, 2); // LCD with I2C address 0x27

volatile unsigned long startMicros = 0;
volatile unsigned long endMicros = 0;
volatile bool measurementComplete = false;
volatile bool firstSensorTriggered = false; // New variable

void setup() {
  // Initialize IR sensors as inputs
  pinMode(IR_Sensor1, INPUT);
  pinMode(IR_Sensor2, INPUT);
  pinMode(Ledred, OUTPUT);
  pinMode(Ledgreen, OUTPUT);

  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(3, 0);
  lcd.print("Free Fall");
  delay(3000);
  digitalWrite(Ledgreen , HIGH);

  // Set up interrupts
  attachInterrupt(digitalPinToInterrupt(IR_Sensor1), ISR_IR_Sensor1, CHANGE);
  attachInterrupt(digitalPinToInterrupt(IR_Sensor2), ISR_IR_Sensor2, CHANGE);
}

void loop() {
  // If the measurement is complete, calculate the free-fall time and display it
  if (measurementComplete && endMicros >0 ) {
    double free_fall_time = (endMicros - startMicros) / 1000000.0; // Convert to seconds

    double sqrt_time = sqrt(free_fall_time);
    double unrooted_time = sqrt_time - 0.0832; // Adjust the calculated time

    // Display the unrooted time on the LCD
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Time (s):");
    lcd.setCursor(0, 1);
    lcd.print(unrooted_time, 4);

    // Reset the measurement flag
    measurementComplete = false;
    firstSensorTriggered = false; // Reset first sensor flag
  }
}

void ISR_IR_Sensor1() {
  // Triggered by a change in the state of IR_Sensor1
  if (digitalRead(IR_Sensor1) == HIGH) {
    // Start timer when the object is released
    startMicros = micros();
    firstSensorTriggered = true; // Set flag to indicate first sensor was triggered
    digitalWrite(Ledred , HIGH);
    digitalWrite(Ledgreen , LOW);
  }
}

void ISR_IR_Sensor2() {
  // Triggered by a change in the state of IR_Sensor2
  if (digitalRead(IR_Sensor2) == LOW) {
    // Only process if the first sensor was triggered
    if (firstSensorTriggered) {
      // Stop timer when the object passes the second sensor
      endMicros = micros();
      measurementComplete = true; // Set flag to indicate measurement is complete

      digitalWrite(Ledred , LOW);
      digitalWrite(Ledgreen , HIGH);
    }
  }
}