// Trackuino custom libs
#include "config.h"
#include "afsk_avr.h"
#include "aprs.h"
#include "gps.h"
#include "pin.h"
#include "power.h"
#include "sensors_avr.h"

// Module constants
static const uint32_t VALID_POS_TIMEOUT = 2000;  // ms

// Module variables
static int32_t next_aprs = 0;

const int LEDDEBUG = 2; //enable LED when speed is above SPEED km value - for debug

/*
pin assignement:
0 GPS
1 GPS

13 TRAFFIC LED
2  SPEED LED DEBUG
11/3 APRS DATA
12 PTT

A0 LM60 TP IN
A1 
A2 VMETER_PIN

*/

void setup()
{
  pinMode(LED_PIN, OUTPUT);
  pin_write(LED_PIN, LOW);
  
  pinMode(LEDDEBUG, OUTPUT); //debug led attached to pin 2, speed value debug
  digitalWrite(LEDDEBUG, LOW); 
  
  Serial.begin(GPS_BAUDRATE);

  afsk_setup();
  gps_setup();
  sensors_setup();

  // Do not start until we get a valid time reference
  // for slotted transmissions.
  if (APRS_SLOT >= 0) {
    do {
      while (! Serial.available())
        power_save();
    } while (! gps_decode(Serial.read()));
    
    next_aprs = millis() + 1000 *
      (APRS_PERIOD_HIGH - (gps_seconds + APRS_PERIOD_HIGH - APRS_SLOT) % APRS_PERIOD_HIGH);
  }
  else {
    next_aprs = millis();
  }  
  // TODO: beep while we get a fix, maybe indicating the number of
  // visible satellites by a series of short beeps?
}

void get_pos()
{
  // Get a valid position from the GPS
  int valid_pos = 0;
  uint32_t timeout = millis();

  gps_reset_parser();

  do {
    if (Serial.available())
      valid_pos = gps_decode(Serial.read());
  } while ( (millis() - timeout < VALID_POS_TIMEOUT) && ! valid_pos) ;
  //BUZZER_ALTITUDE REMOVED
}

void loop()
{
  // Time for another APRS frame
  if ((int32_t) (millis() - next_aprs) >= 0) {
    get_pos();
//////////////////////////////////////////////////////////////////////////////////////////    
    if (gps_speed > 3) 
    {
      aprs_send();
      next_aprs += APRS_PERIOD_HIGH * 1000L;
      while (afsk_flush()) 
      {
      power_save();
    }
      digitalWrite(LEDDEBUG, HIGH); //set LED on D2 to High
    } 
   else
    {
      aprs_send();
      next_aprs += APRS_PERIOD_LOW * 1000L;
      while (afsk_flush()) 
      {
      power_save();
    }
      digitalWrite(LEDDEBUG, LOW); //set LED on D2 to Low
    }
//////////////////////////////////////////////////////////////////////////////////////////    
  } else {
    // Discard GPS data received during sleep window
    while (Serial.available()) {
      Serial.read();
    }
  }
  power_save(); // Incoming GPS data or interrupts will wake us up
}