#include <Wire.h>              //importing libraries for all sensors
#include <Adafruit_BMP280.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <SoftwareSerial.h>
#include "Adafruit_SGP30.h"
#include "MegunoLink.h"
#include "Filter.h"

SoftwareSerial XBee(34, 35);

#include <Servo.h>

#include <TimeLib.h>

#include <SD.h>

#define BMP280_ADDRESS 0x76
Adafruit_BMP280 bmp;
Adafruit_MPU6050 mpu;
Adafruit_SGP30 sgp;

char command;
bool telemetry = 1;
bool simulation = 0;
String sim_pr;
bool timeset = 0;
float acc = 0;

Servo s;
float d;
int countn = 0;
Servo s_Roll;  //gimbals
Servo s_Pitch;
float dR;
float dP;

Servo BLDC;
float start_BLDC;

int pressure[3] = {0, 0, 0}; //will be used as queue to determine S/W states acc. to pressure and altitude readings each second
float altitudes[3] = {0, 0, 0}; //new value pushed each second (with each data packet)

int cam = 32; //mini spy cam
bool vid = 0;

const int buzzer = 9;

File Kalpana_Test;
bool logged = 0;

String packet = "";
char c;

float calibration = 1013.25; //calibration for BMP altitude
float sumG = 0;
float calibratedG = 9.81;

int SWstate = 0;

SoftwareSerial mySerial(16, 17); //GNSS Tx and Rx, mySerial is GNSS object

int cnt = 0; // packet count

ExponentialFilter<long> ADCFilter(5, 0);

void setup() {

  BLDC.attach(6); //ESC activation
  delay(1);
  BLDC.write(10);
  delay(5000);

  Serial.begin(9600);

  XBee.begin(9600);

  mySerial.begin(9600);
  mySerial.println("$PMTK220,1000*5F");

  pinMode(cam, OUTPUT);
  digitalWrite(cam, HIGH);

  unsigned status;
  status = bmp.begin(BMP280_ADDRESS);
  mpu.begin();
  sgp.begin();

  SD.begin(BUILTIN_SDCARD);

  s.attach(33); //servo pwm pin
  s.write(90);

  pinMode(buzzer, OUTPUT);

  Kalpana_Test = SD.open("Flight_2022ASI049.csv", FILE_WRITE);

  if (Kalpana_Test && logged == 0) {
    Kalpana_Test.println("Flight CSV for 16 APRIL, 2024");
    Kalpana_Test.println("TEAM_ID,TIME_STAMPING,PACKET_COUNT,ALTITUDE,PRESSURE,TEMP,VOLTAGE,GNSS_TIME,GNSS_LATITUDE,GNSS_LONGITUDE,GNSS_ALTITUDE,GNSS_SATS,ACC_R,ACC_P,ACC_Y,GYRO_R,GYRO_P,GYRO_Y,FLIGHT_SOFTWARE_STATE,TVOC,eCO2");
    logged = 1;
  }
  Kalpana_Test.close();
}
bool setDone = 1;

// with GCS commands

void loop() {
  if (setDone == 1) {
    packet = 'y';
    delay(5000);
    Xbeetx();
    while (!XBee.available()) {
      packet = 'y';
      Xbeetx();
      delay(1000);
    }
    setDone = 0;
  }

  if (XBee.available()) {
    command = XBee.read();
    if (command == 'x') {
      telemetry = 1;
    }
  }

  if (command == 'c') {      //calibrate
    calibration = 0.00;
    for (int j = 0; j < 10; j++) { //calibration by taking mean of 10 BMP pressure readings
      calibration += bmp.readPressure();
      delay(50);
    }

    calibration /= 1000;

    for (int j = 0; j < 50; j++) { //calibration of bmx
      sensors_event_t a, g, temp;
      mpu.getEvent(&a, &g, &temp);
      sumG += a.acceleration.y;
      delay(20);
    }

    calibratedG = sumG / 50;

    for (int j = 0; j < 30; j++) { //calibration of bmx
      sgp.IAQmeasure();
      delay(50);
    }

    while (command=='c'){
      packet='z';
      Xbeetx(); 
      delay(1000);
      if (XBee.available()){
        command=XBee.read();
      }
    }
  }

  else if (command == 'r') {
    if (simulation == 1) { // reset from simulation mode
      simulation = 0;
    }

    else if (telemetry == 1) { // reset from telemetry
      telemetry = 0;
    }

    else {
      calibration = 1013.25; //uncalibrate
      sumG = 0;
      calibratedG = 9.81;
    }
  }

  else if (command == 's') {
    setTime(0, 0, 0, 24, 5, 2023);
  }

  else if (command == 'u') { // unlock parachute door - rotate servo
    s.write(0);
  }

  else if (command == 'l') { // lock parachute door - rotate servo
    s.write(90);
  }

  else if (command == 'x') { // telemetry on/off
    start_BLDC = millis();

    while (telemetry == 1) {
      data_packet();
      delay(10);
    }
  }

  else if (command == 'e') { // sim enable
    SWstate = 1;
    simulation = 1;
  }

  else if (command == 'd' || command == 'r') { // sim disable
    simulation = 0;
  }

  else if (command == 'a') { // sim activate

    setTime(0, 0, 0, 24, 5, 2023);
    cnt = 0;

    while (simulation == 1) {

      if (XBee.available()) {
        c = XBee.read();

        if (c == 'd' || c == 'r') {
          simulation = 0;
        }

        else if (c == '<') {
          sim_pr = "";

          sim_pr = XBee.readStringUntil('>');
          packet = "";
          cnt++;

          packet += "<2022ASI-049,";
          Timestamp();
          packet += String(cnt) + ",";
          sim_bmp();
          Voltage();
          Quectel_L89();
          BMX();
          packet += "1,";
          aqi();

          Data_Log(); // logging packet to sd card for backup
          Xbeetx();
        }
      }
    }
  }
}

void data_packet() { //data_packet function name with GCS commands
  telemetry = 1;
  cnt = -2;

  while (telemetry && SWstate != 7 )
  { // until the CanSat makes impact, telemetry is sent

    if (timeset == 0) {
      setTime(0, 0, 0, 24, 5, 2023);
      timeset = 1;
    }

    packet = "";
    cnt++;

    packet += "<2022ASI-049";
    packet += ",";
    Timestamp();
    packet += String(cnt) + ",";
    BMP_280();
    Voltage();
    Quectel_L89();
    BMX();

    if (cnt < 1) {
      packet += "0,";
    }

    else {
      SW_state();
    }

    aqi();
    Data_Log(); // logging packet to sd card for backup
    Xbeetx();

    if (XBee.available()) {
      char c = XBee.read();
      if (c == 'x' || c == 'r') {
        telemetry = 0 ;
      }
      else if (c == 'u') { // unlock parachute door - rotate servo
        s.write(0);
      }

    }
    Serial.println(packet); // printing packet to serial monitor
  }

  if (SWstate == 7) { //SW state=7 ; impact made - telemetry stopped and buzzer played intermittently at 1s delay
    while (1) {
      analogWrite(buzzer, 10000);
      delay(500);
      analogWrite(buzzer, 0);
      delay(500);
      if (XBee.available()) {
        c = XBee.read();
        if (c == 'r') { //hard reset
          telemetry = 0;
          SWstate = 0;
          cnt == 0;
          break;
        }
      }
    }
  }

}