#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <exception>

#include <cstdio>

#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "SylphideProcessor.h"

typedef SylphideProcessor<> Processor_t;
typedef Processor_t::A_Observer_t A_Observer_t;
typedef Processor_t::G_Observer_t G_Observer_t;
typedef Processor_t::F_Observer_t F_Observer_t;
typedef Processor_t::P_Observer_t P_Observer_t;
typedef Processor_t::M_Observer_t M_Observer_t;

#define BUFFER_SIZE (PAGE_SIZE * 1) // 32
#define OBSERVER_SIZE (PAGE_SIZE * 32) // 1024

using namespace std;

/**
 * Aページ(AD変換結果)の処理用関数
 * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Aページのオブザーバー
 */
void a_packet_handler(const A_Observer_t &observer){
  if(!observer.validate()){return;}
  
  cout.precision(10);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  
  A_Observer_t::values_t values(observer.fetch_values());
  cout << "A_Page_Values : ";
  for(int i = 0; i < 8; i++){
    cout << values.values[i] << ", ";
  }
  cout << values.temperature << endl;
}

/**
 * Fページ(FPGAサブシステムからの情報)の処理用関数
 * Fページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Fページのオブザーバー
 */
void f_packet_handler(const F_Observer_t &observer){
  if(!observer.validate()){return;}
  
  cout.precision(10);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  
  F_Observer_t::values_t values(observer.fetch_values());
  cout << "F_Page_Values : ";
  for(int i = 0; i < 8; i++){
    cout << values.servo_in[i] << ", "
         << values.servo_out[i] << ", ";
  }
  cout << endl;
}

/**
 * Pページ(エアデータセンサからの情報)の処理用関数
 * Pページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Pページのオブザーバー
 */
void p_packet_handler(const P_Observer_t &observer){
  if(!observer.validate()){return;}
  
  cout.precision(10);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  
  P_Observer_t::values_t values(observer.fetch_values());
  cout << "P_Page_Values : ";
  for(int i = 0; i < 4; i++){
    cout << values.air_speed[i] << ", "
         << values.air_alpha[i] << ", "
         << values.air_beta[i] << ", ";
  }
  cout << endl;
}

/**
 * Mページ(磁気コンパスからの情報)の処理用関数
 * Mページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Mページのオブザーバー
 */
void m_packet_handler(const M_Observer_t &observer){
  if(!observer.validate()){return;}
  
  cout.precision(10);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  
  M_Observer_t::values_t values(observer.fetch_values());
  cout << "M_Page_Values : ";
  for(int i = 0; i < 4; i++){
    cout << values.x[i] << ", "
         << values.y[i] << ", "
         << values.z[i] << ", ";
  }
  cout << endl;
}

#if DEBUG  
int g_good_count(0);
int g_bad_count(0);
#endif

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x01, 0x02}のとき位置情報
 * {class, id} = {0x01, 0x12}のとき速度情報
 * {class, id} = {0x01, 0x03}のとき受信機状態
 * {class, id} = {0x01, 0x04}のときDOP
 * {class, id} = {0x01, 0x06}のとき解情報
 * {class, id} = {0x01, 0x30}のとき衛星情報
 * {class, id} = {0x02, 0x10}のとき生情報
 * 
 * @param obsrever Aページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(observer.validate()){
#if DEBUG
    g_good_count++;
    cout << "Good : " << hex;
    for(int i = 0; i < observer.current_packet_size(); i++){
      cout 
        << setfill('0') 
        << setw(2)
        << (unsigned int)((unsigned char)observer[i]) << ' ';
    }
    cout << dec;
    cout << endl;
#endif
  }
#if DEBUG
  else{
    g_bad_count++;
    cout << "Bad : " << hex;
    for(int i = 0; i < observer.current_packet_size(); i++){
      cout 
        << setfill('0') 
        << setw(2)
        << (unsigned int)((unsigned char)observer[i]) << ' ';
    }
    cout << dec;
    cout << endl;
    return;
  }
#endif
  
  cout.precision(16);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  switch(packet_type.mclass){
    case 0x01: {
      switch(packet_type.mid){
        case 0x02: {
          G_Observer_t::position_t 
            position(observer.fetch_position());
          G_Observer_t::position_acc_t
            position_acc(observer.fetch_position_acc());
          cout << "Position(lat, long, alt) : " 
               << position.latitude << ", "
               << position.longitude << ", "
               << position.altitude << endl;
          cout << "Position_Acc(h, v) : " 
               << position_acc.horizontal << ", "
               << position_acc.vertical << endl;
          break;
        }
        case 0x12: {
          G_Observer_t::velocity_t
            velocity(observer.fetch_velocity());
          G_Observer_t::velocity_acc_t
            velocity_acc(observer.fetch_velocity_acc());
          cout << "Velocity(N, E, D) : " 
               << velocity.north << ", "
               << velocity.east << ", "
               << velocity.down << endl;
          cout << "Velocity_Acc : " 
               << velocity_acc.acc << endl;  
          break;
        }
        case 0x30: {
          unsigned int channels((unsigned char)observer[6 + 4]);
          for(int i(0); i < channels; i++){
            G_Observer_t::svinfo_t
                svinfo(observer.fetch_svinfo(i));
            cout << "SVINFO(" << i << ") : "
                 << svinfo.svid << ", "
                 << svinfo.elevation << ", "
                 << svinfo.azimuth << ", "
                 << svinfo.signal_strength << ", "
                 << svinfo.pseudo_residual << endl;
          }
          break;
        }
      }
      break;
    }
    case 0x02: { 
      switch(packet_type.mid){
        case 0x10: {
          unsigned int num_of_sv((unsigned char)observer[6 + 6]);
          cout << "Number of satellites : "
               << num_of_sv << endl;
          for(int i = 0; i < num_of_sv; i++){
            G_Observer_t::raw_measurement_t
              raw_data(observer.fetch_raw(i));
            cout << "RAW(" << i << ") : "  
                 << raw_data.carrier_phase << ", "
                 << raw_data.pseudo_range << ", "
                 << raw_data.doppler << ", "
                 << raw_data.sv_number << ", "
                 << raw_data.quarity << ", "
                 << raw_data.signal_strength << ", "
                 << raw_data.lock_indicator << endl;
          }
          break;
        }
        case 0x31: {
          G_Observer_t::ephemeris_t
              ephemeris(observer.fetch_ephemeris());
          cout << "Ephemeris SVID " << ephemeris.sv_number << endl;
          if(ephemeris.valid){
            cout << ephemeris.m_0 << ", "
                 << ephemeris.e << ", "
                 << ephemeris.root_a << ", "
                 << ephemeris.omega_0 << ", "
                 << ephemeris.i_0 << ", "
                 << ephemeris.omega << endl;
            cout << ephemeris.delta_n << ", "
                 << ephemeris.omega_0_dot << ", "
                 << ephemeris.i_0_dot << ", "
                 << ephemeris.c_rs << ", "
                 << ephemeris.c_uc << ", "
                 << ephemeris.c_us << ", "
                 << ephemeris.c_ic << ", "
                 << ephemeris.c_is << ", "
                 << ephemeris.c_rc << ", "
                 << ephemeris.a_f0 << ", "
                 << ephemeris.a_f1 << ", "
                 << ephemeris.a_f2 << ", "
                 << ephemeris.t_oc << ", "
                 << ephemeris.t_gd << endl;
          }
          break;
        }
      }
      break;
    }
    case 0x0b: {
      switch(packet_type.mid){
        case 0x02: {  // GPS Health, UTC and ionosphere parameters
          G_Observer_t::health_utc_iono_t
              hui(observer.fetch_health_utc_iono());
          cout << "Health, UTC and ionosphere parameters" << endl;
          if(hui.health.valid){
            cout << "Health:";
            for(int i(0); i < 32; i++){
              cout << (hui.health.healthy[i] ? " 1" : " 0");   
            }
            cout << endl;
          }
          if(hui.utc.valid){
            cout << "UTC: "
                 << hui.utc.a0 << ", "
                 << hui.utc.a1 << ", "
                 << hui.utc.tot << ", "
                 << hui.utc.wnt << ", "
                 << hui.utc.ls << ", "
                 << hui.utc.wnf << ", "
                 << hui.utc.dn << ", "
                 << hui.utc.lsf << ", "
                 << hui.utc.spare << endl;
          }
          if(hui.iono.valid){
            cout << "iono: "
                 << hui.iono.klob_a0 << ", "
                 << hui.iono.klob_a1 << ", "
                 << hui.iono.klob_a2 << ", "
                 << hui.iono.klob_a3 << ", "
                 << hui.iono.klob_b0 << ", "
                 << hui.iono.klob_b1 << ", "
                 << hui.iono.klob_b2 << ", "
                 << hui.iono.klob_b3 << endl;
          }
          break;
        }
      }
      break;
    }
    default:
      break;
  }
}

/**
 * ファイル等のストリームからページ単位で切り出す関数
 * 
 * @param in ストリーム
 */
void stream_processor(istream &in){
  char buffer[PAGE_SIZE];
  
  Processor_t processor(OBSERVER_SIZE);       // ストリーム処理機を生成
  processor.set_a_handler(a_packet_handler);  // Aページの際の処理を登録
  processor.set_g_handler(g_packet_handler);  // Gページの際の処理を登録
  processor.set_f_handler(f_packet_handler);  // Fページの際の処理を登録
  processor.set_p_handler(p_packet_handler);  // Pページの際の処理を登録
  processor.set_m_handler(m_packet_handler);  // Pページの際の処理を登録
  
  int count(0);

  while(!in.eof()){
    count++;
    int read_count;
    in.read(buffer, PAGE_SIZE);
    read_count = in.gcount();

#if DEBUG
    cout << "--read-- : " << count << " page" << endl;
    cout << hex;
    for(int i = 0; i < read_count; i++){
      cout 
        << setfill('0') 
        << setw(2)
        << (unsigned int)((unsigned char)buffer[i]) << ' ';
    }
    cout << dec;
    cout << endl;
#endif

    if(read_count < PAGE_SIZE){
#if DEBUG
      cout << "--skipped-- : " << count << " page ; count = " << read_count << endl;
#endif
      continue;
    }

    processor.process(buffer, read_count);
  }
  
#if DEBUG
  cout << "Good/Bad = " 
       << g_good_count << "/" << g_bad_count
       << "; ratio = " << ((double)g_bad_count / (g_good_count + g_bad_count) * 100)
       << " %" << endl;
#endif
}

int main(int argc, char *argv[]){

  //stream_processor(cin);

  cout << "sample log processor." << endl;
  if(argc < 2){
    cout << "Usage: (exe) log.dat" << endl;
    return -1;
  }
  
  fstream fin(argv[1], std::ios::in | ios::binary);    
  if(fin.fail()){
    cout << "Not found : " << argv[1] << endl;
    return -1;
  }
  
  stream_processor(fin);
  
  return 0;
}