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

// LCD setup
LiquidCrystal_I2C lcd(0x27, 16, 2);

// IR sensor pins
const int IR_SENSOR_1 = 2;
const int IR_SENSOR_2 = 3;

// Distance between sensors in meters
const float SENSOR_DISTANCE = 0.1;  // 10cm

// Variables for speed calculation
volatile bool sensor1_triggered = false;
volatile bool sensor2_triggered = false;
volatile unsigned long time1 = 0;
volatile unsigned long time2 = 0;
volatile bool measurement_complete = false;

// Speed and RPM variables
float current_speed = 0.0;
float current_rpm = 0.0;
float wheel_circumference = 0.094247;  // 3cm diameter wheel

void setup() {
  Serial.begin(9600);
  
  // Initialize LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.print("Speed Monitor");
  delay(1000);
  
  // Setup pins
  pinMode(IR_SENSOR_1, INPUT);
  pinMode(IR_SENSOR_2, INPUT);
  
  // Attach interrupts
  attachInterrupt(digitalPinToInterrupt(IR_SENSOR_1), sensor1_interrupt, FALLING);
  attachInterrupt(digitalPinToInterrupt(IR_SENSOR_2), sensor2_interrupt, FALLING);
  
  // Show initial display
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Speed:0.00 m/s");
  lcd.setCursor(0, 1);
  lcd.print("RPM: 0.00");
}

void loop() {
  // Check if measurement is complete
  if (measurement_complete) {
    calculateSpeed();
    updateDisplay();
    resetSensors();
    measurement_complete = false;
  }
  
  // Reset if timeout (5 seconds)
  if (sensor1_triggered && !sensor2_triggered && (millis() - time1/1000 > 5000)) {
    resetSensors();
  }
}

// Interrupt for first sensor
void sensor1_interrupt() {
  if (!sensor1_triggered) {
    time1 = micros();
    sensor1_triggered = true;
    Serial.print("time1: ");
    Serial.println(time1);
  }
}

// Interrupt for second sensor
void sensor2_interrupt() {
  if (sensor1_triggered && !sensor2_triggered) {
    time2 = micros();
    sensor2_triggered = true;
    measurement_complete = true;
    Serial.print("time2: ");
    Serial.println(time2);
  }
}

void calculateSpeed() {
  if (time2 > time1) {
    float time_diff = (time2 - time1) / 1000000.0;  // Convert to seconds
    current_speed = SENSOR_DISTANCE / time_diff;
    current_rpm = (current_speed / wheel_circumference) * 60.0;
    Serial.print("time_diff: ");
    Serial.println(time_diff,5);
    Serial.print("current_speed: ");
    Serial.println(current_speed,5);
    Serial.print("current_rpm: ");
    Serial.println(current_rpm,5);
  }
}

void updateDisplay() {
  lcd.setCursor(6, 0);
  lcd.print(current_speed, 2);
  lcd.print(" m/s ");
  
  lcd.setCursor(5, 1);
  lcd.print(current_rpm, 2);
  lcd.print("    ");
}

void resetSensors() {
  sensor1_triggered = false;
  sensor2_triggered = false;
  time1 = 0;
  time2 = 0;
}