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

#include <Servo.h>

#include <TimeLib.h>

#include <SD.h>

#define BMP280_ADDRESS 0x76
Adafruit_BMP280 bmp; 
DFRobot_BMX160 bmx160;
Servo s;

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=4; //mini spy cam
bool vid=0;

const int buzzer=9;

File Kalpana_Test;

String packet="";

float calibration; //calibration for BMP altitude

int SWstate=0; 

SoftwareSerial mySerial(7, 8); //GNSS Tx and Rx, mySerial is GNSS object

int cnt=0; // packet count

void setup() {
  Serial.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(28); //servo pwm pin

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

  if (Kalpana_Test){
  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");
  }
  Kalpana_Test.close();

  setTime(0, 0, 0, 24, 5, 2023);
  
//  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");
}

void loop() { 
  if (SWstate!=7){ // until the CanSat makes impact, telemetry is sent
  packet="";
  cnt++;
  packet+="<2022AS1-049";
  packet+=",";
  Timestamp();
  packet+=String(cnt)+",";
  BMP_280(); 
  Voltage();
      
  Quectel_L89();

  BMX();
  SW_state();

  Data_Log(); // logging packet to sd card for backup
  Serial.println(packet); // printing packet to serial monitor
  }

  else{ //SW state=7 ; impact made - telemetry stoppped and buzzer played intermittently at 1s delay
  tone(buzzer,3000);
  delay(1000);
  noTone(buzzer);
  delay(1000);
  }
  delay(700);

}