#include <Wire.h> // Include the Wire library to use I2C bus

// Define sensor pins for 16 cylinders starting from pin 22
const int sensorUpPin[] = {22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52};    // UP Sensors
const int sensorDownPin[] = {23, 25, 27, 29, 31, 33, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53};  // DOWN Sensors

// Variables to store times and direction for each cylinder
unsigned long startTime[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
unsigned long endTime[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
unsigned long movementTime[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
bool measurementInProgress[16] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false};
String direction[16] = {"", "", "", "", "", "", "", "", "", "", "", "", "", "", "", ""};

// Add variables for debounce
unsigned long lastDebounceTime[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; // Last debounce time for each sensor
const unsigned long debounceInterval = 50;        // Time interval to ignore bounces (in milliseconds)

// I2C address to communicate with a Raspberry Pi unit
const int i2cAddress = 8;

void setup() {
  // Start serial communication
  Serial.begin(9600);

  // Initialize I2C communications as a slave
  Wire.begin(i2cAddress);

  // Function to call when I2C data is received
  Wire.onReceive(receiveDataFromMaster);

  // Initialize sensor pins as input
  for (int i = 0; i < 16; i++) {
    if (isValidPin(sensorUpPin[i]) && isValidPin(sensorDownPin[i])) {
      pinMode(sensorUpPin[i], INPUT);
      pinMode(sensorDownPin[i], INPUT);
    } else {
      Serial.println("Error: Invalid pin number specified for sensor.");
    }
  }
}

void loop() {
  for (int i = 0; i < 16; i++) {
    int upState = digitalRead(sensorUpPin[i]);
    int downState = digitalRead(sensorDownPin[i]);

    // Check if sensors are LOW and enough time has passed since the last bounce
    if (upState == LOW && downState == LOW && !measurementInProgress[i] && (millis() - lastDebounceTime[i]) > debounceInterval) {
      // Start measurement
      startTime[i] = millis();
      measurementInProgress[i] = true;
      direction[i] = "waiting"; // Initially, we don't know the direction, it should be UP or DOWN...
      lastDebounceTime[i] = millis(); // Update the last debounce time
    }

    // If measurement is in progress and one of the sensors goes HIGH
    if (measurementInProgress[i] && ((upState == HIGH || downState == HIGH) && (millis() - lastDebounceTime[i]) > debounceInterval)) {
      endTime[i] = millis();
      measurementInProgress[i] = false;

      // Determine the direction based on which sensor went HIGH
      if (upState == HIGH) {
        direction[i] = "DOWN to UP";
      } else if (downState == HIGH) {
        direction[i] = "UP to DOWN";
      }

      // Calculate the movement time
      movementTime[i] = endTime[i] - startTime[i];

      // Print the movement time and direction for the current cylinder
      Serial.print("Cylinder ");
      Serial.print(i + 1);
      Serial.print(": ");
      Serial.print(direction[i]);
      Serial.print(" in ");
      Serial.print(movementTime[i]);
      Serial.println(" ms");

      lastDebounceTime[i] = millis(); // Update the last debounce time

      // Send the data to Raspberry Pi via I2C
      sendDataToPi(i, direction[i], movementTime[i]);

      // Add a delay before measuring again
      delay(1000);
    }
  }
}

// Function to send data to Raspberry Pi via I2C
void sendDataToPi(int cylinder, String dir, unsigned long time) {
  Wire.beginTransmission(i2cAddress); // Begin I2C transmission
  Wire.write(cylinder); // Send cylinder number
  Wire.write(dir.c_str()); // Send direction as a string
  Wire.write((byte*)&time, sizeof(time)); // Send movement time as bytes
  Wire.endTransmission(); // End transmission
}

// Receive data from the I2C master
void receiveDataFromMaster(int byteCount) {
  // Placeholder function, initially this function is not implemented
}

// Function to check if a pin number is valid
bool isValidPin(int pin) {
  return (pin >= 22 && pin <= 53); // Adjusted for Arduino Mega's pin range
}