/**
 * 第3章の内容
 * 
 * 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 WITHOUT_SECT4_FEATURE
  #define NO_MAIN
#else
  #define NO_MAIN_ALREADY_DEFINED_SECT3
#endif

#include "gps_programming_sect2.cpp"
#undef NO_MAIN

#ifdef NO_MAIN_ALREADY_DEFINED_SECT3
  #define NO_MAIN
  #undef NO_MAIN_ALREADY_DEFINED_SECT3
#endif

#ifndef __GPS_PROGRAMMING_SECT3_HEADER
#define __GPS_PROGRAMMING_SECT3_HEADER

template <class FloatT = double>
struct GPS_Time {
  static const unsigned int seconds_day = 60UL * 60 * 24;
  static const unsigned int seconds_week = seconds_day * 7;
  
  static const int days_of_month[];
  
  unsigned int week;
  FloatT seconds;
  
  GPS_Time() {}
  GPS_Time(const GPS_Time &t)
      : week(t.week), seconds(t.seconds) {}
  void canonicalize(){
    if(seconds >= seconds_week){
      int quot(seconds / seconds_week);
      week += quot;
      seconds -= quot * seconds_week;
    }else if(seconds < 0){
      int quot((int)(-seconds / seconds_week) + 1);
      week -= quot;
      seconds += quot * seconds_week;
    }
  }
  GPS_Time(const struct tm &t, const FloatT leap_seconds = 0) {
    int days(-6);
    int y(t.tm_year - 80);
    if(y < 0){y += 100;}
    days += y * 365 + ((y + 3) / 4);
    for(int i(0); i < t.tm_mon; i++){
      days += days_of_month[i];
      if((i == 2) && (t.tm_year % 4 == 0)) days++;
    }
    days += t.tm_mday;
    
    week = days / 7;
    seconds = (days % 7) * seconds_day
        + t.tm_hour * 60 * 60
        + t.tm_min * 60
        + t.tm_sec + leap_seconds;
    canonicalize();
  }
  
  GPS_Time &operator+=(const FloatT &sec){
    seconds += sec;
    canonicalize();
    return *this;
  }
  GPS_Time &operator-=(const FloatT &sec){
    return operator+=(-sec);
  }
  GPS_Time operator+(const FloatT &sec) const {
    GPS_Time t(*this);
    return (t += sec);
  }
  GPS_Time operator-(const FloatT &sec) const {
    return operator+(-sec);
  }
  friend FloatT operator+(FloatT v, const GPS_Time &t){
    return v + ((t.week * seconds_week) + t.seconds);
  }
  friend FloatT operator-(FloatT v, const GPS_Time &t){
    return v - ((t.week * seconds_week) + t.seconds);
  }
  bool operator<(const GPS_Time &t) const {
    return ((week < t.week) ? true : ((week > t.week) ? false : (seconds < t.seconds)));
  }
  bool operator>(const GPS_Time &t) const {
    return t.operator<(*this);
  }
  bool operator==(const GPS_Time &t) const {
    return (week == t.week) && (seconds == t.seconds);
  }
  bool operator!=(const GPS_Time &t) const {
    return !(operator==(t));
  }
  bool operator<=(const GPS_Time &t) const {
    return ((week < t.week) ? true : ((week > t.week) ? false : (seconds <= t.seconds)));
  }
  bool operator>=(const GPS_Time &t) const {
    return t.operator<=(*this);
  }
  
  struct tm c_tm(const FloatT leap_seconds = 0) const {
    struct tm t;
    
    GPS_Time mod_t((*this) + leap_seconds);
    
    t.tm_min = (mod_t.seconds / 60);  t.tm_sec = (mod_t.seconds - t.tm_min * 60);
    t.tm_hour = (t.tm_min / 60);      t.tm_min -= t.tm_hour * 60;
    t.tm_mday = t.tm_hour / 24;       t.tm_hour -= t.tm_mday * 24;
    t.tm_wday = t.tm_mday;  
    t.tm_mday += 6 + (mod_t.week * 7);
    t.tm_year = (t.tm_mday / (365 * 3 + 366) * 4);
    t.tm_mday -= (t.tm_year / 4) * (365 * 3 + 366); 
    if(t.tm_mday > 366){t.tm_mday -= 366; (t.tm_year)++;}
    for(int i(0); i < 2; i++){
      if(t.tm_mday > 365){t.tm_mday -= 365; (t.tm_year)++;}
    }
    t.tm_yday = t.tm_mday;
    (t.tm_year += 80) %= 100;
    for(t.tm_mon = 0; 
        t.tm_mday > days_of_month[t.tm_mon];
        (t.tm_mon)++){
      if((t.tm_mon == 1) && (t.tm_year % 4 == 0)){
        if(t.tm_mday == 29){break;}
        else{t.tm_mday--;}
      }
      t.tm_mday -= days_of_month[t.tm_mon];
    }
    t.tm_isdst = 0;
    
    //cout << t.tm_year << " " << t.tm_mon << " " << t.tm_mday << " " 
    //     << t.tm_hour << ":" << t.tm_min << ":" << t.tm_sec << endl;
    
    return t;
  }
    
  
  /**
   * t >= self のとき正の値
   * t  < self のとき負の値
   */
  FloatT interval(const unsigned int &t_week, 
      const FloatT &t_seconds) const {
    return t_seconds - seconds
        + (t_week - week) * seconds_week;
  }
  /**
   * t >= self のとき正の値
   * t  < self のとき負の値
   */
  FloatT interval(const GPS_Time &t) const {
    return interval(t.week, t.seconds);
  }
  
  friend ostream &operator<<(ostream &out, const GPS_Time &t){
    out << t.week << " week " << t.seconds << " sec.";
    return out;
  }
};

template <class FloatT>
const int GPS_Time<FloatT>::days_of_month[] = {
      31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
    };

template <class FloatT = double>
class GPS_SpaceNode {
  public:
    static const FloatT light_speed;
    static const FloatT SC2RAD;
    
  protected:
    static FloatT rad2sc(const FloatT &rad) {return rad / PI;}
    static FloatT sc2rad(const FloatT &sc)  {return sc * PI;}
  protected:
    typedef unsigned char u8_t;
    typedef signed char s8_t;
    typedef unsigned short u16_t;
    typedef signed short s16_t;
    typedef unsigned int u32_t;
    typedef signed int s32_t;
    
    typedef int int_t;
    typedef unsigned int uint_t;
    
    typedef GPS_SpaceNode<FloatT> self_t;
    
    typedef GPS_Time<FloatT> gps_time_t; 
    typedef System_XYZ<FloatT> xyz_t;
    typedef System_LLH<FloatT> llh_t;
    typedef System_ENU<FloatT> enu_t;
  public:
    /**
     * GPS電離層遅延補正係数
     * 
     */
    struct IonosphericDelayCoef {
      FloatT alpha0;       ///< 電離層補正係数 (s)
      FloatT alpha1;       ///< 電離層補正係数 (s/sc)
      FloatT alpha2;       ///< 電離層補正係数 (s/sc^2)
      FloatT alpha3;       ///< 電離層補正係数 (s/sc^3)
      FloatT beta0;        ///< 電離層補正係数 (s)
      FloatT beta1;        ///< 電離層補正係数 (s/sc)
      FloatT beta2;        ///< 電離層補正係数 (s/sc^2)
      FloatT beta3;        ///< 電離層補正係数 (s/sc^3)
      FloatT A1;           ///< UTCパラメータ  (s/s)
      FloatT A0;           ///< UTCパラメータ  (s)
      FloatT t_ot;         ///< エポック時間(UTC) (s) 
      uint_t wn_t;         ///< エポック時間(UTC) (weeks)
      FloatT delte_t_LS;   ///< 現在の閏秒     (s)
      uint_t wn_LSF;       ///< 閏秒の更新週   (weeks)
      uint_t DN;           ///< 閏秒の更新日   (days)
      uint_t delta_t_LSF;  ///< 更新後の閏秒   (days)
      
      struct raw_t {
        s8_t  alpha0;       ///< 電離層補正係数 (-30, s)
        s8_t  alpha1;       ///< 電離層補正係数 (-27, s/sc)
        s8_t  alpha2;       ///< 電離層補正係数 (-24, s/sc^2)
        s8_t  alpha3;       ///< 電離層補正係数 (-24, s/sc^3)
        s8_t  beta0;        ///< 電離層補正係数 (11, s)
        s8_t  beta1;        ///< 電離層補正係数 (14, s/sc)
        s8_t  beta2;        ///< 電離層補正係数 (16, s/sc^2)
        s8_t  beta3;        ///< 電離層補正係数 (16, s/sc^3)
        s32_t A1;           ///< UTCパラメータ  (-50, s/s)
        s32_t A0;           ///< UTCパラメータ  (-30, s)
        u8_t  t_ot;         ///< エポック時間(UTC) (12E0, s) 
        u8_t  WN_t;         ///< エポック時間(UTC) (weeks)
        u8_t  delte_t_LS;   ///< 現在の閏秒     (s)
        u8_t  WN_LSF;       ///< 閏秒の更新週   (weeks)
        u8_t  DN;           ///< 閏秒の更新日   (days)
        u8_t  delta_t_LSF;  ///< 更新後の閏秒   (days)
        
        IonosphericDelayCoef convert() const {
          IonosphericDelayCoef converted;
#define CONVERT(TARGET, SF) \
{converted.TARGET = SF * TARGET;}
#define POWER_2(n) \
(((n) >= 0) \
  ? (FloatT)(1 << (n)) \
  : (((FloatT)1) / (1 << (-(n) >= 30 ? 30 : -(n))) \
    / (1 << (-(n) >= 30 ? (-(n) - 30) : 0))))
            CONVERT(alpha0,     POWER_2(-30));
            CONVERT(alpha1,     POWER_2(-27));
            CONVERT(alpha2,     POWER_2(-24));
            CONVERT(alpha3,     POWER_2(-24));
            CONVERT(beta0,      POWER_2( 11));
            CONVERT(beta1,      POWER_2( 14));
            CONVERT(beta2,      POWER_2( 16));
            CONVERT(beta3,      POWER_2( 16));
            CONVERT(A1,         POWER_2(-50));
            CONVERT(A0,         POWER_2(-30));
            CONVERT(t_ot,       12E0);
            converted.WN_t = WN_t;
            CONVERT(delte_t_LS, 1E0)
            converted.WN_LSF = WN_LSF;
            converted.DN = DN;
            converted.delta_t_LSF = delta_t_LSF;
#undef POWER_2
#undef CONVERT
          return converted;
        };
      };
    };
  public:
    class Satellite {
      public:
        
        /**
         * GPSエフェメリスデータ
         * (サブフレーム1,2,3)
         * 
         */
        struct Ephemeris {
          uint_t svid;        ///< 衛星番号
          
          // Subframe.1
          uint_t WN;          ///< 週番号
          int_t URA;          ///< 測距精度
          uint_t SV_health;   ///< 衛星健康状態
          int_t iodc;         ///< クロック情報番号
          FloatT t_GD;        ///< 郡遅延           (s)
          FloatT t_oc;        ///< エポック時刻
          FloatT a_f2;        ///< クロック補正係数 (s/s^2)
          FloatT a_f1;        ///< クロック補正係数 (s/s)
          FloatT a_f0;        ///< クロック補正係数 (s)
          
          // Subframe.2
          int_t iode;          ///< 軌道情報番号
          FloatT c_rs;         ///< 軌道補正係数 (m)
          FloatT delta_n;      ///<              (rad/s) 
          FloatT m0;           ///< 平均近点角   (rad)
          FloatT c_uc;         ///< 軌道補正係数 (rad)
          FloatT e;            ///< 離心率       
          FloatT c_us;         ///< 軌道補正係数 (rad)
          FloatT sqrt_A;       ///< 軌道半径     (sqrt(m))
          FloatT t_oe;         ///< エポック時間 (s)
          FloatT fit_interval; ///< フィット間隔
          
          // Subframe.3
          FloatT c_ic;         ///< 軌道補正係数 (rad)
          FloatT Omega0;       ///< 昇交点赤経   (rad)
          FloatT c_is;         ///< 軌道補正係数 (rad)
          FloatT i0;           ///< 軌道傾斜角   (rad)
          FloatT c_rc;         ///< 軌道補正係数 (m)
          FloatT omega;        ///< 近地点引数   (rad)
          FloatT dot_Omega0;   ///< Omega0変化率 (rad/s)
          FloatT dot_i0;       ///< i0変化率     (rad/s)
          
          FloatT period_from_time_of_clock(const gps_time_t &t) const {
            return -t.interval(WN, t_oc);
          }
        
          FloatT period_from_time_of_ephemeris(const gps_time_t &t) const {
            return -t.interval(WN, t_oe);
          }
          
          /**
           * 有効がどうかを返す
           * 
           * @param t GPS時刻
           */
          bool is_valid(const gps_time_t &t) const {
            //cout << fit_interval << endl;
            //cout << t.interval(WN, t_oc) << endl;
            //cout << t << ", " << t_oc << endl;
            return (_abs(t.interval(WN, t_oc)) <= (fit_interval / 2));
          }
          
          /**
           * 新しいものが利用可能であるか可能性を返す
           * 
           */
          bool maybe_newer_one_avilable(const gps_time_t &t) const {
            FloatT interval_to_t_oc(t.interval(WN, t_oc));
            return !((0 <= interval_to_t_oc) && (interval_to_t_oc <= (fit_interval / 2)));
          }
          
          struct raw_t {
            u8_t  svid;         ///< 衛星番号
            
            u16_t WN;           ///< 週番号
            u8_t  URA;          ///< 測距精度
            u8_t  SV_health;    ///< 衛星健康状態
            u16_t iodc;         ///< クロック情報番号
            s8_t  t_GD;         ///< 郡遅延           (-31, s)
            u16_t t_oc;         ///< エポック時刻
            s8_t  a_f2;         ///< クロック補正係数 (-55, s/s^2)
            s16_t a_f1;         ///< クロック補正係数 (-43, s/s)
            s16_t a_f0;         ///< クロック補正係数 (-31, s)
            
            u8_t  iode;         ///< 軌道情報番号
            s16_t c_rs;         ///< 軌道補正係数 (-5,  m)
            s16_t delta_n;      ///<              (-43, sc/s) 
            s32_t m0;           ///< 平均近点角   (-31, sc)
            s16_t c_uc;         ///< 軌道補正係数 (-29, rad)
            u32_t e;            ///< 離心率       (-33)
            s16_t c_us;         ///< 軌道補正係数 (-29, rad)
            u32_t sqrt_A;       ///< 軌道半径     (-19, sqrt(m))
            u16_t t_oe;         ///< エポック時間 (4,   s)
            bool fit_interval_flag;   ///< フィット間隔フラグ(ICD:20.3.4.4)
            
            s16_t c_ic;         ///< 軌道補正係数 (-29, rad)
            s32_t Omega0;       ///< 昇交点赤経   (-31, sc)
            s16_t c_is;         ///< 軌道補正係数 (-29, rad)
            s32_t i0;           ///< 軌道傾斜角   (-31, sc)
            s16_t c_rc;         ///< 軌道補正係数 (-5,  m)
            s32_t omega;        ///< 近地点引数   (-31, sc)
            s32_t dot_Omega0;   ///< Omega0変化率 (-43, sc/s)
            s16_t dot_i0;       ///< i0変化率     (-43, sc/s)
            
            Ephemeris convert() const {
              Ephemeris converted;
#define CONVERT(TARGET, SF) \
{converted.TARGET = SF * TARGET;}
#define POWER_2(n) \
(((n) >= 0) \
  ? (FloatT)(1 << (n)) \
  : (((FloatT)1) / (1 << (-(n) >= 30 ? 30 : -(n))) \
    / (1 << (-(n) >= 30 ? (-(n) - 30) : 0))))
              converted.svid = svid;
              
              converted.WN = WN;
              converted.URA = URA;
              converted.SV_health = SV_health;
              converted.iodc = iodc;
              CONVERT(t_GD, POWER_2(-31));
              converted.t_oc = t_oc;
              CONVERT(a_f0, POWER_2(-55));
              CONVERT(a_f1, POWER_2(-43));
              CONVERT(a_f2, POWER_2(-31));
              
              converted.iode = iode;
              CONVERT(c_rs,       POWER_2(-5));
              CONVERT(delta_n,    SC2RAD * POWER_2(-43));
              CONVERT(m0,         SC2RAD * POWER_2(-31));
              CONVERT(c_uc,       POWER_2(-29));
              CONVERT(e,          POWER_2(-33));
              CONVERT(c_us,       POWER_2(-29));
              CONVERT(sqrt_A,     POWER_2(-19));
              CONVERT(t_oe,       POWER_2(4));
              
              CONVERT(c_ic,       POWER_2(-29));
              CONVERT(Omega0,     SC2RAD * POWER_2(-31));
              CONVERT(c_is,       POWER_2(-29));
              CONVERT(i0,         SC2RAD * POWER_2(-31));
              CONVERT(c_rc,       POWER_2(-5));
              CONVERT(omega,      SC2RAD * POWER_2(-31));
              CONVERT(dot_Omega0, SC2RAD * POWER_2(-43));
              CONVERT(dot_i0,     SC2RAD * POWER_2(-43));
#undef POWER_2
#undef CONVERT
              // From ICD:20.3.4.4
              if(fit_interval_flag == false){
                // normal operation
                converted.fit_interval = 4 * 60 * 60;
              }else{
                // short/long-trem extended operation
                if(iodc >= 240 && iodc <= 247){
                  converted.fit_interval = 8 * 60 * 60;
                }else if((iodc >= 248 && iodc <= 255) || (iodc == 496)){
                  converted.fit_interval = 14 * 60 * 60;
                }else if(iodc >= 497 && iodc <= 503){
                  converted.fit_interval = 26 * 60 * 60;
                }else if(iodc >= 504 && iodc <= 510){
                  converted.fit_interval = 50 * 60 * 60;
                }else if((iodc == 511) || (iodc >= 752 && iodc <= 756)){
                  converted.fit_interval = 74 * 60 * 60;
                }else if(iodc >= 757 && iodc <= 763){
                  converted.fit_interval = 98 * 60 * 60;
                }else if((iodc >= 764 && iodc <= 767) || (iodc >= 1008 && iodc <= 1010)){
                  converted.fit_interval = 122 * 60 * 60;
                }else if(iodc >= 1011 && iodc <= 1020){
                  converted.fit_interval = 146 * 60 * 60;
                }else{
                  converted.fit_interval = 6 * 60 * 60;
                }
              }
              
              return converted;
            }
          };
        };
        
        /**
         * GPSアルマナック
         * (サブフレーム4,5より得られる)
         * 
         */
        struct Almanach {
          uint_t  svid;        ///< 衛星番号
          
          FloatT e;            ///< 離心率
          FloatT t_oa;         ///< エポック時刻 (s)
          FloatT delta_i;      ///< 軌道傾斜角   (rad)
          FloatT dot_Omega0;   ///< Omega0変化率 (rad/s)
          uint_t sv_health;    ///< 衛星健康状態
          FloatT sqrt_A;       ///< 軌道半径     (sqrt(m))
          FloatT Omega0;       ///< 昇交点赤経   (rad)
          FloatT omega;        ///< 近地点引数   (rad)
          FloatT M0;           ///< 平均近点角   (rad)
          FloatT a_f0;         ///< クロック補正係数 (s)
          FloatT a_f1;         ///< クロック補正係数 (s)
          
          /**
           * Ephemerisにキャストできるようにする
           * 
           */
          operator Ephemeris() const {
            Ephemeris converted;
            
            converted.svid          = svid;
            
            // Subframe.1
            converted.WN            = 0;          ///< 週番号、あとで設定すること
            converted.URA           = -1;         ///< 測距精度
            converted.SV_health     = sv_health;  ///< 衛星健康状態
            converted.iodc          = -1;         ///< クロック情報番号
            converted.t_GD          = 0;          ///< 郡遅延
            converted.t_oc          = t_oa;       ///< エポック時刻
            converted.a_f2          = 0;          ///< クロック補正係数 (s/s^2)
            converted.a_f1          = a_f1;       ///< クロック補正係数 (s/s)
            converted.a_f0          = a_f0;       ///< クロック補正係数 (s)
            
            // Subframe.2
            converted.iode          = -1;         ///< 軌道情報番号
            converted.c_rs          = 0;          ///< 軌道補正係数 (m)
            converted.delta_n       = 0;          ///<              (rad/s) 
            converted.m0            = M0;         ///< 平均近点角   (rad)
            converted.c_uc          = 0;          ///< 軌道補正係数 (rad)
            converted.e             = e;          ///< 離心率       
            converted.c_us          = 0;          ///< 軌道補正係数 (rad)
            converted.sqrt_A        = sqrt_A;     ///< 軌道半径     (sqrt(m))
            converted.t_oe          = t_oa;       ///< エポック時間 (s)
            converted.fit_interval  = 4 * 60 * 60;///< フィット間隔
            
            // Subframe.3
            converted.c_ic          = 0;          ///< 軌道補正係数 (rad)
            converted.Omega0        = Omega0;     ///< 昇交点赤経   (rad)
            converted.c_is          = 0;          ///< 軌道補正係数 (rad)
            converted.i0            = delta_i;    ///< 軌道傾斜角   (rad)
            converted.c_rc          = 0;          ///< 軌道補正係数 (m)
            converted.omega         = omega;      ///< 近地点引数   (rad)
            converted.dot_Omega0    = dot_Omega0; ///< Omega0変化率 (rad/s)
            converted.dot_i0        = 0;          ///< i0変化率     (rad/s)
            
            return converted;
          }
          
          struct raw_t {
            u8_t  svid;         ///< 衛星番号
            
            u16_t e;            ///< 離心率       (-21)
            u8_t  t_oa;         ///< エポック時刻 (12, s)
            u16_t delta_i;      ///< 軌道傾斜角   (-19, sc)
            u16_t dot_Omega0;   ///< Omega0変化率 (-38, sc/s)
            u8_t  sv_health;    ///< 衛星健康状態
            u32_t sqrt_A;       ///< 軌道半径     (-11, sqrt(m))
            u32_t Omega0;       ///< 昇交点赤経   (-23, sc)
            u32_t omega;        ///< 近地点引数   (-23, sc)
            u32_t M0;           ///< 平均近点角   (-23, sc)
            u16_t a_f0;         ///< クロック補正係数 (-20, s)
            u16_t a_f1;         ///< クロック補正係数 (-38, s)
            
            Almanach convert() const {
              Almanach converted;
#define CONVERT(TARGET, SF) \
{converted.TARGET = SF * TARGET;}
#define POWER_2(n) \
(((n) >= 0) \
  ? (FloatT)(1 << (n)) \
  : (((FloatT)1) / (1 << (-(n) >= 30 ? 30 : -(n))) \
    / (1 << (-(n) >= 30 ? (-(n) - 30) : 0))))
                converted.svid = svid;
                CONVERT(e,          POWER_2(-21));
                CONVERT(t_oa,       POWER_2(12));
                CONVERT(delta_i,    SC2RAD * POWER_2(-19));
                CONVERT(dot_Omega0, SC2RAD * POWER_2(-38));
                converted.sv_health = sv_health;
                CONVERT(sqrt_A,     POWER_2(-11));
                CONVERT(Omega0,     SC2RAD * POWER_2(-23));
                CONVERT(omega,      SC2RAD * POWER_2(-23));
                CONVERT(M0,         SC2RAD * POWER_2(-23));
                CONVERT(a_f0,       POWER_2(-20));
                CONVERT(a_f1,       POWER_2(-38));
#undef POWER_2
#undef CONVERT
              return converted;
            }
          };
        };
      protected:
        Ephemeris _ephemeris;
         
      public:
        Ephemeris &ephemeris() {return _ephemeris;}
        
        FloatT eccentric_anomaly(const FloatT &period_from_toe) const {
          // 平均近点角M(Mk)
          FloatT n0(sqrt(Earth::mu_Earth) / pow3(_ephemeris.sqrt_A));
          FloatT Mk(_ephemeris.m0
              + (n0 + _ephemeris.delta_n) * period_from_toe);
          
          // 離心近点角E(Ek)
          FloatT Ek(Mk);
#ifndef KEPLER_DELTA_LIMIT
#define KEPLER_DELTA_LIMIT 1E-12
#endif 
          for(int loop(0); loop < 10; loop++){
            FloatT Ek2(Mk + _ephemeris.e * sin(Ek));
            if(_abs(Ek2 - Ek) < KEPLER_DELTA_LIMIT){break;}
            Ek = Ek2;
          }
          
          return Ek;
        }
        
        FloatT eccentric_anomaly(const gps_time_t &t) const {
          return eccentric_anomaly(_ephemeris.period_from_time_of_ephemeris(t));
        }
        
        FloatT clock_error(const gps_time_t &t, const FloatT pseudo_range = 0) const{
          FloatT tk(_ephemeris.period_from_time_of_clock(t));
#ifdef WITHOUT_SECT4_FEATURE
          FloatT tr(0);
#else
          // 伝播時間の考慮
          FloatT period_propagation(pseudo_range / self_t::light_speed);

          // 相対論補正
          FloatT tr(-2.0 * sqrt(Earth::mu_Earth) / pow2(self_t::light_speed) 
              * _ephemeris.e * _ephemeris.sqrt_A 
              * sin(eccentric_anomaly(_ephemeris.period_from_time_of_ephemeris(t) - period_propagation)));
          //cout << "tr => " << tr << endl;
              
          // 伝播時間の除去
          tk -= period_propagation;
#endif
          
          FloatT dt(_ephemeris.a_f0
              + _ephemeris.a_f1 * tk
              + _ephemeris.a_f2 * pow2(tk));
          
          return dt + tr - _ephemeris.t_GD;
        }
        
        xyz_t whereis(const gps_time_t &t, const FloatT pseudo_range = 0) const {
          // 時間差を求める
          FloatT tk0(_ephemeris.period_from_time_of_ephemeris(t));
          FloatT tk(tk0);

#ifndef WITHOUT_SECT4_FEATURE
          // 伝播時間の除去
          tk -= pseudo_range / self_t::light_speed;
#endif
          //cout << "tk => " << tk << endl;

          // 離心近点角E(Ek)
          FloatT Ek(eccentric_anomaly(tk));
          //cout << "Ek => " << Ek << endl;
          
          // 動径長(rk)
          FloatT rk(pow2(_ephemeris.sqrt_A) 
              * (1.0 - _ephemeris.e * cos(Ek)));
          
          // 真近点角theta(vk)
          FloatT vk(atan2(
              sqrt(1.0 - pow2(_ephemeris.e)) * sin(Ek),
              cos(Ek) - _ephemeris.e));
          
          // 緯度引数(pk) [rad]
          FloatT pk(vk + _ephemeris.omega);
          
          // 軌道傾斜角(ik)
          FloatT ik(_ephemeris.i0);
          
          // 補正係数の適用
          {
            FloatT pk2_sin(sin(pk * 2)),
                   pk2_cos(cos(pk * 2));
            FloatT d_uk(
                _ephemeris.c_us * pk2_sin
                + _ephemeris.c_uc * pk2_cos);
            FloatT d_rk(
                _ephemeris.c_rs * pk2_sin
                + _ephemeris.c_rc * pk2_cos);
            FloatT d_ik(
                _ephemeris.c_is * pk2_sin
                + _ephemeris.c_ic * pk2_cos);
            
            pk += d_uk;
            rk += d_rk;
            ik += d_ik
                + _ephemeris.dot_i0 * tk;
          }
          //cout << "ik => " << ik << endl;
          
          // 軌道面内での位置(xk, yk)
          FloatT xk(rk * cos(pk)),
                 yk(rk * sin(pk));
          //cout << "xk, yk => " << xk << ", " << yk << endl;
          
          // 昇交点の経度(Omegak) [rad]
//#define __MISUNDERSTANDING_ABOUT_OMEGA0_CORRECTION__
#ifdef __MISUNDERSTANDING_ABOUT_OMEGA0_CORRECTION__
          FloatT Omegak(_ephemeris.Omega0
              + (_ephemeris.dot_Omega0 - Earth::Omega_Earth_IAU) * tk0
              - Earth::Omega_Earth_IAU * _ephemeris.t_oe);
#else
          FloatT Omegak(_ephemeris.Omega0
              + _ephemeris.dot_Omega0 * tk                          // corrected with the time when the wave is transmitted
              - Earth::Omega_Earth_IAU * (_ephemeris.t_oe + tk0));  // corrected with the time when the wave is received
#endif
          //cout << "Omega0 => " << _ephemeris.Omega0 << endl;
          //cout << Earth::Omega_Earth << endl;
          //cout << "Omegak => " << Omegak << endl;
          
          FloatT Omegak_sin(sin(Omegak)),
                 Omegak_cos(cos(Omegak));
          FloatT ik_sin(sin(ik)),
                 ik_cos(cos(ik));
          //cout << Omegak_cos << ", " << Omegak_sin << endl;
          
          // 結果の出力
          return xyz_t(
              xk * Omegak_cos - yk * Omegak_sin * ik_cos,
              xk * Omegak_sin + yk * Omegak_cos * ik_cos,
              yk * ik_sin);
        }
    };
  public:
    typedef map<int, Satellite> satellites_t;
  protected:
    IonosphericDelayCoef _iono_coef;
    satellites_t _satellites;
  public:
    GPS_SpaceNode() : _satellites() {
    }
    ~GPS_SpaceNode(){
      _satellites.clear();
    }
    IonosphericDelayCoef &iono_coef() {
      return _iono_coef;
    }
    satellites_t &satellites() {
      return _satellites;
    }
    Satellite &satellite(int prn) {
      return _satellites[prn];
    }
    bool has_satellite(int prn) const {
      return _satellites.find(prn) !=  _satellites.end();
    }

#ifndef WITHOUT_SECT4_FEATURE    
    /**
     * 電離層遅延補正量(単位はm)
     * 
     * @param relative_pos 相対座標系での位置
     */
    FloatT iono_correction(
        const enu_t &relative_pos,
        const llh_t &usrllh,
        const gps_time_t &t) const {
      
      
      // 仰角と方位角
      FloatT el(relative_pos.elevation()),
             az(relative_pos.azimuth());
      FloatT sc_el(rad2sc(el)),
             sc_az(rad2sc(az));
             
      // ピアースポイント
      FloatT psi(0.0137 / (sc_el + 0.11) - 0.022);
      FloatT phi_i(rad2sc(const_cast<llh_t *>(&usrllh)->latitude()) 
          + psi * cos(az));
      if(phi_i > 0.416){phi_i = 0.416;}
      else if(phi_i < -0.416){phi_i = -0.416;}
      FloatT lambda_i(rad2sc(const_cast<llh_t *>(&usrllh)->longitude())
          + psi * sin(az) / cos(sc2rad(phi_i)));
      FloatT phi_m(phi_i 
          + 0.064 * cos(sc2rad(lambda_i - 1.617)));
      
      // 地方時
      FloatT lt(4.32E4 * lambda_i + t.seconds);
      while(lt > gps_time_t::seconds_day){
        lt -= gps_time_t::seconds_day;
      }
      while(lt < 0){
        lt += gps_time_t::seconds_day;
      }
      
      // コサイン関数の周期と振幅 
      FloatT amp(0), per(0);
      const FloatT *alpha[] = {
          &(_iono_coef.alpha0), &(_iono_coef.alpha1),
          &(_iono_coef.alpha2), &(_iono_coef.alpha3)};
      const FloatT *beta[] = {
          &(_iono_coef.beta0), &(_iono_coef.beta1),
          &(_iono_coef.beta2), &(_iono_coef.beta3)};
      FloatT phi_mn(1.);
      for(int i(0); i < 4; i++){
        amp += *(alpha[i]) * phi_mn;
        per += *(beta[i]) * phi_mn;
        phi_mn *= phi_m;
      }
      amp = max(amp, FloatT(0));
      per = max(per, FloatT(72000));
      
      // 傾斜係数
      FloatT F(1.0 + 16.0 * pow((0.53 - sc_el), 3));
      
      FloatT x(PI * 2 * (lt - 50400) / per);
      if(x > PI){
        do{x -= PI * 2;}while(x > PI);
      }else if(x < -PI){
        do{x += PI * 2;}while(x < -PI);
      }
      
      FloatT T_iono(5E-9);
      if(_abs(x) < 1.57){
        T_iono += amp * (1. - pow2(x) * (1.0 / 2 - pow2(x) / 24)); // ICD p.148
      }
      T_iono *= F;
      
      return -T_iono * self_t::light_speed; 
    }
    
    FloatT iono_correction(
        const xyz_t &sat,
        const xyz_t &usr,
        const gps_time_t &t) const {
      return iono_correction(
          enu_t::relative(sat, usr), 
          usr.llh(), 
          t);
    }
    
    /**
     * 対流圏遅延補正量(単位はm)
     * 
     */
    FloatT tropo_correction(
        const enu_t &relative_pos,
        const llh_t &usrllh) const {
      
      // 仰角(単位はrad)
      FloatT el(relative_pos.elevation());
      
      // 高度(m)
      FloatT h(const_cast<llh_t *>(&usrllh)->height());
      FloatT f(1.0);
      if(h > (1.0 / 2.3E-5)){
        f = 0;
      }else if(h > 0){
        f -= h * 2.3E-5;
      }
      
      return -2.47 * pow(f, 5) / (sin(el) + 0.0121);
    }
    
    FloatT tropo_correction(
        const xyz_t &sat,
        const xyz_t &usr) const {
      return tropo_correction(
          enu_t::relative(sat, usr),
          usr.llh());
    }
#endif // WITHOUT_SECT4_FEATURE
};

template <class FloatT>
const FloatT GPS_SpaceNode<FloatT>::light_speed = 2.99792458E8;

template <class FloatT>
const FloatT GPS_SpaceNode<FloatT>::SC2RAD = 3.1415926535898;

class RINEX_Reader {
  public:
    typedef map<string, string> header_t;
    
  protected:
    header_t _header;
    istream &src;
    bool _has_next;
    
  public:
    RINEX_Reader(
          istream &in,
          string &(*modify_header)(string &, string &) = NULL) 
        : src(in), _has_next(false){
      if(src.fail()){return;}
      
      char buf[256];
      
      // ヘッダの読み出し
      while(!src.eof()){
        src.getline(buf, sizeof(buf));
        
        string content(buf);
        string label(content, 60, 20);
        {
          int real_length(label.find_last_not_of(' ') + 1);
          if(real_length < label.length()){
            label = label.substr(0, real_length);
          }
        }
        //cout << label << " (" << label.length() << ")" << endl;
        
        if(label.find("END OF HEADER") == 0){break;}
        
        content = content.substr(0, 60);
        if(modify_header){content = modify_header(label, content);}
        _header[label] = content;
      }
    }
    virtual ~RINEX_Reader(){_header.clear();}
    header_t &header() {return _header;}
    bool has_next() const {return _has_next;}
};

template <class FloatT = double>
class RINEX_Nav_Reader : public RINEX_Reader {
  protected:
    typedef RINEX_Nav_Reader<FloatT> self_t;
    typedef RINEX_Reader super_t;
  public:
    typedef GPS_Time<FloatT> gps_time_t;
    typedef typename GPS_SpaceNode<FloatT>::Satellite::Ephemeris ephemeris_t;
    
    struct SatelliteInfo {
      unsigned int svid;
      FloatT t_ot;  ///< 送信時刻[s]
      ephemeris_t ephemeris;
    };
  
  protected:
    SatelliteInfo info;
    static string &modify_header(string &label, string &content){
      if((label.find("ION ALPHA") == 0)
          || (label.find("ION BETA") == 0)
          || (label.find("DELTA-UTC: A0,A1,T,W") == 0)){
        for(int pos(0);
            (pos = content.find("D", pos)) != string::npos;
            ){
          content.replace(pos, 1, "E");
        }
      }
      return content;
    }
    
    void seek_next() {
      char buf[256];
      
      for(int i = 0; (i < 8) && (super_t::src.good()); i++){
        if(super_t::src.getline(buf, sizeof(buf)).fail()){return;}
        string line_data(buf);
        for(int pos(0);
            (pos = line_data.find("D", pos)) != string::npos;
            ){
          line_data.replace(pos, 1, "E");
        }
        stringstream data(line_data);
        
        FloatT dummy;
        switch(i){
          case 0: {
            data >> info.svid;
            struct tm t;
            data >> t.tm_year; // 年
            data >> t.tm_mon;  // 月
            --(t.tm_mon);
            data >> t.tm_mday; // 日
            data >> t.tm_hour; // 時 
            data >> t.tm_min;  // 分
            t.tm_sec = 0;
            gps_time_t gps_time(t);
            info.ephemeris.WN = gps_time.week;
            data >> info.ephemeris.t_oc; // 秒
            info.ephemeris.t_oc += gps_time.seconds;
            data >> info.ephemeris.a_f0;
            data >> info.ephemeris.a_f1;
            data >> info.ephemeris.a_f2;
            break;
          }
#define READ_AND_STORE(line_num, v0, v1, v2, v3) \
case line_num: { \
  FloatT v; \
  data >> v; v0 = v; \
  data >> v; v1 = v; \
  data >> v; v2 = v; \
  data >> v; v3 = v; \
  break; \
}
          READ_AND_STORE(1,
            info.ephemeris.iode,
            info.ephemeris.c_rs,
            info.ephemeris.delta_n,
            info.ephemeris.m0);
          READ_AND_STORE(2,
            info.ephemeris.c_uc,
            info.ephemeris.e,
            info.ephemeris.c_us,
            info.ephemeris.sqrt_A);
          READ_AND_STORE(3,
            info.ephemeris.t_oe,
            info.ephemeris.c_ic,
            info.ephemeris.Omega0,
            info.ephemeris.c_is);
          READ_AND_STORE(4,
            info.ephemeris.i0,
            info.ephemeris.c_rc,
            info.ephemeris.omega,
            info.ephemeris.dot_Omega0);
          READ_AND_STORE(5,
            info.ephemeris.dot_i0,
            dummy,
            info.ephemeris.WN,
            dummy);
          READ_AND_STORE(6,
            info.ephemeris.URA,
            info.ephemeris.SV_health,
            info.ephemeris.t_GD,
            info.ephemeris.iodc);
          READ_AND_STORE(7,
            info.t_ot,
            info.ephemeris.fit_interval,
            dummy,
            dummy);
#undef READ_AND_STORE
        }
      }
      
      if(info.ephemeris.fit_interval == 0){
        info.ephemeris.fit_interval = 4 * 60 * 60; // 最低4時間は有効
      }
      
      super_t::_has_next = true;
    }
  
  public:
    RINEX_Nav_Reader(istream &in) : super_t(in, self_t::modify_header) {
      seek_next();
    }
    ~RINEX_Nav_Reader(){}
    
    SatelliteInfo next() {
      SatelliteInfo current(info);
      super_t::_has_next = false;
      seek_next();
      return current;
    }
    
    /**
     * 電離層遅延係数を取得し、セットする
     * 
     * @return 成功した場合true、それ以外false
     */
    bool extract_iono_coef(GPS_SpaceNode<FloatT> &space_node) const {
      if(_header.find("ION ALPHA") != _header.end()){
        stringstream sstr(const_cast<super_t::header_t *>(&(_header))
            ->operator[]("ION ALPHA"));
        sstr >> space_node.iono_coef().alpha0;
        sstr >> space_node.iono_coef().alpha1;
        sstr >> space_node.iono_coef().alpha2;
        sstr >> space_node.iono_coef().alpha3;
      }else{
        return false;
      }
      
      if(_header.find("ION BETA") != _header.end()){
        stringstream sstr(const_cast<super_t::header_t *>(&(_header))
            ->operator[]("ION BETA"));
        sstr >> space_node.iono_coef().beta0;
        sstr >> space_node.iono_coef().beta1;
        sstr >> space_node.iono_coef().beta2;
        sstr >> space_node.iono_coef().beta3;
      }else{
        return false;
      }
      
      return true;
    }
};

#endif // __GPS_PROGRAMMING_SECT3_HEADER

#ifndef NO_MAIN

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

typedef Matrix<precision_t> matrix_t;

int main(int argc, char *argv[]) {
  
  typedef GPS_Time<> gps_time_t;
  gps_time_t target_time;
  target_time.week = 1349;        // 05/11/13-19の週
  target_time.seconds = 86400.0;  // 月曜日の00:00:00
  
  typedef map<int, precision_t> sat_range_t;
  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;
  
  typedef GPS_SpaceNode<> space_node_t;
  space_node_t space_node;
  
  // RINEXファイルの読み込み
  {
    const char *target_file = "mtka3180.05n";
    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);
    
    /*for(reader_t::header_t::iterator it(reader.header().begin());
        it != reader.header().end();
        ++it){
      cout << "\"" << it->first << "\"" << endl;      
    }*/
    
    // 電離層遅延係数の処理
    if(!reader.extract_iono_coef(space_node)){
      cout << "Iono coef not found!!" << endl;
      exit(-1);
    }
    
    while(reader.has_next()){
      reader_t::SatelliteInfo info(reader.next());
      
      if(sat_range.find(info.svid) != sat_range.end()){
        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;
          }
        }
        sat.ephemeris() = info.ephemeris;
      }
    }
    
    fin.close();
  }
  
  // 初期解の設定
  xyz_t user_position;
  precision_t receiver_error(0);
  
  // デザイン行列の作成
  while(true){
    matrix_t G(sat_range.size(), 4);
    matrix_t delta_r(sat_range.size(), 1);
    unsigned i(0);
    for(sat_range_t::iterator it(sat_range.begin());
        it != sat_range.end();
        ++it, ++i){
      precision_t range(it->second);
      space_node_t::Satellite &sat(space_node.satellite(it->first));
      xyz_t sat_position(sat.whereis(target_time));
      precision_t range_est(user_position.dist(sat_position));
      precision_t pseudo_range_calib(sat.clock_error(target_time)
          * space_node_t::light_speed);
      cout << it->first << ": " << sat_position << endl;
      delta_r(i, 0) = range 
          + pseudo_range_calib
          - range_est
          - receiver_error;
      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;
    }
    //cout << "delta_r:" << delta_r << endl;
    //cout << "G:" << G << endl;
    
    // 最小二乗法の適用
    matrix_t delta_x(
        (G.transpose() * G).inverse() * G.transpose() * delta_r);
    xyz_t delta_user_position(delta_x.partial(3, 1, 0, 0));
    
    user_position += delta_user_position;
    receiver_error += delta_x(3, 0);
    cout << "user: " << user_position
         << "; pusedo_error: " << receiver_error
         << endl;
    
    if(delta_user_position.dist() <= 1E-6){break;}
  }
  
  return 0;
}

#endif