/**
 * 第4章の内容
 * 
 * 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 <ctime>
#include <cmath>

using namespace std;

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

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

#ifndef NO_MAIN
  #define NO_MAIN
#else
  #define NO_MAIN_ALREADY_DEFINED_SECT4
#endif

#include "gps_programming_sect3.cpp"
#undef NO_MAIN

#ifdef NO_MAIN_ALREADY_DEFINED_SECT4
  #define NO_MAIN
  #undef NO_MAIN_ALREADY_DEFINED_SECT4
#endif

#ifndef NO_MAIN

typedef double precision_t;
typedef System_XYZ<precision_t> xyz_t;
typedef System_LLH<precision_t> llh_t;
typedef System_ENU<precision_t> enu_t;

typedef Matrix<precision_t> matrix_t;

typedef GPS_Time<> gps_time_t;
typedef GPS_SpaceNode<> space_node_t;

typedef map<int, precision_t> sat_range_t;

void read_RINEX(
    const char *target_file,
    const gps_time_t &target_time,
    space_node_t &space_node){
  
  // RINEXファイルの読み込み
  ifstream fin(target_file);
  if(fin.fail()){
    cout << "File not found!! : " << target_file << endl;
    exit(-1);
  }
  
  typedef RINEX_Nav_Reader<> reader_t;
  reader_t reader(fin);
  
  // 電離層遅延係数の処理
  if(!reader.extract_iono_coef(space_node)){
    exit(-1);
  }
  
  while(reader.has_next()){
    reader_t::SatelliteInfo info(reader.next());
    
    bool already_exist(space_node.has_satellite(info.svid));
    space_node_t::Satellite &sat(space_node.satellite(info.svid));
    //cout << "PRN: " << info.svid << "; WN: " << info.ephemeris.WN << "; t_oc: " << info.ephemeris.t_oc << endl;
    if(already_exist){
      if(_abs(target_time.interval(sat.ephemeris().WN, sat.ephemeris().t_oe))
          < _abs(target_time.interval(info.ephemeris.WN, info.ephemeris.t_oe))){
        continue;
      }
    }
    
    // 有効性の検証
    
    // 送信時刻よりも後であること
    if(target_time.interval(info.ephemeris.WN, info.t_ot) > 0){continue;}
    
    // 有効期限内であること
    if(!info.ephemeris.is_valid(target_time)){continue;}
    
    sat.ephemeris() = info.ephemeris;
  }
  
  fin.close();
}

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

/**
 * Iterationの1step
 * 
 * @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) {
  
  cout << "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;
    //cout << "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;
    //cout << "r2 => " << delta_r(i, 0) << endl; 
    
    // 衛星の位置を計算   
    xyz_t sat_position(sat.whereis(target_time_est, delta_r(i, 0)));
    cout << 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));
      //cout << "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));
      //cout << "tropo => " << tropo_delta_r << endl;
      delta_r(i, 0) += tropo_delta_r;
    }
  }
  //cout << "delta_r:" << delta_r << endl;
  //cout << "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);
  
  gps_time_t target_time;
  target_time.week = 1349;        // 05/11/13-19の週
  target_time.seconds = 86400.0;  // 月曜日の00:00:00
  
  space_node_t space_node;
  read_RINEX("mtka3180.05n", target_time, space_node);
  
  sat_range_t sat_range;
  sat_range[ 5] = 23545777.534;
  sat_range[14] = 20299789.570;
  sat_range[16] = 24027782.537;
  sat_range[22] = 24367716.061;
  sat_range[25] = 22169926.127;
  
  // 初期解の設定
  xyz_t user_position;
  precision_t receiver_error(0);
  
  matrix_t C;
  
  for(unsigned i(0); i < 10; i++){
    solve_1step_return_t res(
        solve_1step(
          space_node,
          sat_range,
          target_time,
          user_position,
          receiver_error,
          (i < 3)
        ));
    
    cout << "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;
    }
  }
  
  cout << "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));
  cout << "error e, n, u => " << error << endl;
  
  // DOPの表示
  cout << "GDOP: " << sqrt(C.trace()) << endl;
  cout << "PDOP: " << sqrt(C.partial(3, 3, 0, 0).trace()) << endl;
  cout << "HDOP: " << sqrt(C.partial(2, 2, 0, 0).trace()) << endl;
  cout << "VDOP: " << sqrt(C(2, 2)) << endl;
  cout << "TDOP: " << sqrt(C(3, 3)) << endl;
  
  return 0;
}

#endif