#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <Adafruit_BMP280.h>
#include <Servo.h>

#define button 7
#define buzzer 3
#define c1 4
#define c2 2
#define BMP_SCK 13
#define BMP_MISO 12
#define BMP_MOSI 11
#define BMP_CS 10

const float Xtune = 0;
const float Ytune = 0;
Adafruit_BMP280 bmp;
MPU6050 mpu6050(Wire);

File data;
File logs;

Servo servoX;
Servo servoY;

float altitude, initial_altitude, highest_altitude, gyro_x, gyro_y, accel_x, accel_y;
bool log_data = true, apogee = false, failable = false;

void setup() {
  pinMode(button, INPUT_PULLUP);
  pinMode(buzzer, OUTPUT);
  pinMode(c1, OUTPUT);
  pinMode(c2, OUTPUT);
  servoX.attach(6);
  servoY.attach(5);
  Serial.begin(9600);
  digitalWrite(c1, LOW);
  digitalWrite(c2, LOW);

  bmp.begin();
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, Adafruit_BMP280::SAMPLING_X2, Adafruit_BMP280::SAMPLING_X16, Adafruit_BMP280::FILTER_X16, 1);

  Wire.begin();
  mpu6050.begin();

  if (!SD.begin(10)) {
    while (!SD.begin(10)) {
      tone(buzzer, 600);
      delay(200);
      noTone(buzzer);
      delay(200);
      tone(buzzer, 450);
      delay(200);
      noTone(buzzer);
    }
  }

  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Arduino Reset");
  logs.close();
  calibrate();
}

void calibrate() {
  while (digitalRead(button) == LOW) {}

  tone(buzzer, 261);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 329);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 392);
  delay(200);
  noTone(buzzer);
  delay(2000);

  mpu6050.calcGyroOffsets(true);
  initial_altitude = bmp.readAltitude(1013.25);

  tone(buzzer, 659);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 784);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 1047);
  delay(200);
  noTone(buzzer);
}

void beep(int frequency, int duration) {
  tone(buzzer, frequency);
  delay(duration);
  noTone(buzzer);
}

bool handleButtonPresses() {
  bool buttonPressed = false;
  unsigned long lastPressTime = 0;
  unsigned long debounceDelay = 20;
  unsigned long cooldownDuration = 60;
  unsigned int buttonCounter = 0;
  unsigned long startTime = millis();

  while (millis() - startTime < 2000) {
    int buttonState = digitalRead(button);

    if (buttonState == HIGH && millis() - lastPressTime >= debounceDelay && !buttonPressed) {
      buttonPressed = true;
      beep(500, 100);
      lastPressTime = millis();
      buttonCounter++;
      delay(cooldownDuration);
    }

    if (buttonState == LOW && buttonPressed) {
      buttonPressed = false;
    }

    if (buttonCounter == 5 && (millis() - lastPressTime) < 2500) {
      buttonCounter = 0;
      return true;
    }
  }

  buttonCounter = 0;
  return false;
}

bool countdown() {
  unsigned long startTime = millis();
  unsigned long beepInterval = 900;
  unsigned long countdownDuration = 31800;
  unsigned long lastBeepTime = millis();
  fadeBuzzer(200, 2500, 2000, true);
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Countdown Started");
  logs.close();
  failable = true;
  while (millis() - startTime < countdownDuration) {
    if ((digitalRead(button) == HIGH || apogee == true)) {
      fadeBuzzer(200, 2500, 2000, false);
      logs = SD.open("logs.txt", FILE_WRITE);
      logs.println("Countdown Canceled");
      logs.close();
      apogee = false;
      return false;
    }
    sensors();
    if (millis() - lastBeepTime >= beepInterval) {
      if (millis() - startTime >= countdownDuration - 6000) {
        tone(buzzer, 2500);
        if ((digitalRead(button) == HIGH || apogee == true)) {
          fadeBuzzer(200, 2500, 2000, false);
          logs = SD.open("logs.txt", FILE_WRITE);
          logs.println("Countdown Canceled");
          logs.close();
          apogee = false;
          return false;
        }
      } else {
        beep(2500, 100);
      }
      lastBeepTime = millis();
    }
  }
  noTone(buzzer);
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Main Motor Ignition");
  logs.close();
  data = SD.open("data.txt", FILE_WRITE);
  data.println("Launch!");
  digitalWrite(c1, HIGH);
  return true;
}

void sensors() {
  mpu6050.update();
  float current_altitude = bmp.readAltitude(1013.25) - initial_altitude;
  gyro_x = mpu6050.getAngleX();
  gyro_y = mpu6050.getAngleY();
  accel_x = mpu6050.getAccX();
  accel_y = mpu6050.getAccY();
  if (log_data) {
    data.print(current_altitude);
    data.print(",");
    data.print(gyro_x);
    data.print(",");
    data.print(gyro_y);
    data.print(",");
    data.print(accel_x);
    data.print(",");
    data.print(accel_y);
    data.println();
    if ((gyro_x > 70 || gyro_x < -70 || gyro_y > 70 || gyro_y < -70) && failable) {
      apogee = true;
    }
    if (current_altitude < highest_altitude - 1) {
      apogee = true;
    }
    if (apogee) {
      return;
    }
    if (current_altitude > highest_altitude) {
      highest_altitude = current_altitude;
    }
  }
  log_data = !log_data;
}

void fadeBuzzer(int startFrequency, int endFrequency, int totalDuration, bool fadeUp) {
  int steps = abs(endFrequency - startFrequency) / 10;
  int stepDuration = totalDuration / steps;

  if (fadeUp) {
    for (int frequency = startFrequency; frequency <= endFrequency; frequency += 10) {
      beep(frequency, stepDuration);
      sensors();
    }
  } else {
    for (int frequency = endFrequency; frequency >= startFrequency; frequency -= 10) {
      beep(frequency, stepDuration);
    }
  }
}

void land() {
  digitalWrite(c1, LOW);
  digitalWrite(c2, HIGH);
  servoX.write(90 + Xtune);
  servoY.write(90 + Ytune);
  data.println("Parachute deploying");
  data.close();
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Parachute Motor Ignition");
  logs.close();

  unsigned long lastBeepTime = millis();
  while (true) {
    // Beep for 500ms every second
    if (millis() - lastBeepTime >= 1000) {
      tone(buzzer, 2500);
      delay(500);
      noTone(buzzer);
      lastBeepTime = millis();
    }
    data = SD.open("data.txt", FILE_WRITE);
    sensors();
    data.close();
  }
}

void TVC(){
  sensors();

}




void loop() {
  land();
  delay(1000000);
  while (handleButtonPresses() == false) {}
  if (countdown()) {
    while (apogee == false) {
      TVC();
    }
    Serial.println("LANDING");
    land();
  }
}

// servoX on inside, servoY on outside of PCB

#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <Adafruit_BMP280.h>
#include <Servo.h>

#define button 7
#define buzzer 3
#define c1 4
#define c2 2
#define BMP_SCK 13
#define BMP_MISO 12
#define BMP_MOSI 11
#define BMP_CS 10

const float Xtune = 0;
const float Ytune = 0;
Adafruit_BMP280 bmp;
MPU6050 mpu6050(Wire);

File data;
File logs;

Servo servoX;
Servo servoY;

float altitude, initial_altitude, highest_altitude, gyro_x, gyro_y, accel_x, accel_y;
bool log_data = true, apogee = false, failable = false;

void setup() {
  pinMode(button, INPUT_PULLUP);
  pinMode(buzzer, OUTPUT);
  pinMode(c1, OUTPUT);
  pinMode(c2, OUTPUT);
  servoX.attach(6);
  servoY.attach(5);
  Serial.begin(9600);
  digitalWrite(c1, LOW);
  digitalWrite(c2, LOW);

  bmp.begin();
  bmp.setSampling(Adafruit_BMP280::MODE_NORMAL, Adafruit_BMP280::SAMPLING_X2, Adafruit_BMP280::SAMPLING_X16, Adafruit_BMP280::FILTER_X16, 1);

  Wire.begin();
  mpu6050.begin();

  if (!SD.begin(10)) {
    while (!SD.begin(10)) {
      tone(buzzer, 600);
      delay(200);
      noTone(buzzer);
      delay(200);
      tone(buzzer, 450);
      delay(200);
      noTone(buzzer);
    }
  }

  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Arduino Reset");
  logs.close();
  calibrate();
}

void calibrate() {
  while (digitalRead(button) == LOW) {}

  tone(buzzer, 261);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 329);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 392);
  delay(200);
  noTone(buzzer);
  delay(2000);

  mpu6050.calcGyroOffsets(true);
  initial_altitude = bmp.readAltitude(1013.25);

  tone(buzzer, 659);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 784);
  delay(200);
  noTone(buzzer);
  tone(buzzer, 1047);
  delay(200);
  noTone(buzzer);
}

void beep(int frequency, int duration) {
  tone(buzzer, frequency);
  delay(duration);
  noTone(buzzer);
}

bool handleButtonPresses() {
  bool buttonPressed = false;
  unsigned long lastPressTime = 0;
  unsigned long debounceDelay = 20;
  unsigned long cooldownDuration = 60;
  unsigned int buttonCounter = 0;
  unsigned long startTime = millis();

  while (millis() - startTime < 2000) {
    int buttonState = digitalRead(button);

    if (buttonState == HIGH && millis() - lastPressTime >= debounceDelay && !buttonPressed) {
      buttonPressed = true;
      beep(500, 100);
      lastPressTime = millis();
      buttonCounter++;
      delay(cooldownDuration);
    }

    if (buttonState == LOW && buttonPressed) {
      buttonPressed = false;
    }

    if (buttonCounter == 5 && (millis() - lastPressTime) < 2500) {
      buttonCounter = 0;
      return true;
    }
  }

  buttonCounter = 0;
  return false;
}

bool countdown() {
  unsigned long startTime = millis();
  unsigned long beepInterval = 900;
  unsigned long countdownDuration = 31800;
  unsigned long lastBeepTime = millis();
  fadeBuzzer(200, 2500, 2000, true);
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Countdown Started");
  logs.close();
  failable = true;
  while (millis() - startTime < countdownDuration) {
    if ((digitalRead(button) == HIGH || apogee == true)) {
      fadeBuzzer(200, 2500, 2000, false);
      logs = SD.open("logs.txt", FILE_WRITE);
      logs.println("Countdown Canceled");
      logs.close();
      apogee = false;
      return false;
    }
    sensors();
    if (millis() - lastBeepTime >= beepInterval) {
      if (millis() - startTime >= countdownDuration - 6000) {
        tone(buzzer, 2500);
        if ((digitalRead(button) == HIGH || apogee == true)) {
          fadeBuzzer(200, 2500, 2000, false);
          logs = SD.open("logs.txt", FILE_WRITE);
          logs.println("Countdown Canceled");
          logs.close();
          apogee = false;
          return false;
        }
      } else {
        beep(2500, 100);
      }
      lastBeepTime = millis();
    }
  }
  noTone(buzzer);
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Main Motor Ignition");
  logs.close();
  data = SD.open("data.txt", FILE_WRITE);
  data.println("Launch!");
  digitalWrite(c1, HIGH);
  return true;
}

void sensors() {
  mpu6050.update();
  float current_altitude = bmp.readAltitude(1013.25) - initial_altitude;
  gyro_x = mpu6050.getAngleX();
  gyro_y = mpu6050.getAngleY();
  accel_x = mpu6050.getAccX();
  accel_y = mpu6050.getAccY();
  if (log_data) {
    data.print(current_altitude);
    data.print(",");
    data.print(gyro_x);
    data.print(",");
    data.print(gyro_y);
    data.print(",");
    data.print(accel_x);
    data.print(",");
    data.print(accel_y);
    data.println();
    if ((gyro_x > 70 || gyro_x < -70 || gyro_y > 70 || gyro_y < -70) && failable) {
      apogee = true;
    }
    if (current_altitude < highest_altitude - 1) {
      apogee = true;
    }
    if (apogee) {
      return;
    }
    if (current_altitude > highest_altitude) {
      highest_altitude = current_altitude;
    }
  }
  log_data = !log_data;
}

void fadeBuzzer(int startFrequency, int endFrequency, int totalDuration, bool fadeUp) {
  int steps = abs(endFrequency - startFrequency) / 10;
  int stepDuration = totalDuration / steps;

  if (fadeUp) {
    for (int frequency = startFrequency; frequency <= endFrequency; frequency += 10) {
      beep(frequency, stepDuration);
      sensors();
    }
  } else {
    for (int frequency = endFrequency; frequency >= startFrequency; frequency -= 10) {
      beep(frequency, stepDuration);
    }
  }
}

void land() {
  digitalWrite(c1, LOW);
  digitalWrite(c2, HIGH);
  servoX.write(90 + Xtune);
  servoY.write(90 + Ytune);
  data.println("Parachute deploying");
  data.close();
  logs = SD.open("logs.txt", FILE_WRITE);
  logs.println("Parachute Motor Ignition");
  logs.close();

  unsigned long lastBeepTime = millis();
  while (true) {
    if (millis() - lastBeepTime >= 1000) {
      tone(buzzer, 2500);
      delay(500);
      noTone(buzzer);
      lastBeepTime = millis();
    }
    data = SD.open("data.txt", FILE_WRITE);
    sensors();
    data.close();
  }
}

void TVC(){
  sensors();

}




void loop() {
  land();
  delay(1000000);
  while (handleButtonPresses() == false) {}
  if (countdown()) {
    while (apogee == false) {
      TVC();
    }
    Serial.println("LANDING");
    land();
  }
}

// servoX on inside, servoY on outside of PCB