#include "project.h"

#define PPS (GPIO2)
#define PPS_PORT GPIOD

#define UBX_BUF_LEN 256

#define TIMEOUT 4000

static int ubx_ack = 0;
static int ubx_ack_xfer = 0;

static int current_ref_hz = 1;

int gps_locked;
int gps_happy;

static  char fix, fix2;
static int32_t freq = 0;

static const int fish[] = { 1, 2, 3, 4, 5, 6, 7 };

static Event_ring gps_ring;


void exti2_isr (void)
{
  uint32_t now = SCS_DWT_CYCCNT;
  int v;

  v = !!gpio_get (PPS_PORT, PPS);

  nvic_disable_irq (NVIC_EXTI2_IRQ);
  exti_reset_request (EXTI2);

  gps_ring.events[gps_ring.tx_ptr].when = now;
  gps_ring.events[gps_ring.tx_ptr].value = v;
  gps_ring.tx_ptr = (gps_ring.tx_ptr + 1) & ERING_MASK;

  nvic_enable_irq (NVIC_EXTI2_IRQ);

}



static inline int
ubx_recv_byte (uint8_t *v)
{
  return !ring_read_byte (&rx3_ring, v);
}

static inline void
ubx_send_byte (uint8_t v)
{
  usart3_queue (v);
}

static int
ubx_recv_nav_status (uint8_t *ptr, unsigned len)
{
  uint8_t gps_fix, flags;
  uint32_t d;

  if ((!ptr) || (len != 16))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_u8 (ptr, &gps_fix); //fix type
  ptr += ubx_get_u8 (ptr, &flags); //fix type


  switch (gps_fix) {
  case 0:
  case 1:
    fix = '-';
    gps_locked = 0;
    break;

  case 2:
    fix = '2';
    gps_locked = 0;
    break;

  case 3:
    fix = 'L';
    gps_locked = 1;
    break;

  case 4:
    fix = 'R';
    gps_locked = 0;
    break;

  case 5:
    fix = 'T';
    gps_locked = 0;
    break;

  default:
    fix = '?';
    gps_locked = 0;
  }

  switch (flags & 3) {
  case 0:
  case 2:
    fix2 = '!';
    break;

  case 1:
    fix2 = '-';
    break;

  case 3:
    fix2 = 'D';

    if (gps_locked == 1)
      gps_locked = 2;

    break;

  default:
    fix2 = '?';
  }


  //  printf ("fix: %c%c\r\n",fix,fix2);


  if ((gps_locked) && (gps_happy < 10000))
    gps_happy++;
  else
    gps_happy = 0;

  return 0;
}


static int
ubx_recv_clock_stats (uint8_t *ptr, unsigned len)
{
  //char buf[40];
  int32_t drift;
  uint32_t d;

  if ((!ptr) || (len != 20))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_u32 (ptr, &d); //bias
  ptr += ubx_get_i32 (ptr, &drift); //drift
  ptr += ubx_get_u32 (ptr, &d); //time acc estimate
  ptr += ubx_get_i32 (ptr, &freq); //freq acc estimate

  //  printf ("TCXO %+8dE-12\r\n", (int) freq);
#if 0
  sprintf (buf, "TCXO %+8dE-12", (int) freq);
  lcd_erase_line (18, 0);
  lcd_write (buf, 0, 0);
#endif
  return 0;

}

static int
ubx_recv_utc (uint8_t *ptr, unsigned len)
{
  int32_t nano;
  uint32_t acc;
  uint16_t year;
  uint8_t hour, min, sec, day, month;
  uint32_t d;
  //  char buf[40];

  if ((!ptr) || (len != 20))
    return -1;

  ptr += ubx_get_u32 (ptr, &d); //TOW
  ptr += ubx_get_u32 (ptr, &acc); //bias
  ptr += ubx_get_i32 (ptr, &nano);
  ptr += ubx_get_u16 (ptr, &year);
  ptr += ubx_get_u8 (ptr, &month);
  ptr += ubx_get_u8 (ptr, &day);
  ptr += ubx_get_u8 (ptr, &hour);
  ptr += ubx_get_u8 (ptr, &min);
  ptr += ubx_get_u8 (ptr, &sec);

  printf ("GPS META-DATA %04d-%02d-%02d %02d:%02d:%02d Fix:%c%c TXCO  %+8dE-12\r\n",
          year,
          month,
          day,
          hour,
          min,
          sec,
          fix, fix2, (int) freq);

  if (gps_happy > 3) {
    UTC u;
    EPOCH gps_time;
    uint32_t now;
    uint64_t abs;

    u.jday = 0;
    u.year = year;
    u.month = month;
    u.mday = day;
    u.hour = hour;
    u.minute = min;
    u.second = sec;
    u.nanosecond = 0;

    gps_time = time_utc_to_epoch (u);


    now = SCS_DWT_CYCCNT;
    abs = abs_extend (now);
    pll_set_offset (gps_time, abs);
  }


#if 0
  sprintf (buf, "%+6dE-12 %02d%02d%02d", (int) freq,
           (int) hour, (int)min, (int) sec);
  lcd_erase_line (18, 0);
  lcd_write (buf, 0, 0);
#endif

  return 0;
}

static int ubx_recv_rinex (uint8_t *payload, unsigned len)
{

  printf ("Rinex\r\n");
  return 0;
}



static void
ubx_recv (uint8_t class, uint8_t id, uint8_t *payload, unsigned len)
{
  //  printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);

  switch (class) {
  case 1:
    switch (id) {
    case 0x3:
      ubx_recv_nav_status (payload, len);
      break;

    case 0x21:
      ubx_recv_utc (payload, len);
      break;

    case 0x22:
      ubx_recv_clock_stats (payload, len);
      break;

    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;

  case 2:
    switch (id) {
    case 0x10:
      ubx_recv_rinex (payload, len);

      break;
    }

    break;

  case 5:
    ubx_ack++;
    printf (" %s for %02x.%02x\r\n", id ? "ACK" : "NAK", payload[0],
            payload[1]);

    break;

  case 0x0b:
    switch (id) {
    case 0x50:
      printf ("xfer ack\r\n");
      ubx_ack_xfer++;
      break;

    default:
      printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    break;

  default:
    printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);

    if (class != 0x03)
      hexdump (payload, len);
  }


}


typedef enum {
  UBX_SM_LOST = 0,
  UBX_SM_S2,
  UBX_SM_C,
  UBX_SM_I,
  UBX_SM_L1,
  UBX_SM_L2,
  UBX_SM_DATA,
  UBX_SM_CKA,
  UBX_SM_CKB
} ubx_sm_t;




static uint8_t *
ubx_dispatch_search (int s_class, int s_id, unsigned *len_ptr)
{
  static uint8_t buf[UBX_BUF_LEN];
  static ubx_sm_t sm = UBX_SM_LOST;
  static uint8_t class, id;
  static unsigned ptr;
  static unsigned len;
  static uint8_t ck_a, ck_b;


  void ubx_ck (uint8_t v) {
    ck_a += v;
    ck_b = ck_b + ck_a;
  }


  uint8_t c;

  while (ubx_recv_byte (&c)) {

    switch (sm) {
    case UBX_SM_LOST:
      if (c == 0xb5)
        sm = UBX_SM_S2;

      break;

    case UBX_SM_S2:
      if (c == 0x62)
        sm = UBX_SM_C;
      else
        sm = UBX_SM_LOST;

      break;

    case UBX_SM_C:
      ck_a = ck_b = class = c;
      sm = UBX_SM_I;
      break;

    case UBX_SM_I:
      ubx_ck (c);
      id = c;
      sm = UBX_SM_L1;
      break;

    case UBX_SM_L1:
      ubx_ck (c);
      len = c;
      sm = UBX_SM_L2;
      break;

    case UBX_SM_L2:
      ubx_ck (c);
      len |= c << 8;

      ptr = 0;

      if (len)
        sm = UBX_SM_DATA;
      else
        sm = UBX_SM_CKA;

      break;

    case UBX_SM_DATA:
      ubx_ck (c);

      if (ptr < UBX_BUF_LEN)
        buf[ptr] = c;

      ptr++;

      if (ptr == len)
        sm = UBX_SM_CKA;

      break;

    case UBX_SM_CKA:
      if (c == ck_a)
        sm = UBX_SM_CKB;
      else
        sm = UBX_SM_LOST;

      break;

    case UBX_SM_CKB:
      sm = UBX_SM_LOST;

      if (c != ck_b)
        break;

      ubx_recv (class, id, buf, len);

      if ((class == s_class) && (id == s_id)) {
        if (len_ptr)
          *len_ptr = len;

        return buf;
      }

      break;
    }

  }

  return NULL;
}
static void gps_pps_dispatch (void)
{
  //char buf[80];
  uint32_t now;
  uint64_t abs;
  int v;
  EPOCH e;
  //UTC u;

  if (gps_ring.rx_ptr == gps_ring.tx_ptr) return;

  v = gps_ring.events[gps_ring.rx_ptr].value;
  now = gps_ring.events[gps_ring.rx_ptr].when;

  gps_ring.rx_ptr = (gps_ring.rx_ptr + 1) & ERING_MASK;


  if (!v) return;

  abs = abs_extend (now);



  pll_dispatch (abs);

  e = pll_decompose (abs);


  //u = time_epoch_to_utc (e);
  time_print_epoch ("GPS  : ", e);

}


void
gps_dispatch (void)
{
  ubx_dispatch_search (-1, -1, NULL);
  gps_pps_dispatch();
}

void
ubx_send (uint8_t class, uint8_t id, const void *_payload, unsigned len)
{
  uint8_t ck_a = 0, ck_b = 0;
  uint8_t *payload = (uint8_t *) _payload;

  void ubx_send_byte_ck (uint8_t v) {
    ubx_send_byte (v);
    ck_a += v;
    ck_b = ck_b + ck_a;
  }


  ubx_send_byte (0xb5);
  ubx_send_byte (0x62);

  ubx_send_byte_ck (class);
  ubx_send_byte_ck (id);
  ubx_send_byte_ck (len & 0xff);
  ubx_send_byte_ck (len >> 8);

  while (len--)
    ubx_send_byte_ck (* (payload++));

  ubx_send_byte (ck_a);
  ubx_send_byte (ck_b);

  //  printf ("TX> %02x.%02x\r\n", class, id);

}

int
ubx_handshake (uint8_t class, uint8_t id, const void *payload, unsigned len)
{
  uint32_t timeout;

  ubx_ack = 0;
  ubx_send (class, id, payload, len);



  timeout = ticks + TIMEOUT;

  while (!ubx_ack) {

    if (ticks > timeout) {
      printf ("GPS timeout resending packet\r\n");
      usart3_drain();
      ubx_send (class, id, payload, len);
      timeout = ticks + TIMEOUT;
    }




    gps_dispatch();
  }

  return 0;
}

int
ubx_handshake_xfer (uint8_t class, uint8_t id, const void *payload,
                    unsigned len)
{
  uint32_t timeout;

  ubx_ack_xfer = 0;
  //  usart3_drain();
  ubx_send (class, id, payload, len);



  timeout = ticks + TIMEOUT;

  while (!ubx_ack_xfer) {

    if (ticks > timeout) {
      printf ("GPS timeout resending packet\r\n");
      //      usart3_drain();
      ubx_send (class, id, payload, len);
      timeout = ticks + TIMEOUT;
    }




    gps_dispatch();
  }

  return 0;
}




uint8_t *
ubx_fetch (uint8_t class, uint8_t id, void *payload, unsigned len,
           unsigned *len_ptr)
{
  uint8_t *ret;

  while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_ring))
    gps_dispatch();

  ubx_send (class, id, payload, len);

  while (! (ret = ubx_dispatch_search (class, id, len_ptr)));


  return ret;
}

int
ubx_set_message_rate_port1 (uint8_t class, uint8_t id, uint8_t rate)
{
  uint8_t buf[8], *ptr;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, class);
  ptr += ubx_put_u8 (ptr, id);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, rate); //rate on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  return ubx_handshake (0x06, 0x01, buf, (unsigned) (ptr - buf));
}


int
ubx_cfg_rst (uint16_t flags)
{
  uint8_t buf[8], *ptr;

  ptr = buf;

  ptr += ubx_put_u16 (ptr, flags); //Flags
  ptr += ubx_put_u8 (ptr, 0x4); //Hardware reset after shutdown
  ptr += ubx_put_u8 (ptr, 0);   //reserved

  ubx_send (0x06, 0x04, buf, (unsigned) (ptr - buf));
  return 0;
}


int
gps_set_ref (int ref_hz)
{
  uint8_t buf[80], *ptr;
  int ret;

  printf ("setting gps ref to %d hz\r\n", ref_hz);

  current_ref_hz = ref_hz;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0);   //timepluse 1
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0);  //reserved

  //  ptr += ubx_put_u16 (ptr, 32); //ant cable delay ns
  ptr += ubx_put_u16 (ptr, 0); //ant cable delay ns
  ptr += ubx_put_u16 (ptr, 0);  //rf group delay ns
  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period unlocked/us
  ptr += ubx_put_u32 (ptr, 1000000 / ref_hz); //period locked/us

  //  ptr += ubx_put_u32 (ptr, 0);  //pulse width unlocked/us
  ptr += ubx_put_u32 (ptr, 500000 / ref_hz); //pulse width unlocked/us
  ptr += ubx_put_u32 (ptr, 500000 / ref_hz); //pulse width locked/us

  ptr += ubx_put_i32 (ptr, 0);  // ? delay

#if 0
  /*Separate numbers for locked/unlocked*/
  ptr += ubx_put_u32 (ptr, 0xf7);
#else
  /*Same numbers for locked/unlocked*/
  ptr += ubx_put_u32 (ptr, 0xf3);
#endif


  ret = ubx_handshake (0x06, 0x31, buf, (unsigned) (ptr - buf));

  return ret;
}


int
gps_init (void)
{
  uint8_t buf[80], *ptr;
  unsigned len;
  //  uint16_t u2;

  usart3_drain();

  while (!ring_empty (&tx3_ring) || !ring_empty (&rx3_ring))
    gps_dispatch();


  printf ("Testing GNSS...\r\n");
  ubx_handshake (0x06, 0x00, NULL, 0);
  printf ("GNSS there\r\n");
  // Set up port
  ptr = buf;
  ptr += ubx_put_u8 (ptr, 1);   //uart1
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0x0); //flow control off
  ptr += ubx_put_u32 (ptr, 0x8c0); //no parity, 8 bits
  ptr += ubx_put_u32 (ptr, 9600); // baudrate
  ptr += ubx_put_u16 (ptr, 0x7); // receive RTCM, NMEA, UBX
  ptr += ubx_put_u16 (ptr, 0x1); // transmit UBX
  ptr += ubx_put_u16 (ptr, 0x0); // no txtimeout
  ptr += ubx_put_u16 (ptr, 0x0); // reserved
  ubx_handshake (0x06, 0x00, buf, (unsigned) (ptr - buf));

  printf ("configured GNSS protocol\r\n");

#if 1
  ptr = buf;
  ptr += ubx_put_u16 (ptr, 0x14);
  ptr += ubx_put_u16 (ptr, 0x00);
  ubx_handshake (0x06, 0x13, buf, (unsigned) (ptr - buf));

  printf ("configured antenna pins\r\n");
#endif



#if 0
  // Check we're in WGS84
  ptr = ubx_fetch (0x06, 0x06, NULL, 0, &len);
  ptr += ubx_get_u16 (ptr, &u2);

  if (u2)
    return -1;

  printf ("configured GNSS datum\r\n");
#endif

  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0);   //UBX
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, 0x18); //everything on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  ptr += ubx_put_u8 (ptr, 1);   //NMEA
  ptr += ubx_put_u8 (ptr, 0);   //reserved
  ptr += ubx_put_u16 (ptr, 0);  //reserved
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port i2c
  ptr += ubx_put_u8 (ptr, 0);   //nothing on uart
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 2
  ptr += ubx_put_u8 (ptr, 0);   //nothing on usb
  ptr += ubx_put_u8 (ptr, 0);   //nothing on spi
  ptr += ubx_put_u8 (ptr, 0);   //nothing on port 5

  ubx_handshake (0x06, 0x02, buf, (unsigned) (ptr - buf));

  printf ("configured GNSS data\r\n");

  ubx_fetch (0x06, 0x31, NULL, 0, &len);

  printf ("configured GNSS 6.31\r\n");

#if 1
  ubx_set_message_rate_port1 (0x01, 0x03, 1);
  ubx_set_message_rate_port1 (0x01, 0x21, 1);
  ubx_set_message_rate_port1 (0x01, 0x22, 1);
  ubx_set_message_rate_port1 (0x01, 0x32, 0);
  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
  //ubx_set_message_rate_port1 (0x03, 0x10, 0);
  printf ("GNSS ready\r\n");
#else
  ubx_set_message_rate_port1 (0x01, 0x03, 0);
  ubx_set_message_rate_port1 (0x01, 0x21, 0);
  ubx_set_message_rate_port1 (0x01, 0x22, 0);
  ubx_set_message_rate_port1 (0x01, 0x32, 0);
  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
  //ubx_set_message_rate_port1 (0x03, 0x10, 0);
  printf ("GNSS ready\r\n");
#endif

#if 1
  ptr = buf;
  ptr += ubx_put_u8 (ptr, 0x1);
  ptr += ubx_put_u8 (ptr, 0x7);
  ptr += ubx_put_u8 (ptr, 0x1);
  ptr += ubx_put_u8 (ptr, 0xf);
  ptr += ubx_put_u32 (ptr, 0xffffffff);
  ubx_handshake (0x06, 0x16, buf, (unsigned) (ptr - buf));

  printf ("configured SBAS\r\n");

  ubx_handshake (0x06, 0x16, buf, 0);
#endif


  //  printf ("GPS ready\r\n");
  //  ubx_get_clock_stats();

  MAP_INPUT (PPS);

  exti_select_source (EXTI2, PPS_PORT);
  exti_set_trigger (EXTI2, EXTI_TRIGGER_BOTH);
  exti_enable_request (EXTI2);
  nvic_enable_irq (NVIC_EXTI2_IRQ);

  return 0;
}


#define ALMANAC_LUMP 128

int
gps_almanac (void)
{
  uint32_t len = almanac_alp_len;
  const uint8_t *ptr = almanac_alp;
  uint32_t lumpsz;
  uint8_t dummy;

  printf ("Downloading GPS almanac %x %x %x\r\n", (int) ptr[0], (int) ptr[1],
          (int) ptr[2]);


  while (len) {
    printf ("%d bytes to go\r\n", (int) len);

    lumpsz = len > ALMANAC_LUMP ? ALMANAC_LUMP : len;

    ubx_handshake_xfer (0x0b, 0x50, ptr, lumpsz);

    ptr += lumpsz;
    len -= lumpsz;

  }

  ubx_handshake_xfer (0x0b, 0x50, &dummy, 1);

  return 0;
}



void
gps_reset (void)
{
  printf ("Restting gps..\r\n");

#define REPHEMERIDIES (1UL << 0)
#define RALMANAC  (1UL << 1)
#define RPOS    (1UL << 4)
#define RRTC    (1UL << 8)

  ubx_cfg_rst (REPHEMERIDIES | RALMANAC | RPOS | RRTC);

  delay_ms (1000);
  usart3_drain();


  printf ("Testing GNSS...\r\n");
  ubx_handshake (0x06, 0x00, NULL, 0);
  printf ("GNSS there\r\n");

  gps_init();

  gps_set_ref (current_ref_hz);

}

int
ubx_get_clock_stats (void)
{
  uint8_t *ptr;

  unsigned len;

  ptr = ubx_fetch (0x01, 0x22, NULL, 0, &len);

  //return ubx_recv_clock_stats(ptr,len);
  return ptr ? 0 : -1;
}