#include "Wire.h"
#include "SD.h"
#include "Adafruit_BMP085.h"
#include "leonardo_GPS.h"
#include "ServoTimer2.h"
#include "VirtualWire.h"

ServoTimer2 chute;

//sdcard
const int SDSelect = 8;

//GPS shit
leonardo_GPS GPS(&Serial1);
#define GPSECHO  true
boolean usingInterrupt = false;
void useInterrupt(boolean);
int locked;
String lat, lon;

//mostly Altimeter shit
Adafruit_BMP085 bmp;
int ground;
int alti, altf, apogee;
int ti, tf;
int vinit, velocity, vmax;
boolean dep = false;

//shit for the radio
char temp[20];


void setup() {
  vw_set_tx_pin(5);
  vw_setup(1200);    // Bits per sec for radio
  chute.attach(9);
  chute.write(0);
  chute.detach();
  //init sd
  pinMode(SDSelect, OUTPUT);
  pinMode(10, OUTPUT);
  SD.begin(SDSelect);

  //init radio
  pinMode(4,OUTPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,OUTPUT);

  bmp.begin();
  ground=3.28084*bmp.readAltitude(); //get ground level altitude. We do this so we don't have to actually set the altimeter seperately with SLP. calculated at a temperature of 15c

  //set up the GPS
  GPS.begin(9600);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
  GPS.sendCommand(PGCMD_ANTENNA);
  useInterrupt(true);
  delay(1000);
  Serial1.println(PMTK_Q_RELEASE);
  tx_data("BOOT");
}

// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c;
    // writing direct to UDR0 is much much faster than Serial.print
    // but only one character can be written at a time.
#endif
}

void useInterrupt(boolean v) {
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
}
uint32_t timer = millis();
int time = millis();

void loop() {
  String transmit = "";
  String tempstr = "";

 if (! usingInterrupt) {
    // read data from the GPS in the 'main loop'
    char c = GPS.read(); // if you want to debug, this is a good time to do it!
    if (GPSECHO);
  }

  // if a sentence is received, we can check the checksum, parse it...
  if (GPS.newNMEAreceived()) {
    // a tricky thing here is if we print the NMEA sentence, or data
    // we end up not listening and catching other sentences!
    // so be very wary if using OUTPUT_ALLDATA and trytng to print out data
    //Serial.println(GPS.lastNMEA());   // this also sets the newNMEAreceived() flag to false

    if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
      return;  // we can fail to parse a sentence in which case we should just wait for another
  }
  // if millis() or timer wraps around, we'll just reset it
  if (timer > millis())  timer = millis();

  // approximately every 2 seconds or so, print out the current stats
  if (millis() - timer > 2000) {
    timer = millis(); // reset the timer

    if (GPS.fix) {
      digitalWrite(13, LOW);
      lat = GPS.latitude;
      lon = GPS.longitude;
    }
    else {
      digitalWrite(13, HIGH);
      tx_data("SIGNAL LOST");
    }
  }
  tf = ti; //needed for velocity calculation
  ti = time; //get time since boot from the timer - real time is not needed since we are only
             //interested in current flight
    // Calculate altitude assuming 'standard' barometric
    // pressure of 1013.25 millibar = 101325 Pascal
    //calculate altitude AGL, in feet to increase resolution, so we can safely ignore the decimals of
    //a meter and still have accurate enough altitude data without using a floating point number.
    alti = altf; //make the initial alt equal to the alt last recorded, so we store it for comparison
    altf = 3.28084*bmp.readAltitude(); //convert alt to ft
    altf = altf - ground; //get alt AGL

    vinit = velocity; //store previous velocity for logging
    velocity = (altf - alti)/(tf - ti);

    if (altf > alti) {
      apogee = altf;
    }
    if (altf < alti) {
       if (apogee > 100) {
         transmit = "Apogee at ";
         transmit += String(apogee);
         transmit += "M";
         tx_data(transmit);
         if (altf <= 150) {
           if (dep = false) {
             chute.attach(9);
             chute.write(180);
             chute.detach();
             dep = true;
             transmit = "Seconary Deployment at "; transmit += String(altf); transmit += "ft, "; transmit += lat; transmit += ", "; transmit += lon;
             tx_data(transmit);
           }
           if (altf < 50){
             transmit = String(altf); transmit += "ft, "; transmit += lat; transmit += ", "; transmit += lon;
             tx_data(transmit);
           }
           if (altf <= 10) {
             tx_data("ALIVE");
           }
         }
       }
    }

    if (velocity > vinit) {
      vmax = velocity;
    }
    if (vmax > velocity) {
       if (apogee > 20) {
         transmit = "Max Velocity: ";
         transmit += String(vmax);
         transmit += "ft/s";
         tx_data(transmit);
       }
    }
    tempstr = String(time); tempstr += ","; tempstr += String(velocity); tempstr += ","; ; tempstr += String(altf); tempstr += ","; tempstr += lat; tempstr += ","; tempstr += lon;
    File logFile = SD.open("flightlog.csv", FILE_WRITE);
    logFile.println(tempstr);
    logFile.close();
}

//needed for radio

void tx_data(String tx_d)
{
  byte packet;
  int length = tx_d.length()+1;
  char buf[length];
  tx_d.toCharArray(buf, length);
  vw_send((uint8_t *)buf, length);
  vw_wait_tx(); // Wait until the whole message is gone
}