#include <Wire.h>              //importing libraries for all sensors
#include <Adafruit_BMP280.h>
#include <DFRobot_BMX160.h>
#include <SoftwareSerial.h>

SoftwareSerial XBee(34, 35);

#include <Servo.h>

#include <TimeLib.h>

#include <SD.h>

#define BMP280_ADDRESS 0x76
Adafruit_BMP280 bmp;
DFRobot_BMX160 bmx160;

//bool telemetry=0;

bool timeset = 0;

Servo s;
float d;

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 = "";
String prev_packet = "";

float calibration; //calibration for BMP altitude

int SWstate = 0;

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

int cnt = 0; // packet count

void setup() {

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

//    attachInterrupt(digitalPinToInterrupt(38), gimbal, CHANGE);

  Serial.begin(9600);

  XBee.begin(9600);

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

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

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

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

  //Serial.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");

  if (!status) {
//    while (1);
//    delay(10);
  }
  //
  if (!bmx160.begin()) {
    //    Serial.println("NO BMX found");
//    while (1);
  }

  SD.begin(BUILTIN_SDCARD);

  for (int j = 0; j < 10; j++) { //calibration by taking mean of 10 BMP pressure readings
    calibration += bmp.readPressure();
    delay(50);
  }
  calibration /= 1000;
  //  calibration/=10;

  //  calibration=bmp.readPressure();

  //  calibration=bmp.readPressure();
  //  calibration/=100;

  s.attach(25); //servo pwm pin
  // s_Roll.attach(2); //gimbals
  // s_Pitch.attach(4);

  pinMode(buzzer, OUTPUT);

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

  if (Kalpana_Test && logged == 0) {
    //    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");
    logged == 1;
  }
  Kalpana_Test.close();

  //  delay(500);
  Serial.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");
}

// with GCS commands
//void loop() {
//  if (XBee.available()) {
//    while (XBee.available()) {
//     char command = XBee.read();
//  switch (command)
//    {
//      case 'x': //telemetry on/off
//           telemetry=telemetry^1;
//           setTime(0, 0, 0, 24, 5, 2023);
//           start_BLDC=millis();
//           while (telemetry==1){
//            data_packet();
//            char command = XBee.read();
//            switch(command);
//           }
//           break;
//
//      case 't': //set time
//            String sentence = mySerial.readStringUntil('\n');
//
//            while (!sentence.startsWith("$GNGGA")) {
//            sentence = "";
//            sentence += mySerial.readStringUntil('\n');
//            }
//
//            String parts[15];
//            int i = 0;
//            char * pch = strtok (sentence.c_str(), ",");
//            while (pch != NULL) {
//            parts[i++] = pch;
//            pch = strtok (NULL, ",");
//            }
//
//            String time = parts[1];
//
//            IST(time);
//            break;
//
//      case 'c':  //calibrate
//            for (int j = 0; j < 10; j++) { //calibration by taking mean of 10 BMP pressure readings
//            calibration += bmp.readPressure();
//            delay(50);
//            }
//            calibration /= 1000;
//
//          break;
//
//      default:
//      data_packet();
//      break;
//      }
//    }
//  }
//}

void loop() { //data_packet function name with GCS commands
  //  if (SWstate!=7)

  while (SWstate != 7) { // until the CanSat makes impact, telemetry is sent
    //    BLDC.write(80);

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

    packet = "";
    cnt++;

    if (cnt == 30) { //breadboard testing camera, ON at 2 hr 3 mins ..7380
      s.write(180);
      ON_camera();
      BLDC.write(80);
    }

    if (cnt == 150) { //breadboard testing camera, OFF after 8  mins reccording..7860
      OFF_camera();
      BLDC.write(0);
      while (cnt < 210) { //bb test..buzzer should play upto 3 mins only
        cnt++;
        analogWrite(buzzer, 10000);
        delay(500);
        analogWrite(buzzer, 0);
        delay(500);
      }
    }

    packet += "<2022ASI-049";
    packet += ",";
    Timestamp();
    packet += String(cnt) + ",";
    BMP_280(); 
    Voltage();
    Quectel_L89();
//    BMX();
    SW_state();
    
    Data_Log(); // logging packet to sd card for backup
    Xbeetx();
    Serial.println(packet); // printing packet to serial monitor
    //    delay(1000);
  }

  //  else{ //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);
  //      }
  //    }

}