#include <Wire.h>
#include "TinyGPS_2.h"
TinyGPS gps;
 
#define ASCII_BIT 8
#define BAUD_RATE 20000     // 10000 = 100 BAUD 20150
#define SERIAL_SPEED 4800
 
int count = 0, nogps_count = 0;
float flat, flon, falt;
unsigned long fix_age, date, time, chars, age;
unsigned short sentences, failed_checksum, failed;
 
int hour, minute, second;
char latbuf[12], lonbuf[12], ascentratebuf[12], ialtbuf[10];
int lat_deg, lat_min, cutdown = 0;
long int  ialt = 123;
int numbersats;
 
//Ascent rate variables
int last_minute, last_second, last_alt;
float ascentrate, diff_second, diff_alt;
int atfloat = 10, float_time, nofloat = 0;
long floatstart;
 
//Tempsensor variables
int temp0 = 0, temp1 = 0, temp2 = 0, temp3 = 0;
int light1 = 0;
 
//Ballast variables
int totalPumpseconds, blevelperc, ballastmode = 0, heater_status;
 
char superbuffer [100];
char checksum [5];
int n, val = 1;
// ------------------------
// RTTY Functions - from RJHARRISON's AVR Code
void rtty_txstring (char * string)
{
 
	/* Simple function to sent a char at a time to 
	** rtty_txbyte function. 
	** NB Each char is one byte (8 Bits)
	*/
 
	char c;
	c = *string++;
 
	while ( c != '\0')
	{
		rtty_txbyte (c);
		c = *string++;
	}
}
 
 
void rtty_txbyte (char c)
{
	/* Simple function to sent each bit of a char to 
	** rtty_txbit function. 
	** NB The bits are sent Least Significant Bit first
	**
	** All chars should be preceded with a 0 and 
	** proceded with a 1. 0 = Start bit; 1 = Stop bit
	**
	** ASCII_BIT = 7 or 8 for ASCII-7 / ASCII-8
	*/
 
	int i;
 
	rtty_txbit (0); // Start bit
 
	// Send bits for for char LSB first	
 
	for (i=0;i<ASCII_BIT;i++)
	{
		if (c & 1) rtty_txbit(1); 
 
			else rtty_txbit(0);	
 
		c = c >> 1;
 
	}
 
	rtty_txbit (1); // Stop bit
}
 
void rtty_txbit (int bit)
{
		if (bit)
		{
		  // high
                    digitalWrite(4, HIGH);
                    digitalWrite(3, LOW);
                    digitalWrite(13, LOW); //LED
 
		}
		else
		{
		  // low
                    digitalWrite(3, HIGH);
                    digitalWrite(4, LOW);
                    digitalWrite(13, HIGH); //LED
		}
		delayMicroseconds(BAUD_RATE); 
}
 
char * floatToString(char * outstr, double val, byte precision, byte widthp){
  char temp[16];
  byte i;
 
  // compute the rounding factor and fractional multiplier
  double roundingFactor = 0.5;
  unsigned long mult = 1;
  for (i = 0; i < precision; i++)
  {
    roundingFactor /= 10.0;
    mult *= 10;
  }
 
  temp[0]='\0';
  outstr[0]='\0';
 
  if(val < 0.0){
    strcpy(outstr,"-\0");
    val = -val;
  }
 
  val += roundingFactor;
 
  strcat(outstr, itoa(int(val),temp,10));  //prints the int part
  if( precision > 0) {
    strcat(outstr, ".\0"); // print the decimal point
    unsigned long frac;
    unsigned long mult = 1;
    byte padding = precision -1;
    while(precision--)
      mult *=10;
 
    if(val >= 0)
      frac = (val - int(val)) * mult;
    else
      frac = (int(val)- val ) * mult;
    unsigned long frac1 = frac;
 
    while(frac1 /= 10)
      padding--;
 
    while(padding--)
      strcat(outstr,"0\0");
 
    strcat(outstr,itoa(frac,temp,10));
  }
 
  // generate space padding
  if ((widthp != 0)&&(widthp >= strlen(outstr))){
    byte J=0;
    J = widthp - strlen(outstr);
 
    for (i=0; i< J; i++) {
      temp[i] = ' ';
    }
 
    temp[i++] = '\0';
    strcat(temp,outstr);
    strcpy(outstr,temp);
  }
 
  return outstr;
} 
 
void sensors()
{
  val = digitalRead(15);
  Serial.println(val);
 
  if(val == 1) {
    //Send altitude
    Serial.println("Starting i2c Trans");
    int p = 0;
    Wire.beginTransmission(2); // transmit to device #2
    Wire.send(atfloat);              // sends floatdata  
    Wire.endTransmission();    // stop transmitting 
    Serial.println("Sent altitude");
    delay(1000);
    for(p=1; p <=9; p++)
    {
      Wire.beginTransmission(2); // transmit to device #2
      Wire.send(p);              // sends 0 to tell slave that you are about to send altitude  
      Wire.endTransmission();    // stop transmitting 
      Serial.print(p);
      Serial.print(" - ");
      delay(100); 
      Wire.requestFrom(2, 1);    // request data from slave device #2
      while(Wire.available())
      {
        switch(p) {
          case 1:
            temp0 = Wire.receive(); // receive a byte as character
            Serial.println(temp0);
            break;
          case 2:
            temp1 = Wire.receive(); // receive a byte as character
            Serial.println(temp1);
            break;
          case 3:
            temp2 = Wire.receive(); // receive a byte as character
            Serial.println(temp2);
            break;
          case 4:
            temp3 = Wire.receive(); // receive a byte as character
            Serial.println(temp3);
            break;
          case 5:
            totalPumpseconds = Wire.receive(); // receive a byte as character
            Serial.println(totalPumpseconds);
            break;
         case 6:
            blevelperc = Wire.receive(); // receive a byte as character
            Serial.println(blevelperc);
            break;
         case 7:
            ballastmode = Wire.receive(); // receive a byte as character
            Serial.println(ballastmode);
            break;
         case 8:
            heater_status = Wire.receive(); // receive a byte as character
            Serial.println(heater_status);
            break;
         case 9:
            light1 = Wire.receive(); // receive a byte as character
            Serial.println(light1);
            break;
        }
      }
      delay(100);
    }
    if (temp0 > 100) {
      temp0 = temp0 - 256;
    }
    if (temp1 > 100) {
      temp1 = temp1 - 256;
    }
    if (temp2 > 100) {
      temp2 = temp2 - 256;
    }
    if (temp3 > 100) {
      temp3 = temp3 - 256;
    }
  }
}
 
unsigned int gps_checksum (char * string)
{	
	unsigned int i;
	unsigned int XOR;
	unsigned int c;
	// Calculate checksum ignoring any $'s in the string
	for (XOR = 0, i = 0; i < strlen(string); i++)
	{
		c = (unsigned char)string[i];
		if (c != '$') XOR ^= c;
	}
	return XOR;
}
 
void setup()
{
  pinMode(2, OUTPUT); //EN
  pinMode(3, OUTPUT); //Tx
  pinMode(4, OUTPUT); //Tx
  pinMode(13, OUTPUT); //LED
  pinMode(15, INPUT); //Secondary Sense Pin
  digitalWrite(15, HIGH);
  Serial.begin(SERIAL_SPEED);
  digitalWrite(13, HIGH);
  digitalWrite(2, HIGH);
  delay(1000);
  Serial.println("Starting Up...");
  //Turning off all GPS NMEA strings apart from GPGGA
  //Need to decide what is appropriate for lassen
  digitalWrite(13, LOW);
  Wire.begin(); // join i2c bus (address optional for master)
}
 
void loop()
{
  while (Serial.available())
  {
    int c = Serial.read();
    if (gps.encode(c))
    {
      // retrieves +/- lat/long in 100000ths of a degree
      if (fix_age == TinyGPS::GPS_INVALID_AGE) {
          //Serial.println("No fix detected");
      }
      else {
        //Get Data from GPS library
        //Get Time and split it
        gps.get_datetime(&date, &time, &age);
        hour = (time / 1000000);
        minute = ((time - (hour * 1000000)) / 10000);
        second = ((time - ((hour * 1000000) + (minute * 10000))));
        second = second / 100;
        //Get Position
        gps.f_get_position(&flat, &flon);
        falt = gps.f_altitude(); // +/- altitude in meters
        ialt = long(falt);
 
 
        //Ascent rate calculation
        if (minute > (last_minute)) {
          //Calculate ascent
          diff_second = ((60 - last_second) + second + (((minute - last_minute) * 60) - 60));
          diff_alt = ialt - last_alt;
          ascentrate = diff_alt / diff_second;
          //Convert to string
          floatToString(ascentratebuf, ascentrate, 1, 0);
          //Reset calculator
          if(minute == 59) {
            last_minute = -1;
          }
          else {
            last_minute = minute;
          }
          last_second = second;
          last_alt = ialt;
        }
        /*else {
          //Reset calculator
          last_minute = minute;
          last_second = second;
          last_alt = ialt;
        }*/
        //Calculate if we have achieved float, monitor ascentrate if between +1 and -1 start float time if it strays out of +1 or -1 for more then 5 cycles - float has stopped
        if (ascentrate < 1 && ascentrate > -1 && ialt > 15000 && ballastmode == 0) {
          if (atfloat == 10){
            //Start float timer
            floatstart = minute;
          }
          float_time = (minute - floatstart);
          if (float_time<0) {
            Serial.print("Negative ");
             float_time = float_time + 60;
          } 
          if(float_time > 20) {
              ballastmode = 1;
              atfloat = 12;
          }
          //Floating
          else {
            atfloat = 11;
          }  
          Serial.print("Float data = ");
          Serial.print(float_time);
          Serial.print(" ");
          Serial.println(atfloat);
        }
        else {
          //Not floating
          nofloat++;
          if (nofloat >= 5) {
            atfloat = 10;
            nofloat = 0;
          }
        }
        //CUTDOWN Code goes here
 
        floatToString(latbuf, flat, 4, 0);
        floatToString(lonbuf, flon, 4, 0);
 
        numbersats = gps.num_sats();
 
        n=sprintf (superbuffer, "$$ATLAS,%d,%d:%d:%d,%s,%s,%ld,%d,%s;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d", count, hour, minute, second, latbuf, lonbuf, ialt, numbersats, ascentratebuf, atfloat,float_time,temp0,temp1,temp2,temp3,light1,totalPumpseconds,blevelperc,ballastmode,heater_status);
        if (n > -1){
          n = sprintf (checksum, "*%02X\n",gps_checksum(superbuffer));
          n = sprintf (superbuffer, "%s%s", superbuffer, checksum);
          rtty_txstring(superbuffer);
          Serial.println(superbuffer);
        }
        count++;
        delay(100);
        sensors();
        delay(100);
      }
    }
  }
  //No GPS Input
  nogps_count++;
  delay(10);
  if (nogps_count > 1000) {
      sensors();
      numbersats = gps.num_sats();
    n=sprintf (superbuffer, "$$ATLAS/No GPS Data/%d:%d:%d/%s/%s/%ld/%d/%d/%d/%d/%d/%d/%d/%d/%d/%d", hour, minute, second, latbuf, lonbuf, ialt, numbersats, temp0, temp1,temp2,temp3, light1, totalPumpseconds, blevelperc, ballastmode ,heater_status);
    if (n > -1){
          n = sprintf (checksum, "*%02X\n",gps_checksum(superbuffer));
          n = sprintf (superbuffer, "%s%s", superbuffer, checksum);
          rtty_txstring(superbuffer);
          Serial.println(superbuffer);
    }
    nogps_count = 0;
  }
}