#include <SPI.h>
#include <Wire.h>
#include <EEPROM.h>
#include <stdarg.h>
#include <Time.h>
#include <Timezone.h>
#include <FlexCAN.h>
#include <Adafruit_GPS.h>
#include "Adafruit_GFX.h"
#include "Adafruit_RA8875.h"
#include "Gauge.h"

//#define LAYOUT_DEBUG
#define MIN_FRAMETIME 50
#define ADC_SAMPLES 20
#define KNOTS2MPH 1.15078f

FlexCAN CANbus(500000);

//US Eastern Time Zone (New York, Detroit)
TimeChangeRule myDST = {"EDT", Second, Sun, Mar, 2, -240};    //Daylight time = UTC - 4 hours
TimeChangeRule mySTD = {"EST", First, Sun, Nov, 2, -300};     //Standard time = UTC - 5 hours
Timezone myTZ(myDST, mySTD);
TimeChangeRule *tcr = NULL;

// LCD
// Library only supports hardware SPI at this time
// Connect SCLK to UNO Digital #13 (Hardware SPI clock)
// Connect MISO to UNO Digital #12 (Hardware SPI MISO)
// Connect MOSI to UNO Digital #11 (Hardware SPI MOSI)
// CS, Reset
Adafruit_RA8875 tft = Adafruit_RA8875(14, 15);
Adafruit_RA8875 tft2 = Adafruit_RA8875(16, 17);
Adafruit_RA8875 tft3 = Adafruit_RA8875(18, 19);
Adafruit_RA8875 *screens[] = {&tft, &tft2, &tft3, NULL};

Adafruit_GPS GPS(&Serial2);

int RPM, MPH, OilPres, OilTmp, FuelLvl1, FuelLvl2, Cranking, Warmup, Unsync, CltTmp, LTurn, RTurn;
int PulseWidth, Advance, AFRTgt, MAP, MAT, TPS, Volts, AFR, VE, Cruise;
int Hour, Minutes, Seconds, GPSFix, Knots, Angle, Altitude, Satellites;
int Day, Month, Year;
float GPSLat, GPSLon;
char TimeDate[64], GPSLoc[64], Status1[64], Status2[64];
unsigned int RawADC[4][ADC_SAMPLES];
int NoVal = 0xFF, TickNum = 0;

struct AnalogTable {
  int   ADC;
  int   Value;
};
struct FastADCTransform {
  int   Offset;
  float Div;
};

struct FastADCTransform FastFuelLvl1Table = {540, 4.830F};
struct FastADCTransform FastFuelLvl2Table = {560, 4.630F};
struct FastADCTransform FastOilTempTable = {676, 1.16F};
struct FastADCTransform FastOilPressTable = {560, 3.307F};

struct GaugeColorMap LblMap[] = {{0, RA8875_WHITE, RA8875_BLACK}};
struct GaugeColorMap SignalMap[] = {{0, RA8875_BLACK, RA8875_BLACK}, {0, RA8875_GREEN, RA8875_BLACK}};
struct GaugeColorMap WarnMap[] = {{0, RA8875_BLACK, RA8875_BLACK}, {0, RA8875_WHITE, RA8875_RED}};
struct GaugeColorMap RPMMap[] = { {5000, RA8875_WHITE, RA8875_BLACK}, {6000, RA8875_YELLOW, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };
struct GaugeColorMap MPHMap[] = { {65, RA8875_WHITE, RA8875_BLACK}, {75, RA8875_YELLOW, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };
struct GaugeColorMap CLTMap[] = { {180, RA8875_GREEN, RA8875_BLACK}, {250, RA8875_WHITE, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };
struct GaugeColorMap OLTMap[] = { {180, RA8875_GREEN, RA8875_BLACK}, {250, RA8875_WHITE, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };
struct GaugeColorMap OLPMap[] = { {15, RA8875_RED, RA8875_BLACK}, {75, RA8875_WHITE, RA8875_BLACK}, {0, RA8875_YELLOW, RA8875_BLACK} };
struct GaugeColorMap FuelMap[] = { {10, RA8875_RED, RA8875_BLACK}, {25, RA8875_YELLOW, RA8875_BLACK}, {0, RA8875_WHITE, RA8875_BLACK} };
struct GaugeColorMap VoltMap[] = { {110, RA8875_RED, RA8875_BLACK}, {150, RA8875_WHITE, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };
struct GaugeColorMap AFRMap[] = { {130, RA8875_WHITE, RA8875_BLACK}, {150, RA8875_YELLOW, RA8875_BLACK}, {0, RA8875_RED, RA8875_BLACK} };

GaugeMaster *Gauges[] = {
  // Left Display
  new TextGauge (&tft, 570, 250, "%5i RPM", 1, &RPM, 3, RPMMap),
  new SweepGauge (&tft, 560, 240, 235, 0, 7000, 1000, 250, 500, 360, 90, &RPM, 3, RPMMap),

  new TextGauge (&tft, 215, 125, "Coolant Temp", 0, &CltTmp, 3, CLTMap),
  new TextGauge (&tft, 230, 125, "%4i*F", 0, &CltTmp, 3, CLTMap),
  new SweepGauge (&tft, 200, 120, 115, 100, 300, 50, 25, 50, 360, 90, &CltTmp, 3, CLTMap),

  new TextGauge (&tft, 215, 365, "Oil Temp", 0, &OilTmp, 3, OLTMap),
  new TextGauge (&tft, 230, 365, "%4i*F", 0, &OilTmp, 3, OLTMap),
  new SweepGauge (&tft, 200, 360, 115, 100, 300, 50, 25, 50, 360, 90, &OilTmp, 3, OLTMap),

  new TextGauge (&tft, 10, 10, "<----", 2, &LTurn, 2, SignalMap),
  new TextGauge (&tft, 10, 160, "CRANK", 2, &Cranking, 2, SignalMap),
  new TextGauge (&tft, 10, 320, "WARMUP", 2, &Warmup, 2, SignalMap),

  // Center Display
  new TextGauge (&tft2, 210, 125, "Fuel Level", 0, &NoVal, 1, LblMap),
  new TextGauge (&tft2, 225, 125, "1: %4i%%", 0, &FuelLvl1, 3, FuelMap),
  new TextGauge (&tft2, 240, 125, "2: %4i%%", 0, &FuelLvl2, 3, FuelMap),
  new SweepGauge (&tft2, 200, 120, 115, 0, 100, 25, 10, 50, 360, 90, &FuelLvl1, 1, LblMap),
  new SweepGauge (&tft2, 200, 120, 115, 0, 100, 25, 10, 50, 360, 90, &FuelLvl2, 1, LblMap),

  new TextGauge (&tft2, 210, 365, "Manifold", 0, &NoVal, 1, LblMap),
  new TextGauge (&tft2, 225, 365, "%4i KPA", 0, &MAP, 1, LblMap),
  new TextGauge (&tft2, 240, 365, "%4i *F", 0, &MAT, 1, LblMap),
  new SweepGauge (&tft2, 200, 360, 115, 0, 110, 20, 10, 20, 360, 90, &MAP, 1, LblMap),

  // Row 2
  new TextGauge (&tft2, 455, 125, "AFR = %3i", 0, &AFR, 3, AFRMap),
  new TextGauge (&tft2, 470, 125, "TGT = %3i", 0, &AFRTgt, 1, LblMap),
  new SweepGauge (&tft2, 440, 120, 115, 100, 200, 20, 10, 20, 360, 90, &AFRTgt, 1, LblMap),
  new SweepGauge (&tft2, 440, 120, 115, 100, 200, 20, 10, 20, 360, 90, &AFR, 3, AFRMap),

  new TextGauge (&tft2, 455, 365, "VE", 0, &NoVal, 1, LblMap),
  new TextGauge (&tft2, 470, 365, "%4i", 0, &VE, 1, LblMap),
  new SweepGauge (&tft2, 440, 360, 115, 0, 110, 20, 10, 20, 360, 90, &VE, 1, LblMap),

  // Row 3
  new TextGauge (&tft2, 695, 125, "Spark Advance", 0, &NoVal, 1, LblMap),
  new TextGauge (&tft2, 710, 125, "%4i*", 0, &Advance, 1, LblMap),
  new SweepGauge (&tft2, 680, 120, 115, 0, 50, 10, 5, 10, 360, 90, &Advance, 1, LblMap),

  new TextGauge (&tft2, 695, 365, "Inj PW", 0, &NoVal, 1, LblMap),
  new TextGauge (&tft2, 710, 365, "%4i MS", 0, &PulseWidth, 1, LblMap),
  new SweepGauge (&tft2, 680, 360, 115, 0, 30, 10, 5, 10, 360, 90, &PulseWidth, 1, LblMap),

  new TextGauge (&tft2, 10, 10, "UNSYNC", 2, &Unsync, 2, WarnMap),
  //new TextGauge (&tft2, 10, 180, "GPS", 2, &GPSFix, 1, LblMap),
  new TextGauge (&tft2, 5, 180, Status1, 1, &TickNum, 1, LblMap),
  new TextGauge (&tft2, 35, 180, Status2, 1, &TickNum, 1, LblMap),

  // Right Display
  new TextGauge (&tft3, 570, 250, "%5i MPH", 1, &MPH, 3, MPHMap),
  new TextGauge (&tft3, 600, 250, "%5i Cruise", 1, &Cruise, 3, LblMap),
  new SweepGauge (&tft3, 560, 240, 235, 0, 100, 10, 5, 10, 360, 90, &MPH, 3, MPHMap),

  new TextGauge (&tft3, 215, 125, "Oil Pressure", 0, &OilPres, 3, OLPMap),
  new TextGauge (&tft3, 230, 125, "%4i PSI", 0, &OilPres, 3, OLPMap),
  new SweepGauge (&tft3, 200, 120, 115, 0, 100, 25, 10, 25, 360, 90, &OilPres, 3, OLPMap),

  new TextGauge (&tft3, 215, 365, "Alternator", 0, &Volts, 3, VoltMap),
  new TextGauge (&tft3, 230, 365, "%5i Volts", 0, &Volts, 3, VoltMap),
  new SweepGauge (&tft3, 200, 360, 115, 60, 180, 60, 10, 20, 360, 90, &Volts, 3, VoltMap),

  new TextGauge (&tft3, 5, 5, TimeDate, 1, &Minutes, 1, LblMap),
  //new TextGauge (&tft3, 35, 5, GPSLoc, 1, &TickNum, 1, LblMap),
  new TextGauge (&tft3, 10, 360, "---->", 2, &RTurn, 2, SignalMap),

  NULL
};

volatile uint32_t Overflows = 0;
volatile uint32_t LastTick = -1;
const unsigned long   TicksPerSecond = 48000000ul / 128;
const unsigned int    PulsePerMile = 8000;

void ParseCANMsg(CAN_message_t *Msg);

static int Local_printf(const char *Fmt, ...) {
  int r;
  char buf[256];
  va_list args;
  va_start(args, Fmt);
  r = vsnprintf(buf, sizeof(buf), Fmt, args);
  va_end(args);
  Serial.print(buf);
  return r;
}
#define printf(...) Local_printf(__VA_ARGS__)

void tach_pulse() {
  LastTick = (Overflows << 16) | FTM1_CNT;
  Overflows = 0;
  FTM1_CNT = 0;
}


void ftm1_isr(void) {
  Overflows++;
  LastTick = Overflows << 16;
  FTM1_CNT = 0;
  while (FTM1_SC & FTM_SC_TOF) {
    FTM1_SC &= ~ FTM_SC_TOF;
  }
}

void setup() {
  int i, x = 0;
  Serial.begin(115200); // Logs

  Serial2.begin(9600); // GPS
  GPS.sendCommand(PMTK_SET_BAUD_57600);
  Serial2.end();
  Serial2.begin(57600);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
  GPS.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
  //GPS.sendCommand(PGCMD_ANTENNA);

  for (i = 0; screens[i]; i++) {
    while (!screens[i]->begin(RA8875_800x480)) {
      printf("%i LCD %i not found!\n", x++, i);
      delay(250);
    }
    screens[i]->displayOn(true);
    // 1, 0, 1 -- Cable on right
    // 0, 1, 1 -- Cable on left
    screens[i]->scanDirection(0, 1, 1);
    screens[i]->GPIOX(true);      // Enable TFT - display enable tied to GPIOX
    screens[i]->PWM1config(true, RA8875_PWM_CLK_DIV1024); // PWM output for backlight
    screens[i]->PWM1out(255);
    screens[i]->graphicsMode();
    screens[i]->fillScreen(RA8875_BLACK);
  }

  CANbus.begin();

  for (int i = 0; Gauges[i]; i++) {
    Gauges[i]->Init();
  }
  // New loop since Init might overlap some gauges
  for (int i = 0; Gauges[i]; i++) {
    Gauges[i]->Tick();
  }

  // Setup square wave reader for MPH
  noInterrupts();
  pinMode(6, INPUT);
  attachInterrupt(6, tach_pulse, FALLING);
  FTM1_MOD = 65535;
  FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | 7 | FTM_SC_TOIE;
  //FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | _BV(CS32) | FTM_SC_TOIE;
  NVIC_ENABLE_IRQ(IRQ_FTM1);
  interrupts();

  // Turn signal inputs
  pinMode(2, INPUT);
  pinMode(5, INPUT);

  setSyncProvider(getTeensy3Time);
  if (timeStatus() != timeSet) {
    Serial.println("No time set on RTC");
  } else {
    Serial.println("Time set from RTC");
  }

  Serial.println("Online!");
}

void loop() {
  static unsigned long LastTime = millis();
  unsigned int ticks = LastTick;
  if (ticks) {
    unsigned long PulsePerSecond = TicksPerSecond / ticks;
    MPH = PulsePerSecond * 3600 / PulsePerMile;
  } else {
    MPH = 0;
  }

  TickNum++;

  ReadADC();
  UpdateTime();

  static int FullDraw=0;
  Gauges[FullDraw++]->Tick();
  if(!Gauges[FullDraw]) {
    FullDraw=0;
  }
  for (int i = 0; Gauges[i]; i++) {
    Gauges[i]->DiffTick();
  }

  snprintf(Status1, sizeof(Status1), "%3lums", millis() - LastTime);

  do {
    ReadCAN();
    ReadGPS();
  } while (millis() < LastTime + MIN_FRAMETIME);
  snprintf(Status2, sizeof(Status2), "%3lums", millis() - LastTime);
  LastTime = millis();
}

void ReadADC() {
  static unsigned int ADCPos=0;
  int v[4]={0,0,0,0};
  
  RawADC[0][ADCPos]=analogRead(6);
  RawADC[1][ADCPos]=analogRead(7);
  RawADC[2][ADCPos]=analogRead(8);
  RawADC[3][ADCPos]=analogRead(9);
  if(++ADCPos >= ADC_SAMPLES) {
    ADCPos=0;
  }

  for(int i=0;i<4;i++) {
    for (int x = 0; x < ADC_SAMPLES; x++) {
      v[i]+=RawADC[i][x];
    }
    v[i]/=ADC_SAMPLES;
  }
  
  LTurn = digitalReadFast(2);
  RTurn = digitalReadFast(5);
  FuelLvl1 = (v[0] - FastFuelLvl1Table.Offset) / FastFuelLvl1Table.Div;
  FuelLvl2 = (v[1] - FastFuelLvl2Table.Offset) / FastFuelLvl2Table.Div;
  OilTmp = (v[2] - FastOilTempTable.Offset) / FastOilTempTable.Div;
  OilPres = (v[3] - FastOilPressTable.Offset) / FastOilPressTable.Div;
}

void ReadCAN() {
  static unsigned LastCANMsgMils = 0;
  CAN_message_t msg;
  while (CANbus.available()) {
    CANbus.read(msg);
    ParseCANMsg(&msg);
    LastCANMsgMils = millis();
    Unsync = 0;
  }
  if (millis() - LastCANMsgMils > 1000) {
    Unsync = 1;
  }
}

#define WORD(o) (((uint16_t)Msg->buf[o]<<8) | Msg->buf[o+1])
#define SWORD(o) (((int16_t)Msg->buf[o]<<8) | Msg->buf[o+1])
void ParseCANMsg(CAN_message_t *Msg) {
  
  //printf("%lu CAN Msg ID = %lu Len = %u Data = %02x %02x %02x %02x %02x %02x %02x %02x\n", millis(),
      //Msg->id, Msg->len, Msg->buf[0], Msg->buf[1], Msg->buf[2], Msg->buf[3], Msg->buf[4], Msg->buf[5], Msg->buf[6], Msg->buf[7]);
      
  switch (Msg->id) {
    case 500:
      printf("Error from CAN Slave: %.8s\n", Msg->buf);
      break;
    case 1512:
      MAP = SWORD(0) / 10;
      RPM = WORD(2);
      CltTmp = SWORD(4) / 10;
      TPS = SWORD(6) / 10;
      break;
    case 1513:
      PulseWidth = WORD(0) / 1000;
      MAT = SWORD(4) / 10;
      Advance = SWORD(6) / 10;
      break;
    case 1514:
      AFRTgt = Msg->buf[0];
      AFR = Msg->buf[1];
      break;
    case 1515:
      Volts = SWORD(0);
      break;
    case 1520:
      PulseWidth = WORD(2) / 1000;
      RPM = WORD(6);
      break;
    case 1521:
      Advance = SWORD(0) / 10;
      // Engine status at 3
      Cranking = Msg->buf[3] & 0b00000010;
      Warmup =   Msg->buf[3] & 0b00001000;
      AFRTgt = Msg->buf[4];
      break;
    case 1522:
      MAP = SWORD(2) / 10;
      MAT = SWORD(4) / 10;
      CltTmp = SWORD(6) / 10;
      break;
    case 1523:
      TPS = SWORD(0) / 10;
      Volts = SWORD(2);
      AFR = SWORD(4);
      break;
    case 1526:
      VE = SWORD(2) / 10;
      break;
    case 1551:
      AFR = Msg->buf[0];
      break;
    case 557752:
      SendCAN();
      break;
    default:
      printf("%lu Unknown CAN Msg ID=%lu Len=%u Data=%02x %02x %02x %02x %02x %02x %02x %02x\n", millis(),
             Msg->id, Msg->len, Msg->buf[0], Msg->buf[1], Msg->buf[2], Msg->buf[3], Msg->buf[4], Msg->buf[5], Msg->buf[6], Msg->buf[7]);
  }
}

void SendCAN() {
  //4846944 Got CAN Msg ID=557752 Len=3 Data=070d 0800 0000 0000
  // 10 001 0000 0101 0111 000
  // 07 0d 08
  // 00000111 00001101 00001000
  /*const uint32_t OffsetMask   = 0b00001111111111000000000000000000;
  const uint32_t MSG_TypeMask = 0b00000000000000111000000000000000;
  const uint32_t From_IDMask  = 0b00000000000000000111100000000000;
  const uint32_t To_IDMask    = 0b00000000000000000000011110000000;
  const uint32_t TableMask    = 0b00000000000000000000000001111000;
  const uint8_t  myvarblkMask     = 0b00011111;
  const uint8_t  myvaroffestMask  = 0b11100000;
  const uint8_t  varbytMask       = 0b00001111;*/
  const uint32_t SendOffset = (0x0d << 3) | ((0x08 & 0b11100000) >> 5); //2;
  const uint32_t SendMsgType = 2;
  const uint32_t SendFromID = 5;
  const uint32_t SendToID = 0;
  const uint32_t SendTable = 7;
  CAN_message_t msg;

  msg.id = (SendOffset << 18) | (SendMsgType << 15) | (SendFromID << 11) | (SendToID << 7) | (SendTable << 3);
  msg.ext = 1;
  msg.len = 8;
  msg.timeout = 10;
  msg.buf[0] = RawADC[0][0] >> 8;
  msg.buf[1] = RawADC[0][0];
  msg.buf[2] = RawADC[1][0] >> 8;
  msg.buf[3] = RawADC[1][0];
  msg.buf[4] = RawADC[2][0] >> 8;
  msg.buf[5] = RawADC[2][0];
  msg.buf[6] = RawADC[3][0] >> 8;
  msg.buf[7] = RawADC[3][0];
  CANbus.write(msg);
}

int CalculateTableVal(int ADC, const struct AnalogTable * Table) {
  const struct AnalogTable *Prev = &Table[0];

  for (int i = 1;; i++) {
    if (Table[i].ADC >= ADC || (Table[i + 1].ADC == 0 && Table[i + 1].Value == 0)) {
      // This is the last item, so use this as right bound
      //Next = &Table[i];
      return Prev->Value + (Table[i].Value - Prev->Value) * (ADC - Prev->ADC) / (Table[i].Value - Prev->Value);
    } else {
      // Use this as left bound
      Prev = &Table[i];
    }
  }
  return 0xFFFF;
}

int FastTwoTable(int ADC, const struct AnalogTable * Table) {
  return Table[0].Value + (Table[1].Value - Table[0].Value) * (ADC - Table[0].ADC) / (Table[1].ADC - Table[0].ADC);
}

void ReadGPS() {
  static bool clockset = 0;
  //static char GPSBuf[2048];
  //static unsigned int GPSBufPos = 0;
  while (Serial2.available()) {
    //char c = 
    GPS.read();

    /*
    GPSBuf[GPSBufPos++] = c;
    if (GPSBufPos >= sizeof(GPSBuf)) {
      Serial.println("GPS Overflow");
      GPSBufPos = 0;
    }
    */

    if (GPS.newNMEAreceived()) {
      //GPSBuf[GPSBufPos] = 0;
      //GPSBufPos = 0;
      /*
            char *c, *n;
            if ((c = strstr(GPSBuf, "$GPGGA"))) {
              if ((n = strchr(c + 1, '$'))) {
                *n = 0;
              }
              if ((n = strchr(c + 1, '\r'))) {
                *n = 0;
              }

              for (int x = 0; x < 15; x++) {
                if (!(c = strchr(c, ','))) {
                  break;
                }
                c++;
              }

              if (c && (n = strchr(c, ',')) && (n - c == 6) && !strchr(c, '.')) {
                *n = 0;

                int Date = 0;
                Date = atoi(c);

                Year = (Date % 100) + 2000;
                Month = (Date / 100) % 100;
                Day = (Date / 10000);

                setTime(Hour, Minutes, Seconds, Day, Month, Year);

              }
            }
      */
      if (GPS.parse(GPS.lastNMEA())) {
        if (!clockset && GPS.fix && ( (GPS.year < 40 && GPS.year > 14) || (GPS.year < 2040 && GPS.year > 2014))) {
          int y = GPS.year;
          if(y < 100) {
            y+=2000; 
          }
          time_t t = tmConvert_t(y, GPS.month, GPS.day, GPS.hour, GPS.minute, GPS.seconds);
          time_t local = myTZ.toLocal(t, &tcr);
          Teensy3Clock.set(local);
          setTime(local);
          printf("Set GPS time: %i/%i/%04i %02i:%02i:%02i (%s)\n", GPS.month, GPS.day, y, GPS.hour, GPS.minute, GPS.seconds, GPS.fix ? "FIX" : "NO");
          clockset=1;
        }/* else if(!clockset && GPS.fix) {
          printf("Invalid time: %i/%i/%04i %02i:%02i:%02i (%s)\n", GPS.month, GPS.day, GPS.year, GPS.hour, GPS.minute, GPS.seconds, GPS.fix ? "FIX" : "NO");
        }*/

        //UpdateTime();

        if (GPS.fix) {
          GPSFix=1;
          Knots = GPS.speed;
          //MPH = GPS.speed * KNOTS2MPH;
          GPSLat = GPS.latitudeDegrees;
          GPSLon = GPS.longitudeDegrees;
          Angle = GPS.angle;
          Altitude = GPS.altitude;
          Satellites = GPS.satellites;

          //snprintf(GPSLoc, sizeof(GPSLoc), " % 3.4f % 3.4f % i", GPSLat, GPSLon, Angle);

          /*
          Serial.print("Location (in degrees, works with Google Maps): ");
          Serial.print(GPS.latitudeDegrees, 4);
          Serial.print(", ");
          Serial.println(GPS.longitudeDegrees, 4);
          Serial.print("Speed (knots): "); Serial.println(GPS.speed);
          Serial.print("Angle: "); Serial.println(GPS.angle);
          Serial.print("Altitude: "); Serial.println(GPS.altitude);
          Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
          */
          
        } else {
          GPSLoc[0] = 0;
          GPSFix=0;
        }
      }
    }
  }
}

void UpdateTime() {
  static int LastMin=-1;
  //time_t local = Teensy3Clock.get();
  time_t local = now();
  if(timeStatus() == timeSet && LastMin != minute(local)) {
    Hour = hourFormat12(local);
    Minutes = minute(local);
    Seconds = second(local);
    Day = day(local);
    Month = month(local);
    Year = year(local);
    LastMin=minute(local);
    snprintf(TimeDate, sizeof(TimeDate), "%2i:%02i %s %i/%i/%i  ", Hour, Minutes, (isPM() ? "PM" : "AM"), Month, Day, Year);
  }
}

time_t tmConvert_t(int YYYY, byte MM, byte DD, byte hh, byte mm, byte ss) {
  tmElements_t tmSet;
  tmSet.Year = YYYY - 1970;
  tmSet.Month = MM;
  tmSet.Day = DD;
  tmSet.Hour = hh;
  tmSet.Minute = mm;
  tmSet.Second = ss;
  return makeTime(tmSet);         //convert to time_t
}

time_t getTeensy3Time() {
  return Teensy3Clock.get();
}