#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <Arduino.h>
#include <util/atomic.h>

#define VERSION "HomeBrew ProMicro GPS Version 0.3"
/*
 * Version 0.3: don't write to Serial it is connected (connect it before startup).
 * Version 0.2: faster GPS fixes, lower RAM usage, print "Time to Fix", turn off LEDs, other tweaks.
 * Version 0.1: initial release for Pro Micro.
 */

#define Usb                 Serial  // for debug messages

// Definitions for the hardware Serial port, used for Nexstar auxBus
#define AUXBUS_BUSY_PIN     4       // The RTS (aka. "Busy") line for Nexstar auxBus
#define auxBus              Serial1

// Definitions for the software serial port, used for receiving from the GPSr:
#define GPS_RX_PIN          8
#define GPS_TX_PIN          9
SoftwareSerial              gpsr(GPS_RX_PIN, GPS_TX_PIN);

TinyGPSPlus gps;

// Various "known" device IDs on Nexstar auxBus:
#define DEV_MAIN            0x01
#define DEV_HC              0x04
#define DEV_HC2             0x0d
#define DEV_AZM             0x10
#define DEV_ALT             0x11
#define DEV_GPS             0xb0

// GPS related commands over Nexstar auxBus:
#define GPS_GET_LAT         0x01
#define GPS_GET_LONG        0x02
#define GPS_GET_DATE        0x03
#define GPS_GET_YEAR        0x04
#define GPS_GET_SAT_INFO    0x07
#define GPS_GET_RCVR_STATUS 0x08
#define GPS_GET_TIME        0x33
#define GPS_TIME_VALID      0x36
#define GPS_LINKED          0x37
#define GPS_GET_HW_VER      0x55
#define GPS_GET_COMPASS     0xa0
#define GPS_GET_VER         0xfe

#define GPS_HW_VER          0xeb    // GPSr hardware version

// Changing these to use GNSS reports.  Should probably make it either/or.
TinyGPSCustom satellitesInView(gps, "GPGSV", 3);
TinyGPSCustom fix3D(gps, "GPGSA", 2);          // 1 = no fix,  2 = 2D fix, 3 = 3D fix
TinyGPSCustom fixQuality(gps, "GPGGA", 6);     // 0 = invalid, 1 = GPS,    2 = DGPS, etc...
TinyGPSCustom fix3DGNSS(gps, "GNGSA", 2);      // 1 = no fix,  2 = 2D fix, 3 = 3D fix
TinyGPSCustom fixQualityGNSS(gps, "GNGGA", 6); // 0 = invalid, 1 = GPS,    2 = DGPS, etc...

// For more details how convert GPS position into 24 bit format,
//  see "NexStar Communication Protocol", section "GPS Commands".
// https://www.nexstarsite.com/download/manuals/NexStarCommunicationProtocolV1.2.zip
static const double GPS_MULT_FACTOR = 46603.37778;  // = 2^24 / 360

static uint8_t rx_buf[12];    // Big enough for the largest expected request.
static uint8_t rx_count = 0;
static enum { RX_PREAMBLE_WAIT, RX_LENGTH_WAIT, RX_DATA, RX_CKSUM, RX_VALID } rx_state;
static int16_t response_checksum;
static bool have_gps_fix = false;
static bool gps_is_on = true;
static long gps_sleep_timeout = 0;
static long gps_wakeup_timeout = 0;
static long gps_started = 0;  // used to derive Time-To-Fix (TTF)

static bool usb_connected (void)
{
  static bool connected = false;
  if (Usb) {
    if (!connected) {
      connected = true;
      Usb.begin(115200);
    }
  } else {
    connected = false;
  }
  return connected;
}
#define USB usb_connected()

// Get a (non-zero!) future timeout in milliseconds:
static inline long get_timeout (unsigned long offset)
{
  long t = millis() + offset;
  return t ? t : 1;
}

// Compare against current time, handling wraparound:
static inline bool time_after (long a, long b)
{
  return (b - a) < 0;
}
#define time_before(a,b) time_after((b),(a))

static void gps_set_timeout (unsigned int secs)
{
  if (secs == 0) {
    if (gps_sleep_timeout)
      if (USB) Usb.println(F("Cancel timeout"));
    gps_sleep_timeout = 0;
  } else {
    if (!gps_sleep_timeout) {
      if (USB) {
        Usb.print(F("Start timeout: "));
        Usb.print(secs);
        Usb.println(F(" secs"));
      }
    }
    gps_sleep_timeout = get_timeout(1000uL * secs);
  }
}

static void gps_drain (void)
{
  while (gpsr.available() > 0) {
    gpsr.read();   // Empty the GPS serial buffer of spurious data
    delay(2);
  }
}

void gps_turn_off (void)
{
  if (USB) Usb.println(F("GPS Off"));
  static const uint8_t GPS_off[] = {0xb5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x08, 0x00, 0x16, 0x74};
  gps_drain();
  gpsr.write(GPS_off, sizeof(GPS_off));
  delay(20);
  gps_drain();
  gps_is_on = false;
  gps_set_timeout(0);
  gps_wakeup_timeout = 0;
  gps_started = 0;
}

void gps_turn_on (void)
{
  if (USB) Usb.println(F("GPS On"));
  static const uint8_t GPS_on[]  = {0xb5, 0x62, 0x06, 0x04, 0x04, 0x00, 0x00, 0x00, 0x09, 0x00, 0x17, 0x76};
  gpsr.write(GPS_on, sizeof(GPS_on));
  gps_is_on = true;
  gps_set_timeout(0);
  delay(50);                  // Give time for GPS to wake up
  gps_drain();
  gps_wakeup_timeout = get_timeout(3000);
  gps_started = get_timeout(0);
}

static void auxBus_end_transmission (void)
{
  auxBus.flush();                  // doesn't return until AFTER the last byte is actually shifted out
  pinMode(AUXBUS_BUSY_PIN, INPUT_PULLUP);    // float BUSY line high to release the bus
}

#define RESPOND_1_BYTE(d1)         send_message(1, rx_buf[2], rx_buf[1], rx_buf[3], d1,  0,  0)
#define RESPOND_2_BYTES(d1,d2)     send_message(2, rx_buf[2], rx_buf[1], rx_buf[3], d1, d2,  0)
#define RESPOND_3_BYTES(d1,d2,d3)  send_message(3, rx_buf[2], rx_buf[1], rx_buf[3], d1, d2, d3)

static void handle_request (void)
{
  switch (rx_buf[3])
  {
    case GPS_LINKED:
    case GPS_TIME_VALID: {
      bool is_valid = (gps_is_on && have_gps_fix && !gps_wakeup_timeout);
      RESPOND_1_BYTE(is_valid);
      if (USB) {
        Usb.print((rx_buf[3] == GPS_LINKED) ? F("LINKED") : F("TIME_VALID"));
        Usb.print(F(": "));
        Serial_print_true_false(is_valid);
      }
      break;
    }

    case GPS_GET_TIME:
      RESPOND_3_BYTES(gps.time.hour(), gps.time.minute(), gps.time.second());
      if (USB) Usb.println(F("GET_TIME"));
      break;

    case GPS_GET_HW_VER:
      RESPOND_1_BYTE(GPS_HW_VER);
      if (USB) Usb.println(F("GET_HW_VER"));
      break;

    case GPS_GET_YEAR:
      RESPOND_2_BYTES(gps.date.year() >> 8, gps.date.year() & 0xff);
      if (USB) Usb.println(F("GET_YEAR"));
      break;

    case GPS_GET_DATE:
      RESPOND_2_BYTES(gps.date.month(), gps.date.day());
      if (USB) Usb.println(F("GET_DATE"));
      break;

    case GPS_GET_LAT: {
      int32_t lat = (int32_t) (gps.location.lat() * GPS_MULT_FACTOR);
      uint8_t* latBytePtr = (uint8_t*)&lat;
      RESPOND_3_BYTES(latBytePtr[2], latBytePtr[1], latBytePtr[0]);
      if (USB) Usb.println(F("GET_LAT"));
      break;
    }

    case GPS_GET_LONG: {
      int32_t lng = (int32_t) (gps.location.lng() * GPS_MULT_FACTOR);
      uint8_t* lngBytePtr = (uint8_t*)&lng;
      RESPOND_3_BYTES(lngBytePtr[2], lngBytePtr[1], lngBytePtr[0]);
      if (USB) Usb.println(F("GET_LONG"));
      break;
    }

    case GPS_GET_SAT_INFO: {
      String satellitesInViewString(satellitesInView.value());
      RESPOND_2_BYTES(satellitesInViewString.toInt(), gps.satellites.value());
      if (USB) Usb.println(F("GET_SAT_INFO"));
      break;
    }

    case GPS_GET_VER:
      RESPOND_2_BYTES(2, 0);  // Version 2.0
      if (USB) Usb.println(F("GET_VER"));
      break;

    case GPS_GET_RCVR_STATUS:
      if (USB) Usb.println(F("GET_RCVR_STATUS"));
      break;
    case GPS_GET_COMPASS:
      if (USB) Usb.println(F("GET_COMPASS"));
      break;
    default:
      if (USB) Usb.print(F("GPS_"));
      if (USB) Usb.println(rx_buf[3]);
      break;
  }
}

// Handle both GNSS and GPS format strings
static int getGpsQuality()
{
  if (!gps_is_on)
    return -1;
  if (gps_wakeup_timeout) {
    // Give TinyGPS++ library time to realize it has no valid data:
    if (time_before(millis(), gps_wakeup_timeout))
      return -1;
    gps_wakeup_timeout = 0;
  }
  String quality(fixQualityGNSS.value());
  if (quality.length() && quality.toInt() > 0)
    return quality.toInt();
  quality = fixQuality.value();
  if (quality.length() && quality.toInt() > 0)
    return quality.toInt();
  return -1;
}

static void gps_receive (void)
{
  // Feed characters from the GPS module into TinyGPS
  while (gpsr.available()) {
    char c = gpsr.read();
#if 0  //FIXME
    if (USB) Usb.write(c);
#endif
    gps.encode(c);
  }
}

static void Serial_print_true_false (bool b)
{
  if (USB) Usb.println(b ? F("true") : F("false"));
}

static void request_decode(uint8_t c)
{
  switch (rx_state)
  {
    case RX_VALID:
    default:
      rx_state = RX_PREAMBLE_WAIT;
      // Fall thru.
    case RX_PREAMBLE_WAIT:
      if (c == 0x3b)
        rx_state = RX_LENGTH_WAIT;
      break;

    case RX_LENGTH_WAIT:
      if (c < sizeof(rx_buf)) {
        rx_buf[0] = c;
        rx_count = 1;
        rx_state = RX_DATA;
      } else
        rx_state = RX_PREAMBLE_WAIT;
      break;

    case RX_DATA:
      rx_buf[rx_count] = c;
      if (rx_count++ == rx_buf[0])
        rx_state = RX_CKSUM;
      break;

    case RX_CKSUM:
      rx_state = verify_checksum(c) ? RX_VALID : RX_PREAMBLE_WAIT;
      break;
  }
}

static bool verify_checksum(uint8_t target)
{
  int16_t sum = 0;
  for (int i = 0; i <= rx_buf[0]; i++)
    sum += rx_buf[i];
  uint8_t chk = (-sum) & 0xff;
  return (target == chk);
}

static inline void response_begin()
{
  digitalWrite(AUXBUS_BUSY_PIN, LOW);
  pinMode(AUXBUS_BUSY_PIN, OUTPUT);    // pull BUSY line low to claim the bus
  digitalWrite(AUXBUS_BUSY_PIN, LOW);
  response_checksum = 0;
}

static inline void response_write(uint8_t b)
{
  response_checksum += b;
  auxBus.write(b);
}

static inline void response_end()
{
  int8_t c = (-response_checksum) & 0xff;
  auxBus.write(c);
  auxBus_end_transmission();
}

static void send_message (uint8_t databytes, uint8_t src, uint8_t dst, uint8_t cmd, uint8_t data0, uint8_t data1, uint8_t data2)
{
  // Wait up to 2 seconds for the bus to become available for transmit.
  // This allows for other masters on the bus (eg. Wifi) as well as the hand-controller.
  if (digitalRead(AUXBUS_BUSY_PIN) == LOW) {
    long timeout = get_timeout(2000);
    while (digitalRead(AUXBUS_BUSY_PIN) == LOW) {
      if (time_after(millis(), timeout))
        return;  // drop the packet
    }
  }

  response_begin();
  auxBus.write(0x3b);             // preamble (not included in checksum)
  response_write(3 + databytes);  // length
  response_write(src);            // source device
  response_write(dst);            // destination device
  response_write(cmd);            // request type
  response_write(data0);          // data0
  if (databytes > 1) {
    response_write(data1);        // data1
    if (databytes > 2)
      response_write(data2);      // data2
  }
  response_end();                 // checksum
}

void loop (void)
{
  bool old_fix = have_gps_fix;
  have_gps_fix = getGpsQuality() > 0;

  if (have_gps_fix != old_fix) {
    old_fix = have_gps_fix;
    if (USB) Usb.print(F("GPS Fix: "));
    Serial_print_true_false(have_gps_fix);
    gps_set_timeout(0);
    if (have_gps_fix) {
      if (USB) {
        Usb.print(F("Time to Fix: "));
        Usb.print((500uL + millis() - gps_started) / 1000uL);
        Usb.println(F(" secs"));
      }
      gps_started = 0;
    } else {
      gps_started = get_timeout(0);
    }
  }
  gps_receive();

  // Feed characters from the serial port into the request decoder
  while (auxBus.available())
    request_decode(auxBus.read());

  // Check if request is valid
  if (rx_state == RX_VALID) {
    if (rx_buf[2] == DEV_GPS) {
      // Power up GPS unless it's a simple "Get Version" query:
      if (!gps_is_on && rx_buf[3] != GPS_GET_VER)
        gps_turn_on();
      gps_set_timeout(have_gps_fix ? 30 : 0ul);  // power off after 30-secs of no activity
      handle_request();
    }
    rx_state = RX_PREAMBLE_WAIT;
  }
  if (gps_is_on) {
    if (!have_gps_fix) {
      gps_set_timeout(0);
    } else if (!gps_sleep_timeout) {
      gps_set_timeout(120);  // Wait up to 2min for something to query the GPS
    } else if (time_after(millis(), gps_sleep_timeout)) {
      gps_set_timeout(0);
      gps_turn_off();
    }
  }
  // Keep the LEDs turned OFF to save power:
  digitalWrite(LED_BUILTIN_RX, HIGH);
  digitalWrite(LED_BUILTIN_TX, HIGH);
}

void setup (void)
{
  // Initialize the auxBus comms:
  pinMode(AUXBUS_BUSY_PIN, INPUT_PULLUP);    // float BUSY line high to release the bus
  auxBus.begin(19200,SERIAL_8N2);
  auxBus_end_transmission();

  gpsr.begin(9600);                   // Start the GPSr serial port
  rx_state = RX_PREAMBLE_WAIT;
  delay(2000);
  if (USB) Usb.println(F(VERSION));
  gps_turn_on();
}