--  GHDL Run Time (GRT) -  Run Time Informations.
--  Copyright (C) 2002 - 2020 Tristan Gingold
--
--  This program is free software: you can redistribute it and/or modify
--  it under the terms of the GNU General Public License as published by
--  the Free Software Foundation, either version 2 of the License, or
--  (at your option) any later version.
--
--  This program is distributed in the hope that it will be useful,
--  but WITHOUT ANY WARRANTY; without even the implied warranty of
--  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--  GNU General Public License for more details.
--
--  You should have received a copy of the GNU General Public License
--  along with this program.  If not, see <gnu.org/licenses>.
--
--  As a special exception, if other files instantiate generics from this
--  unit, or you link this unit with other files to produce an executable,
--  this unit does not by itself cause the resulting executable to be
--  covered by the GNU General Public License. This exception does not
--  however invalidate any other reasons why the executable file might be
--  covered by the GNU Public License.
with System; use System;
with Grt.Types; use Grt.Types;
with Ada.Unchecked_Conversion;

package Grt.Rtis is
   pragma Preelaborate (Grt.Rtis);

   --  To keep in sync with:
   --   * trans-rtis.ads
   --   * grt.disp_rti.Disp_Kind
   type Ghdl_Rtik is
     (Ghdl_Rtik_Top,
      Ghdl_Rtik_Library,        -- use scalar
      Ghdl_Rtik_Package,
      Ghdl_Rtik_Package_Body,
      Ghdl_Rtik_Entity,

      Ghdl_Rtik_Architecture,
      Ghdl_Rtik_Process,
      Ghdl_Rtik_Block,
      Ghdl_Rtik_If_Generate,
      Ghdl_Rtik_Case_Generate,

      Ghdl_Rtik_For_Generate,   -- 10
      Ghdl_Rtik_Generate_Body,
      Ghdl_Rtik_Instance,
      Ghdl_Rtik_Constant,
      Ghdl_Rtik_Iterator,

      Ghdl_Rtik_Variable,
      Ghdl_Rtik_Signal,
      Ghdl_Rtik_File,
      Ghdl_Rtik_Port,
      Ghdl_Rtik_Generic,

      Ghdl_Rtik_Alias,          -- 20
      Ghdl_Rtik_Guard,
      Ghdl_Rtik_Component,
      Ghdl_Rtik_Attribute,
      Ghdl_Rtik_Type_B1,                      --  Rtin_Type_Enum

      Ghdl_Rtik_Type_E8,                      --  Rtin_Type_Enum
      Ghdl_Rtik_Type_E32,
      Ghdl_Rtik_Type_I32,                     --  Rtin_Type_Scalar
      Ghdl_Rtik_Type_I64,                     --  Rtin_Type_Scalar
      Ghdl_Rtik_Type_F64,                     --  Rtin_Type_Scalar

      Ghdl_Rtik_Type_P32,       -- 30         --  Rtin_Type_Physical
      Ghdl_Rtik_Type_P64,                     --  Rtin_Type_Physical
      Ghdl_Rtik_Type_Access,                  --  Rtin_Type_Fileacc
      Ghdl_Rtik_Type_Array,                   --  Rtin_Type_Array
      Ghdl_Rtik_Type_Record,                  --  Rtin_Type_Record

      Ghdl_Rtik_Type_Unbounded_Record,        --  Rtin_Type_Record
      Ghdl_Rtik_Type_File,                    --  Rtin_Type_Fileacc
      Ghdl_Rtik_Subtype_Scalar,               --  Rtin_Subtype_Scalar
      Ghdl_Rtik_Subtype_Array,                --  Rtin_Subtype_Composite
      Ghdl_Rtik_Subtype_Unbounded_Array,

      Ghdl_Rtik_Subtype_Record, -- 40         --  Rtin_Subtype_Composite
      Ghdl_Rtik_Subtype_Unbounded_Record,
      Ghdl_Rtik_Subtype_Access,               --  Rtin_Type_Fileacc
      Ghdl_Rtik_Type_Protected,
      Ghdl_Rtik_Element,

      Ghdl_Rtik_Unit64,
      Ghdl_Rtik_Unitptr,
      Ghdl_Rtik_Attribute_Transaction,
      Ghdl_Rtik_Attribute_Quiet,
      Ghdl_Rtik_Attribute_Stable,

      Ghdl_Rtik_Psl_Assert,
      Ghdl_Rtik_Psl_Assume,
      Ghdl_Rtik_Psl_Cover,
      Ghdl_Rtik_Psl_Endpoint,

      Ghdl_Rtik_Error);
   for Ghdl_Rtik'Size use 8;

   subtype Ghdl_Rtiks_Psl is
     Ghdl_Rtik range Ghdl_Rtik_Psl_Assert .. Ghdl_Rtik_Psl_Cover;

   type Ghdl_Rti_Depth is range 0 .. 255;
   for Ghdl_Rti_Depth'Size use 8;

   type Ghdl_Rti_U8 is mod 2 ** 8;
   for Ghdl_Rti_U8'Size use 8;

   --  This structure is common to all RTI nodes.
   type Ghdl_Rti_Common is record
      --  Kind of the RTI, list is above.
      Kind : Ghdl_Rtik;

      Depth : Ghdl_Rti_Depth;

      --  * array types and subtypes, record types, protected types:
      --    bit 0: set for complex type
      --    bit 1: set for anonymous type definition
      --    bit 2: set only for physical type with non-static units (time)
      --  * record elements:
      --    bit 0: set for complex type (copy of the type complex bit).
      --  * signals:
      --    bit 0-3: mode (1: linkage, 2: buffer, 3 : out, 4 : inout, 5: in)
      --    bit 4-5: kind (0 : none, 1 : register, 2 : bus)
      --    bit 6: set if has 'active attributes
      Mode : Ghdl_Rti_U8;

      --  * Types and subtypes definition:
      --    maximum depth of all RTIs referenced.
      --  * Others:
      --    0
      Max_Depth : Ghdl_Rti_Depth;
   end record;
   pragma Convention (C, Ghdl_Rti_Common);

   type Ghdl_Rti_Access is access all Ghdl_Rti_Common;

   --  Fat array of rti accesses.
   type Ghdl_Rti_Array is array (Ghdl_Index_Type) of Ghdl_Rti_Access;
   type Ghdl_Rti_Arr_Acc is access Ghdl_Rti_Array;

   subtype Ghdl_Rti_Loc is Integer_Address;
   Null_Rti_Loc : constant Ghdl_Rti_Loc := 0;

   type Ghdl_C_String_Array is array (Ghdl_Index_Type) of Ghdl_C_String;
   type Ghdl_C_String_Array_Ptr is access Ghdl_C_String_Array;

   type Ghdl_Rtin_Block is record
      Common : Ghdl_Rti_Common;
      Name : Ghdl_C_String;
      --  Address of the instantiation data relative to the parent's
      --  instantation data address.
      Loc : Ghdl_Rti_Loc;
      --  Line and column of the declaration.
      Linecol : Ghdl_Index_Type;
      Parent : Ghdl_Rti_Access;
      Nbr_Child : Ghdl_Index_Type;
      Children : Ghdl_Rti_Arr_Acc;
   end record;
   pragma Convention (C, Ghdl_Rtin_Block);
   type Ghdl_Rtin_Block_Acc is access Ghdl_Rtin_Block;
   function To_Ghdl_Rtin_Block_Acc is new Ada.Unchecked_Conversion
     (Source => Ghdl_Rti_Access, Target => Ghdl_Rtin_Block_Acc);
   function To_Ghdl_Rti_Access is new Ada.Unchecked_Conversion
     (Source => Ghdl_Rtin_Block_Acc, Target => Ghdl_Rti_Access);

   type Ghdl_Rtin_Generate is record
      Common : Ghdl_Rti_Common;
      Name : Ghdl_C_String;
      --  Address of the instantiation data relative to the parent's
      --  instantation data address.
      Loc : Ghdl_Rti_Loc;
      --  Line and column of the declaration.
      Linecol : Ghdl_Index_Type;
      Parent : Ghdl_Rti_Access;
      --  The total size of the instantiation data for the contents of
      --  the generate statement. Useful to find the instantation data
      --  for a particular index of the loop. Only used for
      --  for_generate_statement.
      Size : Ghdl_Index_Type;
      Child : Ghdl_Rti_Access;
   end record;
   pragma Convention (C, Ghdl_Rtin_Generate);
   type Ghdl_Rtin_Generate_Acc is access Ghdl_Rtin_Generate;
   function To_Ghdl_Rtin_Generate_Acc is new Ada.Unchecked_Conversion
     (Source => Ghdl_Rti_Access, Target => Ghdl_Rtin_Generate_Ac
#include "project.h"

#define PPS (GPIO9)
#define PPS_PORT GPIOC

#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;

static int gps_locked;
static int gps_happy;

uint64_t gps_last_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;

static char gps_info[60];


void exti9_5_isr (void)
{
  uint32_t now = HW_CLOCK_REG;
  int v;

  v = !!gpio_get (PPS_PORT, PPS);

  nvic_disable_irq (NVIC_EXTI9_5_IRQ);
  exti_reset_request (EXTI9);

  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_EXTI9_5_IRQ);

}



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

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

#define ALMANAC_SIZE

static int ubx_recv_almanac(uint8_t *ptr,unsigned len)
{
uint32_t sv,week;
uint32_t almanac[8];

ptr+=ubx_get_u32(ptr,&sv);
len-=4;
ptr+=ubx_get_u32(ptr,&week);
len-=4;

if (len>sizeof(almanac))
	len=sizeof(almanac);

bzero(almanac,sizeof(almanac));
memcpy(almanac,ptr,len);

printf("    Almanac: %2d %d %06x %06x %06x %06x  %06x %06x %06x %06x\r\n",
	(int) sv,(int) week, 
	(unsigned) almanac[0],
	(unsigned) almanac[1],
	(unsigned) almanac[2],
	(unsigned) almanac[3],
	(unsigned) almanac[4],
	(unsigned) almanac[5],
	(unsigned) almanac[6],
	(unsigned) almanac[7]);

return 0;
}

static int
ubx_recv_nav_status (uint8_t *ptr, unsigned len)
{
  uint8_t gps_fix, flags,fix_stat,flags2;
  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
  ptr += ubx_get_u8 (ptr, &fix_stat);
  ptr += ubx_get_u8 (ptr, &flags2); 


  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,valid;
  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);
  ptr += ubx_get_u8 (ptr, &valid);

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

   sprintf(gps_info, " %04d-%02d-%02d %02d:%02d:%02d V:%02x Fix:%c%c TXCO  %+8dE-12",          

	  (int )year,
          (int )month,
          (int )day,
          (int )hour,
          (int )min,
          (int )sec,
(unsigned) valid, 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 = HW_CLOCK_REG;
    abs = ref_extend (now);

    gps_last_happy = make_happy (abs, 180);

    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_nav_sbas(uint8_t *ptr,unsigned len)
{
uint8_t prn,mode,service;
int8_t sys;
uint8_t  n;

  ptr+=4;

  ptr += ubx_get_u8 (ptr, &prn); 
  ptr += ubx_get_u8 (ptr, &mode); 
  ptr += ubx_get_i8 (ptr, &sys);
  ptr += ubx_get_u8 (ptr, &service); 

  ptr += ubx_get_u8 (ptr, &n); 
  ptr+=3;

  printf("GPS SBAS PRN:%d M:%d S:%d SVC:%02x\r\n",prn,mode,sys,service);

  while (n--) {
  ptr += ubx_get_u8 (ptr, &prn); 
  ptr+=11;
  printf("GPS SBAS     SV:%d\r\n",prn);
  }

return 0;
}

static int ubx_recv_nav_sat(uint8_t *ptr,unsigned len)
{
uint8_t gnssid,prn,cno,ver;
uint8_t  n;

  ptr+=4;

  ptr += ubx_get_u8 (ptr, &ver); 
  ptr += ubx_get_u8 (ptr, &n); 
  ptr+=2;

  while (n--) {
  ptr += ubx_get_u8 (ptr, &gnssid); 
  ptr += ubx_get_u8 (ptr, &prn); 
  ptr += ubx_get_u8 (ptr, &cno); 
  ptr+=9;

  printf("GPS    GNSS:%2d PRN:%3d CNO:%3d\r\n",gnssid,prn,cno);
  }


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;

    case 0x32:
      ubx_recv_nav_sbas(payload,len);
	break;

    case 0x35:
      ubx_recv_nav_sat(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 4:
    switch (id) {
    case 0x4:
      break;
	default:
    printf ("RX> %02x.%02x (%d bytes)\r\n", class, id, len);
      hexdump (payload, len);
    }

    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 0x30:
      ubx_recv_almanac(payload,len);
      break;
    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;

  led3_set(v);

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


  if (!v) return;

  abs = ref_extend (now);

  if (gps_happy)
    pll_dispatch (gps_last_happy, abs, "GPS");

  e = ref_decompose (abs);


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

}


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

}

static 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);

}

static 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");
      usart1_drain();
      ubx_send (class, id, payload, len);
      timeout = ticks + TIMEOUT;
    }




    gps_dispatch();
  }

  return 0;
}

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

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



  timeout = ticks + TIMEOUT;

  while (!ubx_ack_xfer) {

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




    gps_dispatch();
  }

  return 0;
}




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

  while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring))
    gps_dispatch();

  ubx_send (class, id, payload, len);

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


  return ret;
}

static int ubx_set_nav_rate (uint16_t ms)
{
  uint8_t buf[6], *ptr;

  ptr = buf;

  ptr += ubx_put_u16 (ptr, ms);
  ptr += ubx_put_u16 (ptr, 1); /*1:1*/
  ptr += ubx_put_u16 (ptr, 0); /*UTC*/

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

static 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));
}


static 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;
}


static 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);   //version 0
  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, 100000 / ref_hz); //pulse width unlocked/us
  ptr += ubx_put_u32 (ptr, 100000 / 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, 0xf7);
#endif


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

  return ret;
}


static inline int
ubx_put_gnss_cfg (uint8_t *buf, uint8_t gnss_id,uint8_t r_chan,uint8_t m_chan,
uint16_t enable, uint16_t sig_mask)
{
  uint8_t *ptr=buf;

  ptr += ubx_put_u8(ptr,gnss_id);
  ptr += ubx_put_u8(ptr,r_chan);
  ptr += ubx_put_u8(ptr,m_chan);
  ptr += ubx_put_u8(ptr,0);
  ptr += ubx_put_u16(ptr,enable);
  ptr += ubx_put_u16(ptr,sig_mask);

  return (unsigned) (ptr - buf);
}

void gps_set_gnss(void)
{
  uint8_t buf[80], *ptr;

  ptr = buf;
  ptr += ubx_put_u8 (ptr, 0x0); /* Ver 0 */
  ptr += ubx_put_u8 (ptr, 0x20); 
  ptr += ubx_put_u8 (ptr, 0x20); 
  ptr += ubx_put_u8 (ptr, 0x7); 

  ptr += ubx_put_gnss_cfg(ptr,0,8,16,1,0x101);
  ptr += ubx_put_gnss_cfg(ptr,1,1,3,1,0x101);
  ptr += ubx_put_gnss_cfg(ptr,2,4,8,0,0x101);
  ptr += ubx_put_gnss_cfg(ptr,3,8,16,0,0x101);
  ptr += ubx_put_gnss_cfg(ptr,4,0,7,0,0x301);
  ptr += ubx_put_gnss_cfg(ptr,5,0,3,1,0x501);
  ptr += ubx_put_gnss_cfg(ptr,6,8,14,1,0x101);

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

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

  ubx_handshake (0x06, 0x3e, buf, 0);
}


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

  usart1_drain();

  while (!ring_empty (&usart1_tx_ring) || !ring_empty (&usart1_rx_ring))
    gps_dispatch();


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

  //reset GNSS
  
#if 0
  ptr=buf;
  ptr += ubx_put_u32 (ptr, 0xe1f );   //uart1
  ptr += ubx_put_u32 (ptr, 0 );   //uart1
  ptr += ubx_put_u32 (ptr, 0 );   //uart1
  ubx_handshake (0x06, 0x09, buf, (unsigned) (ptr - buf));

  printf ("reset\r\n");
#endif

 
  // 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 0
  ptr = buf;
  ptr += ubx_put_u16 (ptr, 0x1b);
  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");

  gps_set_ref (1);

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

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


  ubx_set_nav_rate(1000);

#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 (0x01, 0x32, 0); /*SBAS*/
  ubx_set_message_rate_port1 (0x01, 0x35, 0); /*SV in view*/

  ubx_set_message_rate_port1 (0x03, 0x0a, 0);
  ubx_set_message_rate_port1 (0x03, 0x10, 0);
  ubx_set_message_rate_port1 (0x03, 0x21, 0);
  ubx_set_message_rate_port1 (0x04, 0x04, 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



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

  MAP_INPUT (PPS);

  exti_select_source (EXTI9, PPS_PORT);
  exti_set_trigger (EXTI9, EXTI_TRIGGER_BOTH);
  exti_enable_request (EXTI9);
  nvic_enable_irq (NVIC_EXTI9_5_IRQ);

  return 0;
}


#define ALMANAC_LUMP 64

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_dump_almanac(void)
{
  ubx_send (0xb, 0x30, NULL,0);
}


#if 0
int gps_bs(void)
{
  uint32_t now = HW_CLOCK_REG;
  uint64_t abs = ref_extend (now);
  EPOCH e = ref_decompose (abs);
  UTC u = time_epoch_to_utc (e);

  uint8_t buf[80], *ptr;
  int ret;

  ptr = buf;

  ptr += ubx_put_u32 (ptr, 391706007); /*19 sleaford st*/
  ptr += ubx_put_u32 (ptr, 955140);
  ptr += ubx_put_u32 (ptr, 501672842);
  ptr += ubx_put_u32 (ptr, 100000); /*1km std dev*/

  ptr += ubx_put_u16 (ptr, 0); /*no time mark*/

  ptr += ubx_put_u16 (ptr, (u.year-2000)*100 + u.month); 
  ptr += ubx_put_u32 (ptr, (u.mday*1000000) + (u.hour*10000) + (u.minute *100) +u.second);
  ptr += ubx_put_u32 (ptr, u.nanosecond);

  ptr += ubx_put_u32 (ptr, 2000); /* time good to 2s */
  ptr += ubx_put_u32 (ptr, 0); 

  ptr += ubx_put_u32 (ptr, 0); 
  ptr += ubx_put_u32 (ptr, 0); 

  ptr += ubx_put_u32 (ptr, 0x403); 

  printf("Bootstrapping GPS\r\n");

  hexdump (buf, (unsigned) (ptr-buf));

 ubx_send (0x0b, 0x01, buf, (unsigned) (ptr - buf));


  ret = 0;


/*
 00000: 97 f5 58 17 
        04 93 0e 00  
        8a eb e6 1d 
        a0 86 01 00 
 00010: 00 00 
        6f 07  		19/03
        03 cb 06 01  	/17 22:24:03
        65 ca af 02 	45075045ns

	d0 07 00 00 	2000ms
 00020: 00 00 00 00     0ns
        00 00 00 00  
        00 00 00 00 
        03 04 00 00 
*/

  return ret;
}

#endif

int gps_bs(void)
{
  uint32_t now = HW_CLOCK_REG;
  uint64_t abs = ref_extend (now);
  EPOCH e = ref_decompose (abs);
  UTC u = time_epoch_to_utc (e);

  uint8_t buf[80], *ptr;
  int ret;

  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0x0);
  ptr += ubx_put_u8 (ptr, 0x0);
  ptr += ubx_put_u8 (ptr, 0x0);
  ptr += ubx_put_u8 (ptr, 0x0);

  ptr += ubx_put_u32 (ptr, 391706007); /*19 sleaford st*/
  ptr += ubx_put_u32 (ptr, 955140);
  ptr += ubx_put_u32 (ptr, 501672842);
  ptr += ubx_put_u32 (ptr, 100000); /*1km std dev*/

  hexdump (buf, (unsigned) (ptr-buf));
  ubx_send (0x13, 0x40, buf, (unsigned) (ptr-buf));


  ptr = buf;

  ptr += ubx_put_u8 (ptr, 0x10);
  ptr += ubx_put_u8 (ptr, 0x0);
  ptr += ubx_put_u8 (ptr, 0x0);
  ptr += ubx_put_u8 (ptr, 0x80);

  ptr += ubx_put_u16 (ptr,u.year-2000);
  ptr += ubx_put_u8 (ptr,u.month);
  ptr += ubx_put_u8 (ptr,u.mday);

  ptr += ubx_put_u8 (ptr,u.hour);
  ptr += ubx_put_u8 (ptr,u.minute);
  ptr += ubx_put_u8 (ptr,u.second);
  ptr += ubx_put_u8 (ptr,0);

  ptr += ubx_put_u32 (ptr,u.nanosecond);

  ptr += ubx_put_u16 (ptr, 2); /* time good to 2s */
  ptr += ubx_put_u8 (ptr,0);
  ptr += ubx_put_u8 (ptr,0);

  ptr += ubx_put_u32 (ptr, 0); 

  hexdump (buf, (unsigned) (ptr-buf));
  ubx_send (0x13, 0x40, buf, (unsigned) (ptr-buf));


  ret = 0;

  return ret;
}


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);
  usart1_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);

  gps_set_gnss();
}

#if 0
static 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;
}
#endif