#include "common.h"
#include "sci.h"
#include "gps.h"
#include <string.h>
#include <stdlib.h>

/************************************************************************/
/*     GPS Data Receiver [GH-80] Library for H8/3048                    */
/*      OUTPUT FORMAT: FURUNO BINARY                                    */
/*           Written by   Pongsatorn  2004.07.13                        */
/************************************************************************/
   
/**
 * Initialize GPS Receiver
 * GPSを初期化します。
 */
void gps_init(){
 
  char b[5];  //コマンド5Byte 
    
  b[0] = 0xC0; //ヘッダー 
  b[1] = 0xA3; //設定コマンドを設定、確認要求無し 
  b[2] = 0x8B; //位置、時刻、誤差データを出力する 
  b[3] = 0xE8; //垂直パリティ 
  b[4] = 0xCA; //ターミネータ 
    
  sci2_write(b, sizeof(b));
}

static inline long LONG_2_long(char c1, char c2, char c3, char c4){
  long result = c1;
  result <<= 7; result |= c2;
  result <<= 7; result |= c3;
  result <<= 7; result |= c4;
  if(result & (1 << 27)) result |= (0xF << 28);
  return result;
}

static inline long USHORT3_2_long(char c1, char c2, char c3){
  long result = c1;
  result <<= 7; result |= c2;
  result <<= 7; result |= c3;
  return result;
}

static inline long USHORT_2_long(char c1, char c2){
  long result = c1;
  result <<= 7; result |= c2;
  return result;
}

/**
 * GPSから受信した位置データを数値データに変換します。
 * データの種類は(-mint32仮定)
 *  (0) 緯度
 *  (1) 経度
 *  (2) 高度
 *  (3) 速度
 *  (4) 方位
 *  (5) 使用衛星数
 *  (6) UTC時刻(時 * 3600 + 分 * 60 + 秒)
 *  (7) 位置データ、測位モード (1 vs 0)
 *       bit 0: 測位 vs 未測位
 *       bit 1: 3D vs 2D
 *       bit 2: 速度・方位有効 vs 無効
 *  (8) 時刻データ、時刻ステータス
 *  (9) 誤差楕円指標値(長軸)
 * (10) 誤差楕円指標値(短軸)
 * (11) 誤差楕円指標値(方位)
 */
long *gps2value(char *gps_char, long *result){
 

  /* 以下の順に変換していく。
   * 
   * バイトデータがやってくる
   * 
   * (1) 0xe0 + 22 byte (23 byte)
   * (2) 0xe1 + 9 byte  (10 byte)
   * (3) 0xe3 + 14 byte (15 byte)
   */
  
  //チェック、規制緩和する可能性あり
  if(gps_char[0] != 0xE0){return NULL;}
  if(gps_char[23] != 0xE1){return NULL;}
  if(gps_char[33] != 0xE3){return NULL;}
  
  // 緯度
  result[0] = LONG_2_long(gps_char[3], gps_char[4], gps_char[5], gps_char[6]);
  // 経度
  result[1] = LONG_2_long(gps_char[7], gps_char[8], gps_char[9], gps_char[10]);
  // 高度
  result[2] = LONG_2_long(gps_char[11], gps_char[12], gps_char[13], gps_char[14]);
  // 速度
  result[3] = USHORT3_2_long(gps_char[15], gps_char[16], gps_char[17]);
  // 方位
  result[4] = USHORT_2_long(gps_char[18], gps_char[19]);
  // 使用衛星数
  result[5] = (int)gps_char[20];
  // UTC
  result[6] = (gps_char[28] * 3600) + (gps_char[29] * 60) + gps_char[30];
  // 位置データ、測位モード
  result[7] = (int)(gps_char[1] & 0x07);
  // 時刻データ、時刻ステータス
  result[8] = (int)(gps_char[24] & 0x03);
  // 誤差楕円指標値(長軸)
  result[9] = USHORT_2_long(gps_char[38], gps_char[39]);
  // (短軸)
  result[10] = USHORT_2_long(gps_char[40], gps_char[41]);
  // (方位)
  result[11] = USHORT_2_long(gps_char[42], gps_char[43]);
  
  return result;
}