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

#include <stdio.h>
#include <io.h>
#include <fcntl.h>

#define DEBUG 0

#define IS_LITTLE_ENDIAN 1
#include "SylphideProcessor.h"
#include "SylphideStream.h"

typedef double float_sylph_t;
typedef SylphideProcessor<float_sylph_t> Processor_t;
typedef Processor_t::A_Observer_t A_Observer_t;
typedef Processor_t::G_Observer_t G_Observer_t;

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

using namespace std;

#include "param/matrix.h"
#include "param/vector3.h"
#include "param/quarternion.h"
#include "param/complex.h"

#include "util/util.h"
#include "util/fifo.h"

#include "calibration.h"
#include "packet.h"

#include "analyze_common.h"

struct Options : public GlobalOptions<float_sylph_t> {

  typedef GlobalOptions<float_sylph_t> super_t;
  
  Options() : super_t() {};
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(const char *spec){
    using std::cerr;
    using std::endl;
    
    return super_t::check_spec(spec);
  }
} options;

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

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x01, 0x02}のとき位置情報が得られる
 * {class, id} = {0x01, 0x12}のとき速度情報が得られる
 * 
 * @param obsrever Aページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(!observer.validate()){return;}
}

/**
 * ファイル等のストリームからページ単位で切り出す関数
 * 
 * @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ページの際の処理を登録
  
  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);
  }
}

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

  cerr << "sample log processor." << endl;
  if(argc < 3){
    cerr << "Usage: (exe) mode log.dat [options]" << endl;
    return -1;
  }
  
  cerr << "Mode checking..." << endl;
  for(int i = 0; i < (sizeof(calibrators) / sizeof(CalibratorItem)); i++){
    cerr << calibrators[i].name << " : ";
    if(!strcmp(argv[1], calibrators[i].name)){
      cerr << "Ready !!" << endl;
      A_Packet::calibrator = calibrators[i].calibrator;
      break;
    }else{
      cerr << "NG" << endl;
    }
    if(i == ((sizeof(calibrators) / sizeof(CalibratorItem)) - 1)){
      cerr << "Target mode not found" << endl;
      return -1;
    } 
  }  
  
  istream &in(options.spec2istream(argv[2]));
  
  // option check...
  cerr << "Option checking..." << endl;
  for(int arg_index(3); arg_index < argc; arg_index++){
    
    const char *value;
    // ex) --rotate=-Y-Z-X
    if(value = Options::get_value(argv[arg_index], "rotate")){
      cerr << "rotate: " << value << endl;
      RotatedCalibrator::rotate_spec_t decoded;
      if(!RotatedCalibrator::decode(value, decoded)){
        cerr << "Invalid Rotation!!" << endl;
        return -1;
      }
      A_Packet::calibrator = new RotatedCalibrator(*A_Packet::calibrator, decoded);
      continue;
    }
    if(value = Options::get_value(argv[arg_index], "calib_file")){
      fstream fin(value);
      if(fin.fail()){continue;}
      cerr << "calib_file: " << value << endl;
      if(strcmp(argv[1], CustomStandardCalibrator::id)){continue;}
      CustomStandardCalibrator *target(
          static_cast<CustomStandardCalibrator *>(A_Packet::calibrator));
      char buf[1024];
      while(!fin.eof()){
        fin.getline(buf, sizeof(buf));
        target->custom(buf);
      }      
      continue;
    }
    
    if(options.check_spec(argv[arg_index])){
      continue;
    }

    cerr << "Unknown option!! : " << argv[arg_index] << endl;
    return -1;
  }
  
  if(options.in_sylphide){
    SylphideIStream sylph_in(in, PAGE_SIZE);
    stream_processor(sylph_in);
  }else{
    stream_processor(in);
  }
  
  return 0;
}