/*
 * 実験用
 * 
 * 
 */
#include <iostream>
#include <fstream>
#include <strstream>
#include <math.h>
#include <time.h>

using namespace std;

#include "util/util.h"
#include "param/parameter.h"
#include "INS.h"
#include "INS_GPS.h"
#include "sensor/MEMS.h"

typedef double accuracy;
#define DUMP_TARGET 9
const char *label[DUMP_TARGET] = {"東経[秒]", "北緯[秒]", "高度[m]", "北方向速度[m/s]", "東方向速度[m/s]", "重力方向速度[m/s]", "Ψ[deg]", "Θ[deg]", "Φ[deg]"};

#define DUMP_ALL 0
#define DUMP_INTERVAL 5

#define FREQUENCY 20      /* 更新周期[Hz] */

string pretty_deg2(double degree){
  char buffer[128];
  (degree -= (int)degree) *= (60 * 60);
  sprintf(buffer, "%f", degree);
  return string(buffer);
}

int main(int argc, char *argv[]){
  
  INS_GPS<accuracy> ins_gps;  //INS_GPS
  
  // 初期値
  /*
  ins_gps.initPosition(ins_true.latitude(), ins_true.longitude(), ins_true.height());
  ins_gps.initVelocity(ins_true.v_north(), ins_true.v_east(), ins_true.v_down());
  ins_gps.initAttitude(0, 0, 0);
  */

  // ファイル
  ifstream ifs(argv[1], ios::in);
  if(ifs.fail()){
    cout << "Usage: arg[1] = target" << endl;
    return -1;
  }
  
  char buffer2[128];
  ifs.getline(buffer2, sizeof(buffer2), '\n');
  cout << buffer2 << endl;
  
  return 0;
  
  /*
  char buffer[1024];
  strstream strout(buffer, sizeof(buffer), strstream::out);
  
  // フィルターの調整
  Matrix<accuracy> P(ins_gps.getFilter().getP());
  {
    P(0, 0) = P(1, 1) = P(2, 2) = 1E+0;
    P(3, 3) = P(4, 4) = P(5, 5) = P(6, 6) = 1E-10;
    P(7, 7) = 1E+3;
    P(8, 8) = P(9, 9) = P(10, 10) = P(11, 11) = 1E-6;
  }
  Matrix<accuracy> Q(ins_gps.getFilter().getQ());    
  {
    Q(0, 0) = pow(pow(1. / FREQUENCY, mems_a0.WN_SLOPE) * mems_a0.WN_INTERCEPT, 2);
    Q(1, 1) = pow(pow(1. / FREQUENCY, mems_a1.WN_SLOPE) * mems_a1.WN_INTERCEPT, 2);
    Q(2, 2) = pow(pow(1. / FREQUENCY, mems_a2.WN_SLOPE) * mems_a2.WN_INTERCEPT, 2);
    Q(3, 3) = pow(pow(1. / FREQUENCY, mems_g0.WN_SLOPE) * mems_g0.WN_INTERCEPT, 2);
    Q(4, 4) = pow(pow(1. / FREQUENCY, mems_g1.WN_SLOPE) * mems_g1.WN_INTERCEPT, 2);
    Q(5, 5) = pow(pow(1. / FREQUENCY, mems_g2.WN_SLOPE) * mems_g2.WN_INTERCEPT, 2);
    Q(6, 6) = 1E-10;
  }

  // ラベル
  cout << "[SEC]" << "\t";
  for(int j = 0; j < DUMP_TARGET; j++) cout << label[j] << "(" << target_label[i] << ')' << "\t"; 
  
  cout << "GPS東経" << "\t"
       << "GPS北緯" << "\t"
       << "GPS高度" << "\t"
       << "GPS速度" << "\t"
       << "GPS真方位" << "\t";
  
  cout << "加速度計[X^b]\t" 
       << "加速度計[Y^b]\t" 
       << "加速度計[Z^b]\t"
       << "ジャイロ[ω_X^b]\t"
       << "ジャイロ[ω_Y^b]\t"
       << "ジャイロ[ω_Z^b]\t";
#if DUMP_ALL
  for(unsigned j = 0; j < INS<accuracy>::STATE_VALUES; j++){
    cout << "状態量[" << j << "]" << "(" << target_label[i] << ')' << "\t"; 
  }
#endif
  cout << endl;
  
  long t = (long)time(NULL);    //実行時間測定用
  
  for(int step = 1; step <= FREQUENCY * SIM_LIMIT; step++){

    // 加速度の生成
    Vector3<accuracy> accel;
    
    // 角速度の生成
    Vector3<accuracy> gyro;
    
    // 更新
    ins_gps.update(accel, gyro, 1. / FREQUENCY);
    
    // 状態の出力
    strout << (1. / FREQUENCY) * step << "\t";
    strout << pretty_deg2(rad2deg(ins_gps.longitude())) << "\t"
           << pretty_deg2(rad2deg(ins_gps.latitude())) << "\t"
           << ins_gps.height() << "\t" 
           << ins_gps.v_north() << "\t"
           << ins_gps.v_east() << "\t"
           << ins_gps.v_down() << "\t"
           << rad2deg(ins_gps.euler_psi()) << "\t" 
           << rad2deg(ins_gps.euler_theta()) << "\t" 
           << rad2deg(ins_gps.euler_phi()) << "\t";
         
    // GPS補正を使うかどうか
    if(step % (FREQUENCY * GPS_DELTA) == 0){
      
      // GPSデータ書き出し
      strout << pretty_deg2(rad2deg(longitude)) << "\t"
             << pretty_deg2(rad2deg(latitude)) << "\t"
             << height << "\t"
             << v << "\t"
             << rad2deg(v_heading) << "\t";
      
      // 修正
      ins_gps.correct(v, v_heading, latitude, longitude, height, GPS_LAT_SIGMA, GPS_LONG_SIGMA, 0);
    }else{strout << "\t" << "\t" << "\t" << "\t" << "\t";}

    //加速度、ジャイロの値
    for(unsigned i = 0; i < Vector3<accuracy>::OUT_OF_INDEX; i++){
      strout << accel[i] << "\t";
    }
    for(unsigned i = 0; i < Vector3<accuracy>::OUT_OF_INDEX; i++){
      strout << gyro[i] << "\t";
    }
#if DUMP_ALL
    strout << ins_gps << "\t";
#endif

    strout << endl << ends;
    
    if(step % DUMP_INTERVAL == 0) cout << buffer;
    strout.seekp(0);
  }
  
  t = (long)time(NULL) - t;
  
  //データ書き出し
  
  cout << "INS/GPS実機試験" << endl;
  
  cout << "[日時] " << (long)time(NULL) << endl;
  
  cout << "[条件]" << endl
       << "積分周波数" << FREQUENCY << "[Hz]" << endl
       << "GPS補正タイミング" << GPS_DELTA  << "[sec]" << endl
       
  cout << "[実行時間] " << t << "[sec]" << endl;
  
  cout << "[最終位置]" << endl 
       << "φ=" << pretty_deg(rad2deg(ins_gps.latitude()))
       << ": λ=" << pretty_deg(rad2deg(ins_gps.longitude()))
       << ": h=" << ins_gps.height() << endl;
  
  cout << ins_gps.getFilter().getP() << endl;
  
  delete ifs;
  
  return 0;
  */
}