/**
 * G_Pageの擬似距離を利用した姿勢の粗推定
 * 
 */
 
#include <iostream>
#include <sstream>
#include <fstream>

#include <iomanip>
#include <string>
#include <exception>

#include <map>
#include <queue>
#include <vector>

#include <cstdio>
#include <ctime>
#include <cmath>

#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "SylphideProcessor.h"

#include "coordinate.h"
#include "GPS.h"
#include "RINEX.h"

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

#include "gps_common.h"

using namespace std;

typedef SylphideProcessor<> 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

#ifndef pow2
#define pow2(x) ((x)*(x))
#endif

typedef double precision_t;

class G_Page_Solver_PR_compass {

  public:
    typedef Matrix<precision_t> matrix_t;
    typedef Quarternion<precision_t> quarternion_t;
    typedef Vector3<precision_t> vector3_t;
  
    typedef GPS_SpaceNode<precision_t> space_node_t;
    typedef space_node_t::gps_time_t gps_time_t;
    
    typedef space_node_t::xyz_t xyz_t;
    typedef space_node_t::llh_t llh_t;
    typedef space_node_t::enu_t enu_t;
    
    typedef map<int, G_Observer_t::raw_measurement_t> sat_measure_t;
    typedef map<int, vector<space_node_t::Satellite::Ephemeris> > nav_data_t;
    
    struct solve_1step_return_t {
      matrix_t G; // デザイン行列
      xyz_t delta_user_position;
      precision_t delta_receiver_error; 
    };
    
  protected:
    space_node_t _space_node;
    nav_data_t nav_data;
  
  public:
    G_Page_Solver_PR_compass() : _space_node(), nav_data() {}
    ~G_Page_Solver_PR_compass(){}
    
    space_node_t &space_node(){return _space_node;}
  
  protected:  
    /**
     * Iterationの1step
     * 
     * @param sat_measure 可視衛星番号と計測情報
     * @param target_time 計測時刻
     * @param user_position ユーザ位置(暫定解, m)
     * @param receiver_error 受信機クロック誤差(暫定解, m)
     * @param is_coarse_mode 精密な補正を行わないかどうか(デフォルト:true)
     * @return ユーザ位置の距離の補正量を返す
     */
    solve_1step_return_t solve_1step(
        const sat_measure_t &sat_measure,
        const gps_time_t &target_time,
        xyz_t &user_position,
        precision_t &receiver_error,
        bool is_coarse_mode = true) throw (exception){
      
      //cerr << "usr => " << user_position << endl;
      
      gps_time_t target_time_est(target_time);
      target_time_est -= (receiver_error / space_node_t::light_speed); 
      
      matrix_t G(sat_measure.size(), 4);
      matrix_t W(matrix_t::getI(sat_measure.size()));;      
      matrix_t delta_r(sat_measure.size(), 1);
      
      llh_t usr_llh(user_position.llh());
      
      unsigned i(0);
      for(sat_measure_t::const_iterator it(sat_measure.begin());
          it != sat_measure.end();
          ++it, ++i){
        
        precision_t range(it->second.pseudo_range);
        space_node_t::Satellite &sat(
            const_cast<space_node_t *>(&_space_node)->satellite(it->first));
        
        // 幾何距離(暫定値)
        delta_r(i, 0) = range - receiver_error;
        //cerr << "r1 => " << delta_r(i, 0) << endl;
        
        // 衛星のクロック誤差を補正
        delta_r(i, 0) += sat.clock_error(target_time_est, delta_r(i, 0)) * space_node_t::light_speed;
        //cerr << "r2 => " << delta_r(i, 0) << endl; 
        
        // 衛星の位置を計算   
        xyz_t sat_position(sat.whereis(target_time_est, delta_r(i, 0)));
        //cerr << it->first << ": " << sat_position << endl;
        precision_t range_est(user_position.dist(sat_position));
        
        // 幾何距離残差を計算
        delta_r(i, 0) -= range_est;
        
        // デザイン行列の作成
        G(i, 0) = -(sat_position.x() - user_position.x()) / range_est;
        G(i, 1) = -(sat_position.y() - user_position.y()) / range_est;
        G(i, 2) = -(sat_position.z() - user_position.z()) / range_est;
        G(i, 3) = 1.0;
        
        // 精密な補正を行う
        if(!is_coarse_mode){
          enu_t relative_pos(enu_t::relative(sat_position, user_position));
          // 電離層遅延補正
          precision_t iono_delta_r(_space_node.iono_correction(relative_pos, usr_llh, target_time_est));
          //cerr << "iono => " << iono_delta_r << endl;
          delta_r(i, 0) += iono_delta_r;
          
          // 対流圏遅延補正
          precision_t tropo_delta_r(_space_node.tropo_correction(relative_pos, usr_llh));
          //cerr << "tropo => " << tropo_delta_r << endl;
          delta_r(i, 0) += tropo_delta_r;
          
          // 重みの設定
          // GPS実用プログラミングより
          W(i, i) *= pow2(sin(relative_pos.elevation())/0.8); 
        }
      }
      //cerr << "delta_r:" << delta_r << endl;
      //cerr << "G:" << G << endl;
      
      try{
        // 最小二乗法の適用
        matrix_t delta_x(
            (G.transpose() * W * G).inverse() * G.transpose() * W * delta_r);
            
        solve_1step_return_t res;
        {
          res.G = G;
          res.delta_user_position = xyz_t(delta_x.partial(3, 1, 0, 0));
          res.delta_receiver_error = delta_x(3, 0);
        }
        
        user_position += res.delta_user_position;
        receiver_error += res.delta_receiver_error;
        
        return res;
      }catch(exception e){throw e;}
    }
      
  public:
    void solve_attitude(
        const sat_measure_t &sat_measure1,
        const sat_measure_t &sat_measure2,
        const gps_time_t &target_time) {
      
      // 初期解の設定
      static xyz_t user_position;
      static precision_t receiver_error(0);
      static unsigned invoked(0);
      
      sat_measure_t available_sat_measure;
      
      for(sat_measure_t::const_iterator it(sat_measure1.begin());
          it != sat_measure1.end();
          ++it){
        
        int prn(it->first);
        sat_measure_t::mapped_type raw_data(it->second);
        
        cerr << "PRN: " << prn << " => " << raw_data.pseudo_range;
        
        // 共通の衛星でない場合は飛ばす
        if(sat_measure2.find(prn) == sat_measure2.end()){
          continue;
        }
        
        bool available(false);
        bool search_ephemeris(true);
        if(_space_node.has_satellite(it->first)){
          available = true;
          if(!_space_node.satellite(prn).ephemeris().maybe_newer_one_avilable(target_time)){
            search_ephemeris = false;
          }
        }
        
        // エフェメリスの設定
        
        // 前にエフェメリスがない場合、
        // あるいは新しいものが利用可能である場合は設定する
        if(search_ephemeris){
          
          // エフェメリスがあるか
          if(nav_data.find(prn) != nav_data.end()){
          
            // エフェメリスを検索
            struct comparator {
              const gps_time_t &_t;
              comparator(const gps_time_t &t) : _t(t) {} 
              bool operator()(
                  const space_node_t::Satellite::Ephemeris &eph1,
                  const space_node_t::Satellite::Ephemeris &eph2){
                return _abs(eph1.period_from_time_of_clock(_t)) 
                    < _abs(eph2.period_from_time_of_clock(_t));
              }
            };
            _space_node.satellite(prn).ephemeris()
                = *min_element(nav_data[prn].begin(), nav_data[prn].end(), 
                    comparator(target_time));
            available = true;
          }
        }
        
        // エフェメリスがないものについては無効とする
        if(!available){
          cerr << endl;
          continue;
        }
        
        cerr << " @ Ephemeris: t_oc => "
             << _space_node.satellite(prn).ephemeris().WN << " week " 
             << _space_node.satellite(prn).ephemeris().t_oc
             << endl;
        
        /*
        cerr << _space_node.satellite(prn).ephemeris().a_f0 << ", "
             << _space_node.satellite(prn).ephemeris().a_f1 << ", "
             << _space_node.satellite(prn).ephemeris().a_f2 << ", "
             << _space_node.satellite(prn).ephemeris().iode << ", "
             << _space_node.satellite(prn).ephemeris().c_rs << ", "
             << _space_node.satellite(prn).ephemeris().delta_n << ", "
             << _space_node.satellite(prn).ephemeris().m0 << ", "
             << _space_node.satellite(prn).ephemeris().c_uc << ", "
             << _space_node.satellite(prn).ephemeris().e << ", "
             << _space_node.satellite(prn).ephemeris().c_us << ", "
             << _space_node.satellite(prn).ephemeris().sqrt_A << ", "
             << _space_node.satellite(prn).ephemeris().t_oe << ", "
             << _space_node.satellite(prn).ephemeris().c_ic << ", "
             << _space_node.satellite(prn).ephemeris().Omega0 << ", "
             << _space_node.satellite(prn).ephemeris().c_is << ", "
             << _space_node.satellite(prn).ephemeris().i0 << ", "
             << _space_node.satellite(prn).ephemeris().c_rc << ", "
             << _space_node.satellite(prn).ephemeris().omega << ", "
             << _space_node.satellite(prn).ephemeris().dot_Omega0 << ", "
             << _space_node.satellite(prn).ephemeris().dot_i0 << ", "
             << _space_node.satellite(prn).ephemeris().t_GD << endl;
        */
             
        
        // 擬似距離の再設定
        available_sat_measure[prn] = raw_data;
      }
      
      if(available_sat_measure.size() < 4){
        cerr << "Minimum satellite not available!! : sat => " << available_sat_measure.size() << endl;
        return;
      }
      
      // 航法計算の実行
      try{
        matrix_t C;
        
        for(unsigned i(0); i < 10; i++){
          solve_1step_return_t res(
              solve_1step(
                available_sat_measure,
                target_time,
                user_position,
                receiver_error,
                ((invoked == 0) ? (i < 3) : (i < 1)) // 初回だけは荒く計算する回数を増やす
              ));
          
          //cerr << "loop(" << i << ") => "
          //     << "user: " << user_position
          //     << "; receiver_error: " << receiver_error
          //     << endl;
          
          if(res.delta_user_position.dist() <= 1E-6){
            C = (res.G.transpose() * res.G).inverse();
            break;
          }
        }
        
        llh_t &user_position_llh(user_position.llh());
        cerr << "user(llh):" << user_position_llh << endl;
        
        // DOPの表示
        precision_t gdop(sqrt(C.trace())),
                    pdop(sqrt(C.partial(3, 3, 0, 0).trace())),
                    hdop(sqrt(C.partial(2, 2, 0, 0).trace())),
                    vdop(sqrt(C(2, 2))),
                    tdop(sqrt(C(3, 3)));
        
        // CSV形式で吐き出しておく
        cout << target_time.week << ", "
             << target_time.seconds << ", ";
        cerr << rad2deg(user_position_llh.latitude()) << ", "
             << rad2deg(user_position_llh.longitude()) << ", "
             << user_position_llh.height() << ", "
             << gdop << ", "
             << pdop << ", "
             << hdop << ", "
             << vdop << ", "
             << tdop << endl;
         
         invoked++;
      }catch(exception e){
        cerr << "No solution!!" << endl;
        //cerr << e.what() << endl;
      }
      
      // 姿勢推定の実行
      {
        // 行列の作成
        matrix_t diff_pseudo_range(available_sat_measure.size(), 1);
        matrix_t G(available_sat_measure.size(), 4);
        matrix_t W(matrix_t::getI(diff_pseudo_range.rows()));
        
        int i(0);
        for(sat_measure_t::iterator it(available_sat_measure.begin());
            it != available_sat_measure.end();
            ++it, ++i){
          int prn(it->first);
          precision_t pseudo_range1(it->second.pseudo_range);
          precision_t pseudo_range2(const_cast<sat_measure_t *>(&sat_measure2)->operator[](prn).pseudo_range);
          diff_pseudo_range(i, 0) = pseudo_range1 - pseudo_range2; 
          
          space_node_t::Satellite &sat(_space_node.satellite(prn));
          
          enu_t relative_pos(enu_t::relative(sat.whereis(target_time), user_position));
          precision_t el(relative_pos.elevation()), az(relative_pos.azimuth());
          
          G(i, 0) = cos(az) * cos(el);
          G(i, 1) = sin(az) * cos(el);
          G(i, 2) = -sin(el);
          G(i, 3) = 1;
          
          cerr << "PRN: " << prn << " => " 
               << diff_pseudo_range(i, 0) << ", "
               << rad2deg(el) << ", " << rad2deg(az) << endl;
          
          // 重みの設定
          // GPS実用プログラミングより
          W(i, i) *= pow2(sin(el)/0.8);
        }
        
        // 最小二乗法
        matrix_t baseline_deltat(
            (G.transpose() * W * G).inverse() * G.transpose() * W * diff_pseudo_range);
        
        // 残差
        //cerr << "residual:" << (diff_pseudo_range - G * baseline_deltat) << endl;
        
        xyz_t baseline(baseline_deltat(0, 0), baseline_deltat(1, 0), baseline_deltat(2, 0));
        matrix_t baseline_unit(baseline_deltat.partial(3, 1, 0, 0) / baseline.dist());
        
        cerr << "base_line_unit:" << baseline_unit << endl;
        
        // 回転行列を解く
        {
          
          vector3_t x(1, 0, 0); ///< 元の基線ベクトル
          vector3_t y(baseline_unit); ///< GPSによって導出された基線ベクトル
          precision_t theta(0), psi(0); ///< 推定するピッチ、ヨー
          
          // 解析的に求める
          // u = sin_psi
          precision_t a(pow2(x[0]) + pow2(x[1])), 
                      b(- y[1] * x[0] * 2), 
                      c(pow2(y[1]) - pow2(x[1]));
          //cout << a << ", " << b << ", " << c  << endl;
          //cout << (pow2(b) - a * c * 4) << endl;
          precision_t u1(-b / (a * 2)),
                      u2(sqrt(pow2(b) - a * c * 4) / (a * 2));
          //cout << u1 << ", " << u2 << endl;
          psi = asin(u1 + u2);
          
          if(psi > 0){psi = PI - psi;}
          else{psi = -PI + psi;}
          
          
          cout //<< "yaw, pitch: " 
               << rad2deg(psi) << "," 
               << rad2deg(theta) << endl;
        }
      }
    }
    
    void regist_epehemris(
        const G_Observer_t::ephemeris_t &ephemeris,
        const int cycle_NO = 0){
      
      // Ephemeris情報の変換
      space_node_t::Satellite::Ephemeris eph(
          EphemerisConverter<precision_t>(ephemeris, cycle_NO));
              
      // 衛星順、時刻順に並べる
      if(nav_data[eph.svid].empty()){
        nav_data[eph.svid].push_back(eph);
      }else{
        for(nav_data_t::mapped_type::reverse_iterator it(nav_data[eph.svid].rbegin()); 
            it != nav_data[eph.svid].rend(); 
            ++it){
          if(gps_time_t(eph.WN, eph.t_oc) >= gps_time_t(it->WN, it->t_oc)){
            if(eph.t_oc != it->t_oc){
              nav_data[eph.svid].insert(it.base(), eph);
            }
            break;
          }
        }
      }
      
      cerr << "NAV data : " << nav_data.size()
           << " ; " << eph.svid << " => " << nav_data[eph.svid].size() << ", " << eph.WN
           << endl;
    }
} solver;

bool require_switch_processor(false);

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x02, 0x10}のとき生情報(Carrire, 擬似距離)
 * {class, id} = {0x02, 0x31}のときエフェメリス
 * 
 * @param obsrever Gページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(!observer.validate()){return;}
  
  static int cycle_NO(0);
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  switch(packet_type.mclass){
    case 0x02: { 
      switch(packet_type.mid){
        case 0x10: {
          static G_Page_Solver_PR_compass::gps_time_t previous_invoked(0, 0);
          static G_Page_Solver_PR_compass::sat_measure_t previous_sat_measure;
          
          G_Page_Solver_PR_compass::sat_measure_t sat_measure;
          G_Page_Solver_PR_compass::gps_time_t t_epoc(observer.fetch_WN(), observer.fetch_ITOW());
          cycle_NO = t_epoc.week / 1024;
          unsigned int num_of_sv((unsigned char)observer[6 + 6]);
          cerr << "GPS_Time : " << t_epoc << endl;
          //cerr << "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));
            
            // 観測情報の設定 
            sat_measure[raw_data.sv_number] = raw_data;
          }
          
          if(previous_invoked <= t_epoc){
            if(previous_invoked.interval(t_epoc) <= 0.02){
              solver.solve_attitude(previous_sat_measure, sat_measure, previous_invoked);
            }
            require_switch_processor = true;
            previous_invoked = t_epoc;
            previous_sat_measure = sat_measure;
          }
          
          break;
        }
        case 0x31: {
          G_Observer_t::ephemeris_t ephemeris(observer.fetch_ephemeris());
          if(!ephemeris.valid){break;}
          
          solver.regist_epehemris(ephemeris, cycle_NO);
        }
      }
      break;
    }
    default:
      break;
  }
}

int main(int argc, char *argv[]){
  
  cout.precision(10);
  cerr.precision(16);

  cerr << "G_Page pseudo range compass." << endl;
  if(argc < 4){
    cerr << "Usage: (exe) log1.dat log2.dat rinex.nav" << endl;
    return -1;
  }
  
  // ログファイルの読み込み
  fstream fin_logs[] = {
      fstream(argv[1], ios::in | ios::binary),
      fstream(argv[2], ios::in | ios::binary)};
  
  for(int i(0); i < (sizeof(fin_logs) / sizeof(fstream)); i++){
    if(fin_logs[i].fail()){
      cout << "Not found!! : " << argv[i + 1] << endl;
      return -1;
    }
  }
  
  // RINEX航法ファイルの読み込み
  fstream fin_n(argv[3], ios::in | ios::binary);
  if(fin_n.fail()){
    cerr << "Not found!! : " << argv[3] << endl;
    return -1;
  }
  
  // 航法ファイルの処理
  {
    typedef RINEX_Nav_Reader<> reader_n_t;
    reader_n_t reader_n(fin_n);
    
    // 電離層遅延係数の処理
    if(!reader_n.extract_iono_coef(solver.space_node())){
      cerr << "Iono coef not found!!" << endl;
      return -1;
    }
  }
  
  char buffer[PAGE_SIZE];
  
  // ストリーム処理機を生成
  Processor_t processors[] = {Processor_t(OBSERVER_SIZE), Processor_t(OBSERVER_SIZE)};
  
  // Gページの際の処理を登録
  for(int i(0); i < (sizeof(processors) / sizeof(Processor_t)); i++){
    processors[i].set_g_handler(g_packet_handler);
  }
  
  int count(0);

  int mode(0);
  Processor_t *processor(&processors[mode]);
  iostream *in(&fin_logs[mode]);
  while(!(in->eof())){
        
    count++;
    
    int read_count;
    in->read(buffer, PAGE_SIZE);
    read_count = in->gcount();

    if(read_count < PAGE_SIZE){continue;}
    
    processor->process(buffer, read_count);
    
    // 時刻順に交互に処理する
    if(require_switch_processor){
      require_switch_processor = false;
      mode = (++mode) % (sizeof(processors) / sizeof(Processor_t));
      processor = &processors[mode];
      in = &fin_logs[mode];
    }
  }
  
  return 0;
}