#include <NewSoftSerial.h>
#include <TinyGPS.h>

// Modem chip info
#define M0 11
#define M1 12
#define TXD 13
#define TX 15
#define BAUD_DELAY 822

byte frame_buffer[512];
int buffer_pos = 0;
unsigned short int crc = 0xFFFF;
byte num_ones = 0;

unsigned long date = 0;
unsigned long time = 0;
unsigned long gps_fix_age = 0;
long lat = 0;
long lon = 0;
float f_lat = 0.0;
float f_lon = 0.0;
byte fix = 0;

NewSoftSerial gps_serial(9, 10, true);
TinyGPS gps;

void setup() {
  // Setup the serial connection
  Serial.begin(9600);
  
  // Set up the GPS serial connection
  gps_serial.begin(4800);
  
  // Initialize pins for packet modem
  pinMode(M0, OUTPUT);
  pinMode(M1, OUTPUT);
  pinMode(TXD, OUTPUT);
  pinMode(TX, OUTPUT);
  
  digitalWrite(M0, HIGH); // Power Down Mode
  digitalWrite(M1, HIGH); // Power Down Mode
  digitalWrite(TXD, HIGH); // TX Idle (2200Hz)
  digitalWrite(TX, LOW); // Don't transmit yet
}

void loop(){
  // -- DATA UPDATE SECTION ---------------------------------------------------------
  // Get GPS Data
  unsigned long timer = millis();
  while(timer + 1500 > millis()){
    if(gps_serial.available()){
      temp1 = gps_serial.read();
      gps.encode(temp1);
      Serial.write(temp1);
    }
  }

  // Parse GPS Data
  // Updates the associated GPS variables
  gps.get_datetime(&date, &time, &gps_fix_age);
  if(gps_fix_age == TinyGPS::GPS_INVALID_AGE){
    lat = 0;
    lon = 0;
    f_lat = 0.0;
    f_lon = 0.0;
    date, time = 0;
  }else{
    gps.get_position(&lat, &lon);
    gps.f_get_position(&f_lat, &f_lon);
    gps.get_datetime(&date, &time, &gps_fix_age);
  }

  // -- APRS TRANSMISSION SECTION -------------------------------------------------
  reset_frame(); // Reset everything before we begin the frame
  push_frame_buffer_str("APRS  ", 1); // Dest Address
  push_frame_buffer(('0' + 0) << 1); // SSID = 0
  push_frame_buffer_str("K7NVH ", 1); // Source Address
  push_frame_buffer(('0' + 9) << 1); // SSID = 9
  push_frame_buffer_str("WIDE2 ", 1); // Digipeater Address
  push_frame_buffer(('0' + 1) << 1 | B00000001); // SSID = 1 then OR 1 to indicate last callsign
  push_frame_buffer(0x03); // Control Byte
  push_frame_buffer(0xF0); // Protocol Byte
  aprs_data();
  transmit_buffer();
  delay(1000);
}

// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ AX.25/APRS Functions ~~~~~~~~
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

void aprs_data(){
  push_frame_buffer('/'); // APRS position with timestamp without messaging capability
//  push_frame_buffer('!'); // APRS position without timestamp without messaging capability
  
  if((time/100) < 100000) push_frame_buffer('0');
  push_frame_buffer_num(time/100, 10); // Time
  push_frame_buffer('h');
 
  int lat_degs = (int)f_lat;
  float lat_mins = ((f_lat - (int)f_lat) * 60.0);
  if(lat_degs < 10) push_frame_buffer('0');
  push_frame_buffer_num(lat_degs, 10);
  if(lat_mins < 10) push_frame_buffer('0');
  push_frame_buffer_float(lat_mins, 2);
  push_frame_buffer('N');
  push_frame_buffer('/');

  if(f_lon < 0) f_lon = -f_lon;
  int lon_degs = (int)f_lon;
  float lon_mins = ((f_lon - (int)f_lon) * 60.0);
  if(lon_degs < 100) push_frame_buffer('0');
  if(lon_degs < 10) push_frame_buffer('0');
  push_frame_buffer_num(lon_degs, 10);
  if(lon_mins < 10) push_frame_buffer('0');
  push_frame_buffer_float(lon_mins, 2);
  push_frame_buffer('W');
  push_frame_buffer('>'); // '>' = Car icon, '-' = House icon, '0' = Balloon

  push_frame_buffer('/'); // Altitude (in comments section)
  push_frame_buffer('A');
  push_frame_buffer('=');
  long alt_feet = alt * 3.2808;
  if(alt_feet < 100000) push_frame_buffer('0');
  if(alt_feet < 10000) push_frame_buffer('0');
  if(alt_feet < 1000) push_frame_buffer('0');
  if(alt_feet < 100) push_frame_buffer('0');
  if(alt_feet < 10) push_frame_buffer('0');
  push_frame_buffer_num(alt_feet, 10);
  
  push_frame_buffer_str(" UW HIGH ALTITUDE BALLOON EXPERIMENT", 0);
}

void transmit_buffer(){
  digitalWrite(M0, HIGH); // Enter tone transmit mode
  digitalWrite(M1, LOW);
  
  delay(20);

  cli(); // Disable interrupts
  
  digitalWrite(LED_R, HIGH);
  digitalWrite(TX, HIGH); // Begin transmission
  
  // Transmit the flag byte for a time while the transmitter stabilizes
  for(byte i = 0; i < 50; i++){
    transmit_flag();
  }

  byte working = pop_frame_buffer();
  while(working != B01111110){ // Working with valid data, continue.
    transmit_byte(working);
    working = pop_frame_buffer();
  }
  
  unsigned short int final_crc = crc;
  final_crc = ~(final_crc & 0xFFFF);
  transmit_byte(final_crc & 0xFF);
  final_crc >>= 8;
  transmit_byte(final_crc & 0xFF);
  
  for(byte i = 0; i < 10; i++){
    transmit_flag();
  }
    
  digitalWrite(TX, LOW); // End transmission
  digitalWrite(LED_R, LOW);

  sei(); // Re-enable interrupts

  digitalWrite(M0, HIGH); // Enter power save mode
  digitalWrite(M1, HIGH);
}

void transmit_byte(byte input){ // Transmit byte with bit stuffing. LSB First
  for(byte i = 0; i < 8; i++){
    if((input & 0x01) == 0){ // Bit is a 0, flip output
      digitalWrite(TXD, !digitalRead(TXD));
      num_ones = 0;
      update_crc_bit(0x00);
    }else{ // Bit is a 1, don't flip, increment counter for bit stuffing
      num_ones++;
      update_crc_bit(0x01);
      if(num_ones == 5){ // We've hit enough ones in a row, bit stuff a 0 and reset the counter.
        delayMicroseconds(BAUD_DELAY);
        digitalWrite(TXD, !digitalRead(TXD));
        num_ones = 0;
      }
    }
    input = input >> 1;
    delayMicroseconds(BAUD_DELAY);
  }
}

void transmit_flag(){ // Transmit flag without bit stuffing. Flag is symmetrical, MSB/LSB doesn't matter.
  byte input = B01111110;
  for(byte i = 0; i < 8; i++){
    if((input & 0x01) == 0){ // Bit is a 0, flip output
      digitalWrite(TXD, !digitalRead(TXD));
      num_ones = 0;
    }else{ // Bit is a 1, don't flip
    
    }
    input = input >> 1;
    delayMicroseconds(BAUD_DELAY);
  }
}

void transmit_crc(byte input){ // Transmit the CRC MSB First
  for(byte i = 0; i < 8; i++){
    if((input & B10000000) == 128){ // Bit is a 1, don't flip
      num_ones++;
      if(num_ones == 5){ // Too many ones. Stuff a zero.
        delayMicroseconds(BAUD_DELAY);
        digitalWrite(TXD, !digitalRead(TXD));
        num_ones = 0;
      }
    }else{ // Bit is a 0, flip output
      digitalWrite(TXD, !digitalRead(TXD));
      num_ones = 0;
    }
    input = input << 1;
    delayMicroseconds(BAUD_DELAY);
  }
}

byte pop_frame_buffer(){ // Pop the bottom byte off the buffer for transmission
  byte output = frame_buffer[0];
  for(int i = 1; i < 512; i++){
    frame_buffer[(i - 1)] = frame_buffer[i];
    if(frame_buffer[i] == '~'){
      buffer_pos = i - 1;
      break;
    }
  }
  return output;
}

// Feed in a string of chars, and it adds them in order to the queue
void push_frame_buffer_str(const char *s, byte shift){
  if(shift == 1){
    while(*s) push_frame_buffer(*s++ << 1);
  }else{
    while(*s) push_frame_buffer(*s++);
  }
}

// Feed in the integer number you want to send out, plus the base and it queues it.
void push_frame_buffer_num(long n, uint8_t base){
  unsigned char buf[8 * sizeof(long)];
  long i = 0;
  
  // Handle negative numbers
  if(n < 0){
    push_frame_buffer('-');
    n = -n;
  }
  
  // If it's zero, save us some work
  if(n == 0){
    push_frame_buffer('0');
    return;
  }
  
  while(n > 0){
    buf[i++] = n % base;
    n /= base;
  }
  
  for(; i > 0; i--){
    push_frame_buffer((char)(buf[i-1] < 10 ? '0' + buf[i - 1] : 'A' + buf[i - 1] - 10));
  }
}

// Feed in floating point numbers, and the number of digits after the decimal point
// and it will be added to the queue.
void push_frame_buffer_float(double number, uint8_t digits){ 
  // Handle negative numbers
  if(number < 0.0){
     push_frame_buffer('-');
     number = -number;
  }

  // Round correctly so that print(1.999, 2) prints as "2.00"
  double rounding = 0.5;
  for (uint8_t i=0; i<digits; ++i)
    rounding /= 10.0;
  
  number += rounding;

  // Extract the integer part of the number and print it
  unsigned long int_part = (unsigned long)number;
  double remainder = number - (double)int_part;
  push_frame_buffer_num(int_part, 10);

  // Print the decimal point, but only if there are digits beyond
  if(digits > 0) push_frame_buffer('.'); 

  // Extract digits from the remainder one at a time
  while(digits-- > 0){
    remainder *= 10.0;
    int toPrint = int(remainder);
    push_frame_buffer_num(toPrint, 10);
    remainder -= toPrint; 
  } 
}

void push_frame_buffer(byte input){ // Add a byte to the data buffer. Everything gets put through here except the flags. Also update the CRC while we're at it.
  frame_buffer[buffer_pos] = input; 
  buffer_pos++;
}

void reset_frame(){ // Clear out the frame buffer before the next frame, reset buffer_pos, frame_pos, num_ones and crc
  buffer_pos = 0;
  num_ones = 0;
  crc = 0xFFFF;
  for(int i = 0; i < 512; i++){
    frame_buffer[i] = B01111110;
  }
}

void update_crc_bit(unsigned char input){ // Calculate the crc. Present implementation uses only the LSB of the input byte. Everything but the FCS bytes and the flags need CRC.
  crc ^= input;
  if(crc & 0x01){
    crc = (crc >> 1) ^ 0x8408;
  }else{
    crc = crc >> 1;
  }
}