#include <LibAPRS.h>

#include <LM75A.h>
// Create I2C LM75A instance
LM75A lm75a_sensor(false,  //A0 LM75A pin state
                   false,  //A1 LM75A pin state
                   false); //A2 LM75A pin state
// Equivalent to "LM75A lm75a_sensor;"

#include <Wire.h>
#include <MS5611.h>
MS5611 ms5611;
double referencePressure;

#include "DHT.h"
#define DHTPIN 30     // what digital pin we're connected to
#define DHTTYPE DHT22   // DHT 22  (AM2302), AM2321
DHT dht(DHTPIN, DHTTYPE);

char byteString[80]; //Char Array holding up to 80 bytes filled with the $GNGGA NMEA sentence

char timeUTC[10]; //array holding time
char latitudeGPS[12]; // array holding latitude decimal form as chars
char dirLat[2]; //array holding latitude hemisphere
char longitudeGPS[12]; // array holding longitude decimal form as chars
char dirLon[2];  //array holding longitude hemesphere

char fixNum[2]; //array holding fix quality 0-8
char satNum[3]; // array holding total number of tracked satellites
char dilution[4];  //array holding horizontal dilution of position
char altMeters[10]; //array holding  Altitude, Meters, above mean sea level

float altFeet = 0;
char temp2[6];
char baroP[4];

double realTemperature = 0;
long realPressure =  0;

char intT1Char[8];
char intT2Char[8];
char intT3Char[8];
char gpsAltFeetChar[11];
char barChar[11];
char humChar[3];

char stringOut[64]; //libAPRS max 64 char

float dhtH = 0; // Read humidity in %
float dhtT = 0; // Read temperature as Celsius (the default)
float dhtF = 0;// Read temperature as Fahrenheit (isFahrenheit = true)

float temperature_in_degreesC = 0;
float temperature_in_degreesF = 0;
float absoluteAltitude = 0;
float relativeAltitude = 0;

int battV = 0;

char combLat[15]; //array holding the final input into LibAPRS's locationUpdate() func - latitude
char combLon[15];//array holding the final input into LibAPRS's locationUpdate() func - longitude

boolean gotGGA = false; //make our if statement possible I think this can be simplified TO DO

long previousMillis = 0;        //timer define counter

long rfInterval = 90000; // every 30 seconds approx SEND OUT RF PACKET locationUpdate()

#define ADC_REFERENCE REF_5V // OR #define ADC_REFERENCE REF_3V3

#define OPEN_SQUELCH false //// You can also define whether your modem will be running with an open squelch radio:

//boolean gotPacket = false; //Get rid of soon.
//uint8_t *packetData; //make sure we dont need em.

void aprs_msg_callback(struct AX25Msg *msg) {
}

void setup() {
  Serial.begin(115200);
  Serial1.begin(9600);
  Serial3.begin(9600);

  dht.begin();
  ms5611.begin();

  // Get reference pressure for relative altitude
  referencePressure = ms5611.readPressure();

  APRS_init(ADC_REFERENCE, OPEN_SQUELCH);  // Initialise APRS library - This starts the modem

  APRS_setCallsign("KM4PGH", 9);  //   You must at a minimum configure your callsign and SSID

  APRS_setDestination("BEACON", 0);  // You don't need to set the destination identifier, but can.

  APRS_setPath1("WIDE1", 1);   // Path parameters are set to sensible values by default, but this is how you can configure them:

  APRS_setPath2("WIDE2", 2);

  APRS_setPreamble(150);    // You can define preamble and tail like this:
  APRS_setTail(50);   // You can define preamble and tail like this:

  // APRS_useAlternateSymbolTable(false);  // You can use the normal or alternate symbol table:

  APRS_setSymbol('O');    // And set what symbol you want to use:

  APRS_printSettings();   // We can print out all the settings
  Serial.print(F("Free RAM:     ")); Serial.println(freeMemory()); //debug memory problems
}

void getGPS() {
  while (Serial1.available()) {

    Serial1.find("$GNGGA"); //reads Serial1 untill we find $GNGGA
    if (gotGGA = true) {  //set Flag if so gotGGA
      Serial.println("we recieved $GNGGA !"); //Debug to serial mon
      Serial1.readBytesUntil('\n', byteString, 80); //lost word $GNGGA during the find, but store the rest into char array byteString 80 bytes (76 needed?)
    }
    else {
      Serial.println("we did NOT get $GNGGA!");
    }
  }
}

void parseGPS() {

  // split the data into its parts with strtok()

  char * strtokIndx; // this is used by strtok() as an index

  strtokIndx = strtok(byteString, ",");     // get the first part - the string - TIME
  strcpy(timeUTC, strtokIndx); // copy it to timeUTC

  strtokIndx = strtok(NULL, ","); //get next word of NMEA sentence until next delimiter, "," comma
  memcpy(latitudeGPS, strtokIndx, 7); //fill our array with only part of the NMEA word because LibAPRS requires it

  strtokIndx = strtok(NULL, ","); //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(dirLat, strtokIndx); // fill our array with the NMEA word for the hemisphere indicator N/S

  strtokIndx = strtok(NULL, ","); //get next word of NMEA sentence until next delimiter, "," comma
  memcpy(longitudeGPS, strtokIndx, 8); //fill our array with only part of the NMEA word because LibAPRS requires it

  strtokIndx = strtok(NULL, ","); //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(dirLon, strtokIndx);  //fill our array with the NMEA word for the hemisphere indicator E/W

  strcat(combLat, latitudeGPS); //combine trunicated latitude to the hemisphere indicator N/S
  strcat(combLat, dirLat);

  strcat(combLon, longitudeGPS); //combine trunicated longitude to the hemisphere indicator E/W
  strcat(combLon, dirLon);

  strtokIndx = strtok(NULL, ",");  //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(fixNum, strtokIndx); // fill our arrary with NMEA word for Satellite Fix quaity.

  //0 = invalid (nofix) 1 = GPS fix (SPS) 2 = DGPS fix 3 = PPS fix 4 = Real Time Kinematic
  //5 = Float RTK 6 = estimated (dead reckoning) (2.3 feature) 7 = Manual input mode 8 = Simulation mode

  strtokIndx = strtok(NULL, ",");  //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(satNum, strtokIndx); // fill our arrary with NMEA word for Number of tracked satellites

  strtokIndx = strtok(NULL, ",");  //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(dilution, strtokIndx); // fill our arrary with NMEA word for  Horizontal dilution of position

  strtokIndx = strtok(NULL, ",");  //get next word of NMEA sentence until next delimiter, "," comma
  strcpy(altMeters, strtokIndx); // fill our arrary with NMEA word for Altitude, Meters, above mean sea level

  altFeet = atof (altMeters) * 3.2808;
}

void getBaro() {
  // Read raw values
  uint32_t rawTemp = ms5611.readRawTemperature();
  uint32_t rawPressure = ms5611.readRawPressure();

  // Read true temperature & Pressure
  realTemperature = ms5611.readTemperature();
  realPressure =  ms5611.readPressure()  ;

  // Calculate altitude
  absoluteAltitude = ms5611.getAltitude(realPressure);
  relativeAltitude = ms5611.getAltitude(realPressure, referencePressure);

}

void getDHT22() {
  // Reading temperature or humidity takes about 250 milliseconds!
  // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
  dhtH = dht.readHumidity();
  // Read temperature as Celsius (the default)
  dhtT = dht.readTemperature();
  // Read temperature as Fahrenheit (isFahrenheit = true)
  dhtF = dht.readTemperature(true);

  // Check if any reads failed and exit early (to try again).
  if (isnan(dhtH) || isnan(dhtT) || isnan(dhtF)) {
    Serial.println("Failed to read from DHT sensor!");
    return;
  }
}

void getLM75A() {
  temperature_in_degreesC = lm75a_sensor.getTemperatureInDegrees();
  temperature_in_degreesF = (LM75A::degreesToFahrenheit(temperature_in_degreesC ));

  if (temperature_in_degreesC == INVALID_LM75A_TEMPERATURE) {
    Serial.println("Error while getting temp LM75A");
  } else {
  }
}

void battMon() {
  // read the input on analog pin 0:
  battV = analogRead(A1);
}

void buildComment() {

  strcpy(stringOut, "{ ALT");
  dtostrf(altFeet, 7, 0, gpsAltFeetChar);
  strcat(stringOut, gpsAltFeetChar);
  strcat(stringOut, "ft");

  strcat(stringOut, " SAT");
  strcat(stringOut, satNum);

  //strcat(stringOut, " FIX");
  //strcat(stringOut, fixNum);

  strcat(stringOut, " B ");
  ltoa(realPressure, barChar, 10);
  strcat(stringOut, barChar);
  //strcat(stringOut, "P");

  strcat(stringOut, " H");
  dtostrf(dhtH, 2, 1, humChar);
  strcat(stringOut, humChar);
  strcat(stringOut, "%");

  strcat(stringOut, " T1");
  dtostrf((realTemperature * 1.8) + 31, 5, 1, intT1Char);
  strcat(stringOut, intT1Char);
  strcat(stringOut, "F");

  strcat(stringOut, " T2");
  dtostrf(dhtF, 5, 1, intT2Char);

  strcat(stringOut, intT2Char);
  strcat(stringOut, "F");

  strcat(stringOut, " T3");
  dtostrf(temperature_in_degreesF, 5, 1, intT3Char);
  strcat(stringOut, intT3Char);
  strcat(stringOut, "F");

  //Serial.println(stringOut);
}

void serialOut() {
  if (strcmp(fixNum, "1") == 0) { //Check location FIX status if no fix report it

    Serial.println();
    Serial.print("LAT : ");
    Serial.println(combLat); // filled with formatted strcat() array for LibAPRS locationUpdate - latitude
    Serial.print("LON : ");
    Serial.println(combLon); // filled with formatted strcat() array for LibAPRS locationUpdate - longitude
    Serial.print("TIME : ");
    Serial.println(timeUTC); // time is in raw format. UTC HHMMSS.ss format TBD per APRS packet format

    Serial.print("FIX : ");
    Serial.println(fixNum); // filled with formatted strcat() array for satellite fix quality
    Serial.print("SATS : ");
    Serial.println(satNum); // filled with formatted strcat() array for Number of satellites tracked
    Serial.print("ALT : ");
    Serial.print(altMeters); // filled with formatted strcat() array for Altitude, Meters, above mean sea level
    Serial.print(" meters ");
    Serial.print(altFeet, 1); //filled with altFeet = atof (altMeters) * 3.2808;  in parse function
    Serial.println(" feet");
  }

  else {
    Serial.println("NO GPS FIX!"); //Report lost GPS fix. keep polling tho
  }

  //APRS_printSettings();   // We can print out all the settings
  Serial.print(F("Free RAM:     ")); Serial.println(freeMemory()); //debug memory problems

  Serial.print("EXT-TEMP1: ");
  Serial.print(temperature_in_degreesC);
  Serial.print("*C ");
  Serial.print(LM75A::degreesToFahrenheit(temperature_in_degreesC ));
  Serial.println("*F");

  Serial.print("HUM: ");
  Serial.print(dhtH);
  //Serial.print("%\t");
  Serial.print("%");
  Serial.print(" EXT-TEMP2: ");
  Serial.print(dhtT);
  Serial.print("*C ");
  Serial.print(dhtF);
  Serial.println("*F\t");

  Serial.print("INT-TEMP: ");
  Serial.print(realTemperature);
  Serial.print("*C ");
  Serial.print((realTemperature * 1.8) + 31);
  Serial.println("*F");
  Serial.print("BARO: ");
  Serial.print(realPressure);
  Serial.print(" Pa ");
  Serial.print(realPressure / 6894.76);
  Serial.println(" psi");
  Serial.print("BARO-ALT: ");
  Serial.print(absoluteAltitude);
  Serial.print(" m ");
  Serial.print(absoluteAltitude * 3.2808);
  Serial.println(" ft");

  Serial.print("A1: ");
  Serial.print(battV);
  Serial.print(" Volts: ");
  Serial.println(battV / 113.2);

  Serial.println(stringOut);
  Serial.println();
}

void logData() {

    Serial.print(F("Free RAM:     ")); Serial.println(freeMemory()); //debug memory problems

  //Out Data to send out
  Serial3.print("DATA:");
  Serial3.println(stringOut);

  //MS5611
  Serial3.print("MS5611: ");
  Serial3.print(referencePressure);
  Serial3.print(" Pa Start");
  Serial3.print(realTemperature);
  Serial3.print("*C ");
  Serial3.print(realPressure);
  Serial3.print(" Pa ");
  Serial3.print(realPressure / 6894.76);
  Serial3.print(" psi");
  Serial3.print(absoluteAltitude);
  Serial3.print(" m ");
  Serial3.print(absoluteAltitude * 3.2808);
  Serial3.println(" ft");

  //DHT22
  Serial3.print("DHT: ");
  Serial3.print(dhtH);
  // Serial3.print("%\t ");
  Serial3.print("%");
  Serial3.print(dhtT);
  Serial3.print("*C ");
  Serial3.print(dhtF);
  Serial3.println("*F\t");

  //LM75A
  Serial3.print(temperature_in_degreesC);
  Serial3.print("*C ");
  Serial3.print(temperature_in_degreesF);
  //Serial.print(LM75A::degreesToFahrenheit(temperature_in_degrees));
  Serial3.println("*F");

  //BATT
  Serial3.print("A1: ");
  Serial3.print(battV);
  Serial3.print(" Volts ");
  Serial3.println(battV / 113.2);
}

void locationUpdate() {
  // Let's first set our latitude and longtitude.
  // These should be in NMEA format!
  APRS_setLat(combLat);   //APRS_setLat("5530.80N");
  APRS_setLon(combLon);    //APRS_setLon("01143.89E");

  char *comment = stringOut;  // We'll define a comment string

  APRS_sendLoc(comment, strlen(comment)); // And send the update

}

//MAIN LOOP
void loop() {

  getGPS(); // Poll GPS data for our string to parse out

  Serial.print("$GNGGA");
  Serial.println(byteString); //print NMEA Sentence MUST BE BEFORE PARSE strtok() breaks string after DEBUG
  Serial3.print("$GNGGA");
  Serial3.println(byteString); //print NMEA Sentence MUST BE BEFORE PARSE strtok() breaks string after DEBUG

  parseGPS(); // Break up our GPS data in this function.

  getBaro();
  getLM75A();
  getDHT22();
  battMon();

  serialOut(); // Send data out on Serial1 for debug
  logData(); //Print out data on Serial3 for SD card

  buildComment();

  unsigned long currentMillis = millis(); //timer define counter
  if (currentMillis - previousMillis > rfInterval) {
    // save the last time you sent RF out
    previousMillis = currentMillis;
    locationUpdate();
    Serial.println("WE SENT OUT RF PACKET!!!!!");
    Serial.println();
    currentMillis = 0;
  }
  else {
    //nada
  }

  //APRS_printSettings();
  combLat[0] = '\0'; //MAKE LAST THING TO DO BEFOR DELAY else get nada! makes sure LibAPRS gets fresh data             CHANGED THIS!!!! NOT SURE IF NEEDED
  combLon[0] = '\0'; //MAKE LAST THING TO DO BEFOR DELAY else get nada! makes sure LibAPRS gets fresh data

  delay(5000); //slow loop to approx 5Hz
}