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

#define IR_Sensor1 2
#define IR_Sensor2 3
#define Ledred 13
#define Ledgreen 10

#define TRIG_PIN 9
#define ECHO_PIN 8
#define MAX_DISTANCE 400

LiquidCrystal_I2C lcd(0x27, 16, 2);
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

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

double g = 9.81;
double t ;

void setup() {
  
  Serial.begin(9600);
  pinMode(IR_Sensor1, INPUT);
  pinMode(IR_Sensor2, INPUT);
  pinMode(Ledred, OUTPUT);
  pinMode(Ledgreen, OUTPUT);


  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Free Fall (s):");
  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() {

  unsigned int distanceCm = sonar.ping_cm();
  float distanceM = (distanceCm ) / 100.0;
  Serial.print(distanceM);
  Serial.print("m");
  Serial.println();
  double t = sqrt((2 * distanceM) / g);

  if (measurementComplete) {
    double free_fall_time = (endMicros - startMicros);

    double sqrt_time = sqrt(free_fall_time);
    double unrooted_time = sqrt_time - 0.0832;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Time (s):");
    lcd.setCursor(0, 1);
    lcd.print(t, 4);

    measurementComplete = false;
    firstSensorTriggered = false;
  }
}

void ISR_IR_Sensor1() {

  if (digitalRead(IR_Sensor1) == HIGH) {
    startMicros = micros();
    firstSensorTriggered = true;
    digitalWrite(Ledred , HIGH);
    digitalWrite (Ledgreen,LOW);
  }
}

void ISR_IR_Sensor2() {

  if (digitalRead(IR_Sensor2) == LOW) {
    if (firstSensorTriggered) {
      endMicros = micros();
      measurementComplete = true;
      digitalWrite(Ledgreen, HIGH);
      digitalWrite(Ledred, LOW);
    }
  }
}