#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

// Initialize the gyroscope sensor
Adafruit_MPU6050 gyro;
sensors_event_t event;

// Define the solenoid pin
const int solenoidPin = 7;

// Define ultrasonic sensor trigger pin
#define PIN_TRIG 3

// Define ultrasonic sensor echo pin
#define PIN_ECHO 2

// Define a struct to hold orientation data
struct OrientationData {
  float pitch;
  float roll;
  float yaw;
};

// Define a struct to hold sensor data
struct SensorData {
  float x;
  float y;
  float z;
};

// Initialize the current orientation and acceleration data
OrientationData currentOrientation = {0, 0, 0};
SensorData currentAcceleration = {0, 0, 0};

unsigned long emergencyAccelerationTimer = 0;
unsigned long emergencyAltitudeTimer = 0;
unsigned long emergencyDeployDelay = 5 * 1000;

// Define the threshold for detecting freefall
const float freefallThreshold = -10.0;
// Define the ultrasonic sensor threshold for desired altitude in centimeters
const float altitudeThreshold = 300.0;
// Define the ultrasonic sensor threshold for safe altitude in centimeters
const float altitudeSafetyWindow = 100.0;

// Define the debounce interval in milliseconds
const unsigned long debounceIntervalAccel = 3 * 1000;
const unsigned long debounceIntervalAltitude = 3 * 1000;
// Initialize the debounce timer
unsigned long debounceTimerAccel = 0;
unsigned long debounceTimerAltitude = 0;

// Initialize the state of the falling condition
bool isFalling = false;

// Initialize the state of the overAltitude condition
bool isOverAltitude = false;

// Initialize the state of the desired altitude condition
bool isAtDesiredAltitude = false;

// Initialize the state of the desired acceleration condition
bool isAtDesiredAcceleration = false;

// Initialize the emergency deploy condition
bool isEmergencyDeploy = false;

bool isHatchDeployed = false;

void setup() {
  // Initialize the serial communication
  Serial.begin(115200);

  // Initialize the gyroscope sensor
  while (!gyro.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  // Initialize the solenoid pin as an output
  pinMode(solenoidPin, OUTPUT);

  // Initialize the trigger pin as an output
  pinMode(PIN_TRIG, OUTPUT);

  // Initialize the echo pin as an input
  pinMode(PIN_ECHO, INPUT);
}

void loop() {
  if (!isHatchDeployed) {
    gyro.getAccelerometerSensor()->getEvent(&event);
    testFalling();
    testAltitude();
    testDesiredCondition(measureDistance() >= altitudeThreshold, isAtDesiredAltitude, emergencyAltitudeTimer, debounceTimerAccel, debounceIntervalAltitude, emergencyDeployDelay, "altitude", "Altimeter reports currently below safe altitude window after passing altitude threshold.", measureDistance() <= altitudeSafetyWindow);
    testDesiredCondition(event.acceleration.z <= freefallThreshold, isAtDesiredAcceleration, emergencyAccelerationTimer, debounceTimerAltitude, debounceIntervalAccel, emergencyDeployDelay, "acceleration", "Accelerometer reports negative G force for greater than safe duration.", true);
    testAllConditions();
  }
}

void testAllConditions() {
  gyro.getAccelerometerSensor()->getEvent(&event);

  // Get the z-axis acceleration
  float zAcceleration = event.acceleration.z;
  // Check if either of the desired conditions has been met
  if ((isAtDesiredAltitude && isAtDesiredAcceleration) || isEmergencyDeploy) {
    // Start the debounce timer
    debounceTimerAccel = millis();

    // Print a message to the serial monitor
    Serial.println("Deploying hatch!");

    // Deploy the hatch using the solenoid
    digitalWrite(solenoidPin, HIGH);

    // Wait for a brief period of time to ensure the hatch is deployed
    unsigned long hatchDeployTime = millis();
    while (millis() - hatchDeployTime < 5000) {
      //wait 5 seconds while hatch deploys
    }

    // Turn off the solenoid
    digitalWrite(solenoidPin, LOW);

    // Print a message to the serial monitor
    Serial.println("Hatch deployed!");

    // Reset the desired conditions
    isAtDesiredAltitude = false;
    isAtDesiredAcceleration = false;
    isEmergencyDeploy = false;
    isHatchDeployed = true;

    // TODO: Perform other functions which rely on the hatch being deployed if needed
  }

  // Check if the debounce timer has expired
  if (debounce(debounceTimerAccel, debounceIntervalAccel) && zAcceleration < altitudeThreshold) {
    // Reset the falling condition
    isFalling = false;
  }

  if ( debounce(debounceTimerAltitude, debounceIntervalAltitude) && measureDistance() < altitudeThreshold) {
    // Reset the falling condition
    isOverAltitude = false;
  }
}

// Measure the distance using the ultrasonic sensor
float measureDistance() {
  // Send a 10 microsecond pulse to the ultrasonic sensor trigger pin
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // Measure the duration of the echo pulse
  float duration = pulseIn(PIN_ECHO, HIGH);

  // Calculate the distance in centimeters using the speed of sound
  float distance = duration * 0.034 / 2.0;

  return distance;
}


void testFalling() {
  // Get the gyroscope data
  gyro.getAccelerometerSensor()->getEvent(&event);

  // Get the z-axis acceleration
  float zAcceleration = event.acceleration.z;

  // Check if the rocket has started falling
  if (zAcceleration < freefallThreshold && !isFalling) {
    // Set the falling condition
    isFalling = true;

    // Start the debounce timer
    debounceTimerAccel = millis();

    // Print a message to the serial monitor
    Serial.println("Rocket reports that it has started falling! Initiating debounce function.");
  }
  else if (zAcceleration >= freefallThreshold) {
    // Reset the falling condition if the rocket has stopped falling
    isFalling = false;
  }
}

void testAltitude() {

  // Check if the rocket has started falling
  if (measureDistance() >= altitudeThreshold && !isOverAltitude) {
    // Set the falling condition
    isOverAltitude = true;

    // Update the debounce timer
    debounceTimerAltitude = millis();

    // Print a message to the serial monitor
    Serial.println("Rocket altimeter reports above desired altitude! Initiating debounce function.");

  }
}

void testDesiredCondition(float threshold, bool &isAtDesiredCondition, unsigned long &emergencyTimer, unsigned long &debounceTimer, const unsigned long debounceInterval, const unsigned long emergencyDeployDelay, String conditionName, String emergencyWarning, bool optionalThreshold) {
  if (threshold && debounce(debounceTimer, debounceInterval) && !isAtDesiredCondition) {
    // Set the desired condition flag
    isAtDesiredCondition = true;
    emergencyTimer = millis();


    // Print a message to the serial monitor
    Serial.println("Debounce complete: Rocket has reached the desired " + conditionName + "!");
  }

  if (!threshold) {
    debounceTimer = millis();
  }

  // Check if the rocket has been at the desired condition for more than the emergency deploy delay
  if (isAtDesiredCondition && millis() - emergencyTimer >= emergencyDeployDelay && optionalThreshold) {
    // Set the emergency deploy flag
    isEmergencyDeploy = true;

    // Print a message to the serial monitor
    Serial.println(emergencyWarning + " Preparing for emergency hatch deployment.");
  }
}

// Debounce function to confirm the desired condition
bool debounce(unsigned long &debounceTimer, const unsigned long debounceInterval) {
  const long elapsedTime = millis() - debounceTimer;
  if (elapsedTime >= debounceInterval) {
    return true;
  }
  else {
    return false;
  }
}

void getOrientationAndAcceleration(float &orientation, float &acceleration) {
  sensors_event_t gyroEvent, accelEvent;
  float gx, gy, gz, ax, ay, az;
  float pitch, roll, accel;

  // Read the gyroscope data
  gyro.getEvent(&gyroEvent);
  gx = gyroEvent.gyro.x;
  gy = gyroEvent.gyro.y;
  gz = gyroEvent.gyro.z;

  // Read the accelerometer data
  accel.getEvent(&accelEvent);
  ax = accelEvent.acceleration.x;
  ay = accelEvent.acceleration.y;
  az = accelEvent.acceleration.z;

  // Convert the accelerometer data to pitch and roll angles
  pitch = atan2(ax, sqrt(ay*ay + az*az));
  roll = atan2(ay, sqrt(ax*ax + az*az));

  // Convert the gyroscope data to orientation angles
  float dt = (gyroEvent.timestamp - lastGyroTime) / 1000000.0; // Convert microseconds to seconds
  lastGyroTime = gyroEvent.timestamp;
  orientation += gx * dt;

  // Calculate the combined orientation and acceleration values
  acceleration = sqrt(ax*ax + ay*ay + az*az) - GRAVITY;
  orientation += roll;
}