/**
 * 第5章の内容を外部ヘッダ化したものでコンパイル可能にした
 * 仰角マスクや重み付き更新はまだサポートしていない
 * 
 * @see navigation/GPS.h
 * @see navigation/RINEX.h
 * @see navigation/coordinate.h
 * 
 * This program is derived from C language programs 
 * which are described in "実用GPSプログラミング" wrote by Dr.Sakai,
 * and re-written and modified with C++ language by fenrir(M.Naruoka)
 * 
 * All copyright is held by fenrir(M.Naruoka).
 * 
 * @see https://fenrir.naruoka.org/archives/000620.html
 */

#include <iostream>
#include <sstream>
#include <fstream>
#include <map>
#include <string>
#include <queue>
#include <vector>

#include <ctime>
#include <cmath>

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

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

using namespace std;

typedef double precision_t;

typedef Matrix<precision_t> matrix_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, precision_t> sat_range_t;

struct solve_1step_return_t {
  matrix_t G; // デザイン行列
  xyz_t delta_user_position;
  precision_t delta_receiver_error; 
};

/**
 * Iterationの1step、セクション4と同じ
 * 仰角マスクや重み付き更新はコーディングしていない
 * 
 * @param space_node
 * @param sat_range 可視衛星番号と計測した擬似距離(これらの衛星はすべてエフェメリスあること前提)
 * @param target_time 計測時刻
 * @param user_position ユーザ位置(暫定解, m)
 * @param receiver_error 受信機クロック誤差(暫定解, m)
 * @param is_coarse_mode 精密な補正を行わないかどうか(デフォルト:true)
 * @return ユーザ位置の距離の補正量を返す
 */
solve_1step_return_t solve_1step(
    const space_node_t &space_node,
    const sat_range_t &sat_range,
    const gps_time_t &target_time,
    xyz_t &user_position,
    precision_t &receiver_error,
    bool is_coarse_mode = true) {
  
  //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_range.size(), 4);
  matrix_t delta_r(sat_range.size(), 1);
  
  llh_t usr_llh(user_position.llh());
  
  unsigned i(0);
  for(sat_range_t::const_iterator it(sat_range.begin());
      it != sat_range.end();
      ++it, ++i){
    
    precision_t range(it->second);
    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;
    }
  }
  //cerr << "delta_r:" << delta_r << endl;
  //cerr << "G:" << G << endl;
  
  // 最小二乗法の適用
  matrix_t delta_x(
      (G.transpose() * G).inverse() * G.transpose() * 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;
}

int main() {
  
  cout.precision(16);
  cerr.precision(16);
  
  const char *target_file_n = "mtka3180.05n";
  const char *target_file_o = "mtka3180.05o";
  
  // 真値
  xyz_t mtka_true(-3947762.7496, 3364399.8789, 3699428.511);
  
  space_node_t space_node;
  
  // RINEX航法ファイルの読み込み
  ifstream fin_n(target_file_n);
  if(fin_n.fail()){
    cerr << "File not found!! : " << target_file_n << endl;
    exit(-1);
  }
  
  // 航法ファイルの処理
  typedef RINEX_Nav_Reader<> reader_n_t;
  reader_n_t reader_n(fin_n);
  
  /*for(reader_t::header_t::iterator it(reader_n.header().begin());
      it != reader_n.header().end();
      ++it){
    cerr << it->first << " : " << it->second << endl; 
  }*/
  
  // 電離層遅延係数の処理
  if(!reader_n.extract_iono_coef(space_node)){
    cerr << "Iono coef not found!!" << endl;
    exit(-1);
  }
  
  typedef map<int, vector<reader_n_t::SatelliteInfo> > nav_data_t;
  nav_data_t nav_data;
  
  // すべての航法情報の取得を行い、衛星順、時刻順に並べる
  while(reader_n.has_next()){
    reader_n_t::SatelliteInfo info(reader_n.next());
    {
      nav_data_t::mapped_type::reverse_iterator it(nav_data[info.svid].rbegin());
      for(; it != nav_data[info.svid].rend(); ++it){
        if(info.t_ot > it->t_ot){break;}
      }
      nav_data[info.svid].insert(it.base(), info);
    }
  }
  
  
  // RINEX観測ファイルの読み込み
  ifstream fin_o(target_file_o);
  if(fin_o.fail()){
    cerr << "File not found!! : " << target_file_o << endl;
    exit(-1);
  }
  
  // 観測ファイルの処理
  typedef RINEX_Observe_Reader<> reader_o_t; 
  reader_o_t reader_o(fin_o);
  
  precision_t leap_seconds(0);
  {
    reader_o_t::header_t::iterator it(reader_o.header().find("LEAP SECONDS"));
    if(it != reader_o.header().end()){
      stringstream ss(it->second);
      ss >> leap_seconds;
    }     
  }
  cerr << "leap seconds => " << leap_seconds << endl;
  
  /*for(reader_t::header_t::iterator it(reader_o.header().begin());
      it != reader_o.header().end();
      ++it){
    cerr << it->first << " : " << it->second << endl; 
  }*/
  
  // データライン上でのL1擬似距離のインデックスを取得
  int L1_pseudo_index;
  if((L1_pseudo_index = reader_o.observed_index("C1")) < 0){
    if((L1_pseudo_index = reader_o.observed_index("P1")) < 0){
      cerr << "C1/P1 not found!!" << endl;
      exit(-1);
    }
  }
  
  /*while(reader_o.has_next()){
    reader_o_t::ObservedItem item(reader_o.next());
    cerr << "epoc: " << item.t_epoc << endl;
    for(reader_o_t::ObservedItem::sat_data_t::iterator it(item.sat_data.begin());
        it != item.sat_data.end();
        ++it){
      cerr << "PRN: " << it->first << " => ";
      for(reader_o_t::ObservedItem::data_1sat_t::iterator it2(it->second.begin());
          it2 != it->second.end();
          ++it2){
        cerr << it2->observed << ", "; 
      }
      cerr << endl;
    }
  }*/
  
  // 初期解の設定
  xyz_t user_position;
  precision_t receiver_error(0);
  
  // 以下、観測ファイルを順に処理
  for(unsigned data_o_index(0); reader_o.has_next(); data_o_index++){
    reader_o_t::ObservedItem item(reader_o.next());
    
    sat_range_t sat_range;
    
    cerr << "epoc: " << item.t_epoc << endl;
    for(reader_o_t::ObservedItem::sat_data_t::iterator it(item.sat_data.begin());
        it != item.sat_data.end();
        ++it){
      int prn(it->first);
      precision_t pseudo((it->second)[L1_pseudo_index].observed);
      cerr << "PRN: " << prn << " => " << pseudo;
      
      // 利用可能衛星(可視かつエフェメリスがあること)を以下調べる
      bool available(false);
      bool search_ephemeris(true);
      
      // エフェメリスの設定
      
      // 前にエフェメリスがある場合
      if(space_node.has_satellite(prn)){
        available = true;
        // 新しいものが利用可能である場合は検索する
        if(!space_node.satellite(prn).ephemeris().maybe_newer_one_avilable(item.t_epoc)){
          search_ephemeris = false;
        }
      }
      
      if(search_ephemeris){
        
        // エフェメリスがあるか
        if(nav_data.find(prn) != nav_data.end()){
        
          // エフェメリスを検索
#ifdef __GNUC__
          // GCCはローカルスコープのstructをうまく扱ってくれないのでベタ書き
          nav_data_t::mapped_type::iterator eph1(nav_data[prn].begin());
          for(nav_data_t::mapped_type::iterator eph2(nav_data[prn].begin()); 
              (++eph2) != nav_data[prn].end();
              ){
            if(_abs(eph1->ephemeris.period_from_time_of_clock(item.t_epoc))
                > _abs(eph2->ephemeris.period_from_time_of_clock(item.t_epoc))){
              eph1 = eph2;
            }
          }
          space_node.satellite(prn).ephemeris() = eph1->ephemeris;
#else
          
          struct comparator {
            const gps_time_t &_t;
            comparator(const gps_time_t &t) : _t(t) {}
            bool operator()(
                const reader_n_t::SatelliteInfo &info1,
                const reader_n_t::SatelliteInfo &info2){
              return _abs(info1.ephemeris.period_from_time_of_clock(_t)) 
                  < _abs(info2.ephemeris.period_from_time_of_clock(_t));
            }
          };
          space_node.satellite(prn).ephemeris()
              = min_element(nav_data[prn].begin(), nav_data[prn].end(), 
                  comparator(item.t_epoc))->ephemeris;
#endif
          
          available = true;
          
          cerr << " @ New ephemeris: t_oc => "
               << space_node.satellite(prn).ephemeris().WN << " week " 
               << space_node.satellite(prn).ephemeris().t_oc;
        }
      }
      
      cerr << endl;
      
      if(!available){continue;}
      
      // 擬似距離の設定
      sat_range[prn] = pseudo;
    }
    
    // 航法計算の実行
    {
      matrix_t C;
      
      for(unsigned i(0); i < 10; i++){
        solve_1step_return_t res(
            solve_1step(
              space_node,
              sat_range,
              item.t_epoc,
              user_position,
              receiver_error,
              ((data_o_index == 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;
      
      // 測位誤差の検証
      xyz_t mtka_true(-3947762.7496, 3364399.8789, 3699428.5111);
      
      enu_t error(enu_t::relative(user_position, mtka_true));
      cerr << "error e, n, u => " << error << 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)));
      
      // 真値との差
      enu_t user_position_error(enu_t::relative(user_position, mtka_true));
      
      struct tm t_epoc_tm(item.t_epoc.c_tm(leap_seconds));
      
      // CSV形式で吐き出しておく
      cout << t_epoc_tm.tm_year << ", "
           << (t_epoc_tm.tm_mon + 1) << ", "
           << t_epoc_tm.tm_mday << ", "
           << t_epoc_tm.tm_hour << ", "
           << t_epoc_tm.tm_min << ", "
           << t_epoc_tm.tm_sec << ", "
           << rad2deg(user_position_llh.latitude()) << ", "
           << rad2deg(user_position_llh.longitude()) << ", "
           << user_position_llh.height() << ", "
           << user_position_error.east() << ","
           << user_position_error.north() << ","
           << user_position_error.up() << ","
           << gdop << ", "
           << pdop << ", "
           << hdop << ", "
           << vdop << ", "
           << tdop << endl;
    }
    
  }
  
  return 0;
}