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

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

//#define DEBUG 1

#define IS_LITTLE_ENDIAN 1

typedef double float_sylph_t;

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

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

#include "calibration.h"

using namespace std;

void pretty_print(unsigned char *buf, int count){
  cout << hex;
  while(count--){
    cout 
      << setfill('0') 
      << setw(2)
      << (unsigned int)(*(buf++)) << ' ';
  }
  cout << dec;
  cout << endl;
}

void vector2bytes(const Vector3<float_sylph_t> &v, unsigned char *buf){
  for(int i = 0; i < 3; i++){
    float value(v.get(i));
    unsigned char *bytes(reinterpret_cast<unsigned char *>(&value));
    for(int j = 0; j < sizeof(float); j++){
      *(buf++) = *(bytes + j);
    }
  }
}

void mat2bytes(
    const Matrix<float_sylph_t> &mat, 
    const unsigned &i, 
    unsigned char *buf){
  for(int j = 0; j < 3; j++){
    float value(const_cast<Matrix<float_sylph_t> *>(&mat)->operator()(i, j));
    unsigned char *bytes(reinterpret_cast<unsigned char *>(&value));
    for(int k = 0; k < sizeof(float); k++){
      *(buf++) = *(bytes + k);
    }
  }
}

/**
 * キャリブレーションパラメータが記載された設定ページ(Sページ)0を生成します
 * 
 * === S0ページ仕様(32bytes) ===
 * Header (4byte)
 *   'S' (1byte)
 *   シーケンスNo (1byte)
 *   Sページの種類 = 0 (1byte)
 *   拡張 (1byte)
 * 
 * シーケンスNo (1byte) = 0x00 (+27bytes)
 *   [(4byte,float) _acc_bias0(3), _acc_bias1(3)], [(2byte,short)_acc_bias_t0]
 *   [(1byte, uchar)index_base]
 * シーケンスNo (1byte) = 0x01 (+24bytes)
 *   [(4byte,float) _acc_sf(3), _sigma_acc(3)]
 * シーケンスNo (1byte) = 0x02 (+24bytes)
 *   [(4byte,float) _acc_mis00, _acc_mis01, _acc_mis02, 
 *      _acc_mis10, _acc_mis11,_acc_mis12]
 * シーケンスNo (1byte) = 0x03 (+12bytes)
 *   [(4byte,float) _acc_mis20, _acc_mis21, _acc_mis22]
 * 
 * シーケンスNo (1byte) = 0x08 (+26bytes)
 *   [(4byte,float) _gyro_bias0(3), _gyro_bias1(3)], [(2byte,short)_gyro_bias_t0]
 * シーケンスNo (1byte) = 0x09 (+24bytes)
 *   [(4byte,float) _gyro_sf(3), _sigma_gyro(3)]
 * シーケンスNo (1byte) = 0x0A (+24bytes)
 *   [(4byte,float) _gyro_mis00, _gyro_mis01, _gyro_mis02, 
 *      _gyro_mis10, _gyro_mis11,_gyro_mis12]
 * シーケンスNo (1byte) = 0x0B (+12bytes)
 *   [(4byte,float) _gyro_mis20, _gyro_mis21, _gyro_mis22]
 * 
 * @param snapshot 送信対象 
 */
void encode(
    const StandardCalibrator &calibrator, ostream &out,
    RotatedCalibrator *rotation = NULL){
  unsigned char spage[32];
  spage[0] = 'S';
  spage[2] = 0;
  spage[3] = 0; 
  
  // 0x00
  {
    spage[1] = 0x00;
    Vector3<float_sylph_t> bias_t380(calibrator.acc_bias(380));
    Vector3<float_sylph_t> bias_grad(calibrator.acc_bias(381) - bias_t380);
    vector2bytes(bias_t380, &spage[4]);
    vector2bytes(bias_grad, &spage[16]);
    *reinterpret_cast<short *>(&spage[28]) = 380;
    spage[30] = (unsigned char)calibrator.index_base;
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
  }
  
  // 0x01
  {
    spage[1] = 0x01;
    Vector3<float_sylph_t> sf_t380(calibrator.acc_sf(380));
    Vector3<float_sylph_t> sigma(calibrator.sigma_accel());
    vector2bytes(sf_t380, &spage[4]);
    vector2bytes(sigma, &spage[16]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32); 
  }
  
  // 0x02,0x03
  {
    Matrix<float_sylph_t> mis(calibrator.acc_mis());
    if(rotation){
      mis = rotation->rotate_mat() * mis;
    }
    spage[1] = 0x02;
    mat2bytes(mis, 0, &spage[4]);
    mat2bytes(mis, 1, &spage[16]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
    spage[1] = 0x03;
    mat2bytes(mis, 2, &spage[4]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
  }
  
  // 0x08
  {
    spage[1] = 0x08;
    Vector3<float_sylph_t> bias_t380(calibrator.gyro_bias(380));
    Vector3<float_sylph_t> bias_grad(calibrator.gyro_bias(381) - bias_t380);
    vector2bytes(bias_t380, &spage[4]);
    vector2bytes(bias_grad, &spage[16]);
    *reinterpret_cast<short *>(&spage[28]) = 380;
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
  }
  
  // 0x09
  {
    spage[1] = 0x09;
    Vector3<float_sylph_t> sf_t380(calibrator.gyro_sf(380));
    Vector3<float_sylph_t> sigma(calibrator.sigma_gyro());
    vector2bytes(sf_t380, &spage[4]);
    vector2bytes(sigma, &spage[16]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32); 
  }
  
  // 0x0A,0x0B
  {
    Matrix<float_sylph_t> mis(calibrator.gyro_mis());
    if(rotation){
      mis = rotation->rotate_mat() * mis;
    }
    spage[1] = 0x0A;
    mat2bytes(mis, 0, &spage[4]);
    mat2bytes(mis, 1, &spage[16]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
    spage[1] = 0x0B;
    mat2bytes(mis, 2, &spage[4]);
    pretty_print(spage, 32);
    out.write(reinterpret_cast<char *>(spage), 32);
  }
}

int main(int argc, char *argv[]){
  
  cout << "calibrator => S page format" << endl;
  if(argc < 2){
    cout << "Usage: (exe) target [oprions]" << endl;
    return -1;
  }
  
  const CalibratorItem *calibrator_item(NULL);
  
  cout << "Target checking..." << endl;
  for(int i = 0; i < (sizeof(calibrators) / sizeof(CalibratorItem)); i++){
    cout << calibrators[i].name << " : ";
    if(!strcmp(argv[1], calibrators[i].name)){
      cout << "Ready !!" << endl;
      calibrator_item = &calibrators[i];
      break;
    }else{
      cout << "NG" << endl;
    }
  }
  
  if(!calibrator_item){
    cout << "Target mode not found" << endl;
    return -1;
  }
  
  Calibrator *calibrator(calibrator_item->calibrator);
  RotatedCalibrator *rotation(NULL);
  
  for(int arg_index(2); arg_index < argc; arg_index++){
    
    // ex) --ratate=-Y-Z-X
    if(strstr(argv[arg_index], "--rotate=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--rotate="));
      cerr << "rotate: " << spec << endl;
      RotatedCalibrator::rotate_spec_t decoded;
      if(!RotatedCalibrator::decode(spec, decoded)){
        cerr << "Invalid Rotation!!" << endl;
        return -1;
      }
      rotation = new RotatedCalibrator(*calibrator, decoded);
      cerr << "rotate_mat: " << rotation->rotate_mat() << endl;
      continue;
    }
    if(strstr(argv[arg_index], "--calib_file=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--calib_file="));
      fstream fin(spec);
      if(fin.fail()){continue;}
      cerr << "calib_file: " << spec << endl;
      if(strcmp(argv[1], CustomStandardCalibrator::id)){continue;}
      CustomStandardCalibrator *target(
          static_cast<CustomStandardCalibrator *>(calibrator));
      char buf[1024];
      while(!fin.eof()){
        fin.getline(buf, sizeof(buf));
        target->custom(buf);
      }      
      continue;
    }
    
    cerr << "Unknown option!! : " << argv[arg_index] << endl;
    return -1;
  }
  
  string fname(calibrator_item->name);
  fname.append("_config.dat");
  fstream fout(fname.c_str(), std::ios::out | ios::binary);    
  if(fout.fail()){
    cout << "Output file cannot open : " << fname << endl;
    return -1;
  }
  
  encode(
      *static_cast<StandardCalibrator *>(calibrator), 
      fout,
      rotation);
}