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

#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <fcntl.h>

#ifdef _WIN32
#include <io.h>
#endif

#include "util/comstream.h"

#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "CatCAMStream.h"
#include "CatCAMProcessor.h"

typedef CatCAMProcessor<> Processor_t;
typedef Processor_t::A_Observer_t A_Observer_t;
typedef Processor_t::G_Observer_t G_Observer_t;
typedef Processor_t::I_Observer_t I_Observer_t;

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

using namespace std;

int verbose = 0;

/**
 * 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 < 9; i++){
    cout << values.values[i].x << ", "
      << values.values[i].y << ", "
      << values.values[i].z << ", ";
  }
  cout << endl;
}

/**
 * Iページ(カメラからの画像データ)の処理用関数
 * Iページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Iページのオブザーバー
 */
void i_packet_handler(const I_Observer_t &observer){
  if(!observer.validate()){return;}
  
  cout.precision(10);
  cout << "GPS_Time : " << observer.fetch_ITOW() << endl;
  unsigned int length(observer.fetch_data_length());
  cout << "I_Page : length => " << length << endl;
  
  char *buf(new char[length]);
  observer.fetch_data(buf, length);
  if(verbose > 1){
    cout << "Good : " << hex;
    for(int i(0); i < length; i++){
      cout 
        << setfill('0') 
        << setw(2)
        << (unsigned int)((unsigned char)buf[i]) << ' ';
    }
    cout << dec;
    cout << endl;
  }
  delete [] buf;
}

int g_good_count(0);
int g_bad_count(0);

/**
 * 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(verbose){
      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;
    }
  }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;
  }
  
  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, ostream *out = NULL){
  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_i_handler(i_packet_handler);  // Iページの際の処理を登録
  
  int count(0);

  while(!in.eof()){
    count++;
    int read_count;
    in.read(buffer, PAGE_SIZE);
    read_count = in.gcount();
    
    if(out){
      out->write(buffer, read_count);
    }

    if(verbose > 1){
      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;
    }

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

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

#ifdef _WIN32
#define COMPORT_PREFIX "COM"
#else
#define COMPORT_PREFIX "/dev/"
#endif

istream &spec2istream(const char *spec){
  if(strcmp(spec, "-") == 0){
    // 標準出力は'-'で指定
    cerr << "[std::cin]" << endl;
#ifdef _WIN32
    setmode(fileno(stdout), O_BINARY);
#endif
    return cin;
  }else if(strstr(spec, COMPORT_PREFIX) == spec){
    // COMポート
    cerr << spec << endl;
    return *(new ComportStream(spec));
  }else{
    cerr << spec;
    fstream *fin(new fstream(spec, ios::in | ios::binary));
    if(fin->fail()){
      cerr << " => File not found!!" << endl;
      exit(-1);
    }
    cerr << endl;
    return *fin;
  }
}

std::ostream &spec2ostream(
    const char *spec,
    const bool force_fstream = false){
  if(!force_fstream){
    if(strcmp(spec, "-") == 0){
      // 標準出力は'-'で指定
      std::cerr << "[std::cout]" << std::endl;
#if defined(_MSC_VER)
      setmode(fileno(stdout), O_BINARY);
#endif
      return std::cout;
    }else if(strstr(spec, COMPORT_PREFIX) == spec){
      // COMポート
      cerr << spec << endl;
      return *(new ComportStream(spec));
    }
  }
  cerr << spec;
  fstream *fout(new fstream(spec, ios::out | ios::binary));
  if(fout->fail()){
    cerr << " => File cannot open!!" << endl;
    exit(-1);
  }
  cerr << endl;
  return *fout;
}

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

  //stream_processor(cin);

  cout << "sample log processor." << endl;
  if(argc < 2){
    cout << "* Usage: " << argv[0] << " log.dat [options]" << endl;
    cout << "* options list" << endl;
    cout << "   --direct=(on|off) : Use direct connection / Use logfile(default)" << endl;
    cout << "* example" <<endl;
    cout << "   " << argv[0] << " COM1 --direct=on (for Windows)" << endl;
    cout << "   " << argv[0] << " /dev/tty.FireFly-6530-SPP-1 --direct=on (for MacOSX)" << endl;
    return -1;
  }
  
  ostream *raw_out(NULL);
  
  bool direct_connection(false);
  for(int arg_index(2); arg_index < argc; arg_index++){
    
    if(strstr(argv[arg_index], "--direct=") == argv[arg_index]){
      char *value(argv[arg_index] + strlen("--direct="));
      direct_connection = (strcmp(value, "on") == 0);
      cerr << "direct_connection: " 
          << (direct_connection ? "on" : "off") << endl;
      continue;
    }
    if(strstr(argv[arg_index], "--verbose=") == argv[arg_index]){
      char *value(argv[arg_index] + strlen("--verbose="));
      verbose = atoi(value);
      cerr << "verbose: " << verbose << endl;
      continue;
    }
    if(strstr(argv[arg_index], "--raw_out=") == argv[arg_index]){
      char *value(argv[arg_index] + strlen("--raw_out="));
      raw_out = &spec2ostream(value);
      continue;
    }
    
    cerr << "Unknown option!! : " << argv[arg_index] << endl;
    return -1;
  }
  
  istream &in(spec2istream(argv[1]));
  if(direct_connection){
    CatCAMIStream catcam_in(in, PAGE_SIZE);
    stream_processor(catcam_in, raw_out);
  }else{
    stream_processor(in, raw_out);
  }
  
  return 0;
}