#include <ezusb.h>
#include <ezregs.h>

#include "type.h"

#include "gps.h"
#include "led.h"
#include "timer.h"

static void checksum(unsigned char *ck_a, unsigned char *ck_b, unsigned char *packet, int size)
{
  u32 a, b;
  a = b = 0x00;
  while(size-- > 0){
    a += *(packet++);
    b += a;
  }
  *ck_a = a & 0xFF;
  *ck_b = b & 0xFF;
}

#define swap(x) (\
(((x) & 0x80) ? 0x01 : 0x00) | \
(((x) & 0x40) ? 0x02 : 0x00) | \
(((x) & 0x20) ? 0x04 : 0x00) | \
(((x) & 0x10) ? 0x08 : 0x00) | \
(((x) & 0x08) ? 0x10 : 0x00) | \
(((x) & 0x04) ? 0x20 : 0x00) | \
(((x) & 0x02) ? 0x40 : 0x00) | \
(((x) & 0x01) ? 0x80 : 0x00) \
)

static void com1_write(unsigned char *packet, int size)
{
 
  int i;
  //割り込みを使用せずに送信するルーチン、9600bps用
  while(size-- > 0){
    SBUF1 = *(packet++);
    for(i = 0; i < 680; i++);
    //while(!TI);
    //TI = 0;
  }
}

#define UBX_CFG_RATE_PS   (u16)6
#define UBX_CFG_RATE_MEAS (u16)250
#define UBX_CFG_RATE_NAV  (u16)1
#define UBX_CFG_RAET_TIME (u16)0

static void set_ubx_cfg_rate()
{
  unsigned char packet[14];
  packet[0] = 0xB5;
  packet[1] = 0x62;
  packet[2] = 0x06;
  packet[3] = 0x08;
  packet[4 + 0]  = (UBX_CFG_RATE_PS & 0xFF);
  packet[4 + 1]  = (UBX_CFG_RATE_PS >> 8) & 0xFF;
  packet[6 + 0]  = (UBX_CFG_RATE_MEAS & 0xFF);
  packet[6 + 1]  = (UBX_CFG_RATE_MEAS >> 8) & 0xFF;
  packet[6 + 2]  = (UBX_CFG_RATE_NAV & 0xFF);
  packet[6 + 3]  = (UBX_CFG_RATE_NAV >> 8) & 0xFF;
  packet[6 + 4]  = (UBX_CFG_RAET_TIME & 0xFF);
  packet[6 + 5]  = (UBX_CFG_RAET_TIME >> 8) & 0xFF;
  checksum(&(packet[sizeof(packet) - 2]), &(packet[sizeof(packet) - 1]), &(packet[2]), sizeof(packet) - 4);
  com1_write(packet, sizeof(packet));
}

#define UBX_CFG_TP_PS          (u16)20
#define UBX_CFG_TP_INTERVAL    (u32)1000000
#define UBX_CFG_TP_LENGTH      (u32)1000
#define UBX_CFG_TP_STATUS      (s8)1
#define UBX_CFG_TP_TIMEREF     (u8)0
#define UBX_CFG_TP_CABLE_DELAY (u16)50
#define UBX_CFG_TP_RF_DELAY    (u16)820
#define UBX_CFG_TP_USER_DELAY  (u32)0

static void set_ubx_cfg_tp()
{
  unsigned char packet[28];
  packet[0] = 0xB5;
  packet[1] = 0x62;
  packet[2] = 0x06;
  packet[3] = 0x07;
  packet[4 + 0]  = (UBX_CFG_TP_PS & 0xFF);
  packet[4 + 1]  = (UBX_CFG_TP_PS >> 8) & 0xFF;
  packet[6 + 0]  = (UBX_CFG_TP_INTERVAL & 0xFF);
  packet[6 + 1]  = (UBX_CFG_TP_INTERVAL >> 8) & 0xFF;
  packet[6 + 2]  = (UBX_CFG_TP_INTERVAL >> 16) & 0xFF;
  packet[6 + 3]  = (UBX_CFG_TP_INTERVAL >> 24) & 0xFF;
  packet[6 + 4]  = (UBX_CFG_TP_LENGTH & 0xFF);
  packet[6 + 5]  = (UBX_CFG_TP_LENGTH >> 8) & 0xFF;
  packet[6 + 6]  = (UBX_CFG_TP_LENGTH >> 16) & 0xFF;
  packet[6 + 7]  = (UBX_CFG_TP_LENGTH >> 24) & 0xFF;
  packet[6 + 8]  = (UBX_CFG_TP_STATUS & 0xFF);
  packet[6 + 9]  = (UBX_CFG_TP_TIMEREF & 0xFF);
  packet[6 + 10] = 0x00;
  packet[6 + 11] = 0x00;
  packet[6 + 12] = (UBX_CFG_TP_CABLE_DELAY & 0xFF);
  packet[6 + 13] = (UBX_CFG_TP_CABLE_DELAY >> 8) & 0xFF;
  packet[6 + 14] = (UBX_CFG_TP_RF_DELAY & 0xFF);
  packet[6 + 15] = (UBX_CFG_TP_RF_DELAY >> 8) & 0xFF;
  packet[6 + 16] = (UBX_CFG_TP_USER_DELAY & 0xFF);
  packet[6 + 17] = (UBX_CFG_TP_USER_DELAY >> 8) & 0xFF;
  packet[6 + 18] = (UBX_CFG_TP_USER_DELAY >> 16) & 0xFF;
  packet[6 + 19] = (UBX_CFG_TP_USER_DELAY >> 24) & 0xFF;
  checksum(&(packet[sizeof(packet) - 2]), &(packet[sizeof(packet) - 1]), &(packet[2]), sizeof(packet) - 4);
  com1_write(packet, sizeof(packet));
}

#define UBX_CFG_MSG_PS    (u16)6

static void set_ubx_cfg_msg(u8 class, u8 id, u8 rate1, u8 rate2)
{
  unsigned char packet[14];
  packet[0] = 0xB5;
  packet[1] = 0x62;
  packet[2] = 0x06;
  packet[3] = 0x01;
  packet[4 + 0]  = (UBX_CFG_MSG_PS & 0xFF);
  packet[4 + 1]  = (UBX_CFG_MSG_PS >> 8) & 0xFF;
  packet[6 + 0]  = class;
  packet[6 + 1]  = id;
  packet[6 + 2]  = (u8)0;
  packet[6 + 3]  = rate1;
  packet[6 + 4]  = rate2;
  packet[6 + 5]  = (u8)0;
  checksum(&(packet[sizeof(packet) - 2]), &(packet[sizeof(packet) - 1]), &(packet[2]), sizeof(packet) - 4);
  com1_write(packet, sizeof(packet));
}

void gps_init(void)
{
	// uart0 application specific SIO-0 register initialization
	PORTCCFG |= 0x03; // using UART0, TxD0 & RxD0 pins
	//RCAP2H = TH2 = 0xFF; // auto-reload for 19200 baud
	//RCAP2L = TL2 = 0xD9; // auto-reload for 19200 baud
  //SMOD0 = 1;  // baud rate doubler on. 19200 -> 38400
  RCAP2H = TH2 = 0xFF; // auto-reload for 38400 baud
  RCAP2L = TL2 = 0xEC; // auto-reload for 38400 baud
	T2CON = 0x34; // clk overflow, start timer2 ticks
	SCON0 |= 0x53; // mode1, rx enable, set TI0
  
  // uart1 application specific SIO-1 register initialization
  PORTBCFG |= 0x0C; // using UART1, TxD1 & RxD1 pins
  SMOD1 = 1;
  TL1 = TH1 = 0xD9; // auto-reload for 9600 baud
  CKCON |= 0x10; // Timer 1 using CLK 24/4
  TMOD &= 0x0F; // clear Timer 1 mode bits
  TMOD |= 0x20; // Timer1 => 8bit with auto-reload
  TR1 = 1;
  SCON1 |= 0x53; // mode1, rx enable, set TI1

  // set U-blox configuration
  set_ubx_cfg_rate();
  set_ubx_cfg_tp();
  set_ubx_cfg_msg(0x01, 0x01, 0, 0);  // NAV-POSECEF
  set_ubx_cfg_msg(0x01, 0x11, 0, 0);  // NAV-VELECEF
  set_ubx_cfg_msg(0x01, 0x20, 0, 0);  // NAV-TIMEGPS
  set_ubx_cfg_msg(0x01, 0x21, 0, 0);  // NAV-TIMEUTC
  set_ubx_cfg_msg(0x01, 0x22, 0, 0);  // NAV-CLOCK
  set_ubx_cfg_msg(0x02, 0x10, 0, 1);  // RXM-RAW
  set_ubx_cfg_msg(0x02, 0x11, 0, 1);  // RXM-SFRB
  
  PORTCCFG |= 0x08; // using INT1
  IT1 = 1; // INT1 edge sense

	// Interrupt config
  PS0 = 1; // INT_COM0 is higher priority
  ES0 = 1; // INT_COM0 enable
  //PS1 = 1; // INT_COM1 is higher priority
  //ES1 = 1; // INT_COM1 enable
  PX1 = 1;
  EX1 = 1; // INT1 enable
}

u8 gps_ms_modified = FALSE;
static s32 gps_ms = 0;

void ISR_EXTR1(void) interrupt INT1_VECT
{
  led0_toggle();
  if(gps_ms_modified){
    global_ms = ((gps_ms / 1000) + 1) * 1000;
    gps_ms_modified = FALSE;
  }
}

void ISR_COM0(void) interrupt COM0_VECT
{
  static u8 ep_in4_index = 0;
  static u8 raw_packet_index = 0;
 
  if(TI){TI = 0;}
  
	if(RI){
    u8 temp = SBUF0;
    RI = 0;
    
    if(temp == 0xB5){raw_packet_index = 1;}
    else if(raw_packet_index > 0){
      switch(raw_packet_index){
        case 1: raw_packet_index = (temp == 0x62) ? (raw_packet_index + 1) : 0; break;
        case 2: raw_packet_index = (temp == 0x02) ? (raw_packet_index + 1) : 0; break;
        case 3:
          if(temp != 0x10){raw_packet_index = 0;}
          else{raw_packet_index++;}
          break;
        default:
          switch(++raw_packet_index){
            case 10:
            case 9:
            case 8:
              gps_ms += ((s32)temp << ((raw_packet_index - 7) * 8));
              if(raw_packet_index == 10){
                gps_ms_modified = TRUE;
                raw_packet_index = 0;
              }
              break;
            case 7:
              gps_ms = temp;
              break;
          }
      }  
    }
    
    // packetの整理はPC側で行うこと
    if( !(EPIO[IN4BUF_ID].cntrl & bmEPBUSY) ){  // Is the IN4BUF available,
      IN4BUF[ep_in4_index++] = temp;
      if(ep_in4_index == 64){
        EPIO[IN4BUF_ID].bytes = ep_in4_index;
        ep_in4_index = 0;
      }
    }else{led0_toggle();}
  }
}

void ISR_COM1(void) interrupt COM1_VECT
{
  if(TI1){TI1 = 0;}
  
  if(RI1){
    /*
    u8 temp = SBUF1;
    RI1 = 0;
    if(temp == '$'){packet_index = 1;}
    else if(packet_index > 0){
      switch(packet_index){
        case 1: packet_index = (temp == 'G') ? (packet_index + 1) : 0; break;
        case 2: packet_index = (temp == 'P') ? (packet_index + 1) : 0; break;
        case 3: packet_index = (temp == 'G') ? (packet_index + 1) : 0; break;
        case 4: packet_index = (temp == 'G') ? (packet_index + 1) : 0; break;
        case 5:
          if(temp != 'A'){packet_index = 0;}
          else{
            led0_toggle();
            packet_index++;
          }
          break;
        default:
          packet_index++;
          if( !(EPIO[IN4BUF_ID].cntrl & bmEPBUSY) ){  // Is the IN4BUF available,
            IN4BUF[ep_in4_index++] = temp;
            if(temp == '\n' || ep_in4_index == 64){
              EPIO[IN4BUF_ID].bytes = ep_in4_index;
              ep_in4_index = 0;
            }
          }
      }  
    }
    */
  }
}