#ifndef __GPS_H__
#define __GPS_H__

/**
 * GPSに関するヘッダ
 * C/AコードやEphemerisについて記述
 * 
 */

#include <map>    // C67用は考える必要あり
#include <bitset>
#include <ctime>

#include "WGS84.h"
#include "util/util.h"
#include "util/iostream_format.h"

#include "coordinate.h"

#ifdef pow2
#define POW2_ALREADY_DEFINED
#else
#define pow2(x) ((x)*(x))
#endif
#ifdef pow3
#define POW3_ALREADY_DEFINED
#else
#define pow3(x) ((x)*(x)*(x))
#endif

template <class FloatT = double>
class CA_Code {
  public:
    static const FloatT FREQENCY;
    static const FloatT LENGTH_1CHIP;
  public:
    class PRN {
      public:
        typedef std::bitset<10> content_t;
      protected:
        content_t _content;
      public:
        void reset(){_content.set();}
        PRN() : _content() {reset();}
        virtual ~PRN(){}
        virtual bool get() const = 0;
        virtual void next() = 0;
#if ENABLE_IOSTREAM
        friend std::ostream &operator<<(std::ostream &out, const PRN &prn){
          out << const_cast<PRN *>(&prn)->_content;
          return out;
        }
#endif
    };
    
    class G1 : public PRN {
      public:
        G1() : PRN() {}
        ~G1(){}
        bool get() const {return PRN::_content[9];}
        void next(){
          bool tmp(PRN::_content[2] ^ PRN::_content[9]);
          PRN::_content <<= 1;
          PRN::_content[0] = tmp;
        }
    };
    
    class G2 : public PRN {
      protected:
        const int _selector1, _selector2;
      public:
        G2(int selector1, int selector2) : PRN(), _selector1(selector1), _selector2(selector2) {}
        ~G2(){}
        bool get() const {return PRN::_content[_selector1] ^ PRN::_content[_selector2];}
        void next(){
          bool tmp(PRN::_content[1] 
                     ^ PRN::_content[2]
                     ^ PRN::_content[5]
                     ^ PRN::_content[7]
                     ^ PRN::_content[8]
                     ^ PRN::_content[9]);
          PRN::_content <<= 1;
          PRN::_content[0] = tmp;
        }
        static G2 Satellite_G2(int satellite_ID){
          switch(satellite_ID){
            case  1: return G2(1, 5);
            case  2: return G2(2, 6);
            case  3: return G2(3, 7);
            case  4: return G2(4, 8);
            case  5: return G2(0, 8);
            case  6: return G2(1, 9);
            case  7: return G2(0, 7);
            case  8: return G2(1, 8);
            case  9: return G2(2, 9);
            case 10: return G2(1, 2);
            case 11: return G2(2, 3);
            case 12: return G2(4, 5);
            case 13: return G2(5, 6);
            case 14: return G2(6, 7);
            case 15: return G2(7, 8);
            case 16: return G2(8, 9);
            case 17: return G2(0, 3);
            case 18: return G2(1, 4);
            case 19: return G2(2, 5);
            case 20: return G2(3, 6);
            case 21: return G2(4, 7);
            case 22: return G2(5, 8);
            case 23: return G2(0, 2);
            case 24: return G2(3, 5);
            case 25: return G2(4, 6);
            case 26: return G2(5, 7);
            case 27: return G2(6, 8);
            case 28: return G2(7, 9);
            case 29: return G2(0, 5);
            case 30: return G2(1, 6);
            case 31: return G2(2, 7);
            case 32: return G2(3, 8);
            case 33: return G2(4, 9);
            case 34: return G2(3, 9);
            case 35: return G2(0, 6);
            case 36: return G2(1, 7);
            default:  return G2(3, 9);
          }
        }
    };
  protected:
    G1 _g1;
    G2 _g2;
  public:
    CA_Code(int sid) : _g1(), _g2(G2::Satellite_G2(sid)){}
    ~CA_Code(){}
    bool get() const {return _g1.get() ^ _g2.get();}
    int get_multi() const {return get() ? 1 : -1;}
    void next(){
      _g1.next();
      _g2.next();
    }
};

template <class FloatT>
const FloatT CA_Code<FloatT>::FREQENCY = 1.023E6;
template <class FloatT>
const FloatT CA_Code<FloatT>::LENGTH_1CHIP = 1. / CA_Code<FloatT>::FREQENCY;

template <class FloatT = double>
struct GPS_Time {
  static const unsigned int seconds_day = 60U * 60 * 24;
  static const unsigned int seconds_week = seconds_day * 7;
  
  static const int days_of_month[];
  
  int week;
  FloatT seconds;
  
  GPS_Time() {}
  GPS_Time(const GPS_Time &t)
      : week(t.week), seconds(t.seconds) {}
  GPS_Time(const int &_week, const FloatT &_seconds)
      : week(_week), seconds(_seconds) {}
  void canonicalize(){
    if(seconds >= seconds_week){
      int quot((int)(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 = leap_seconds + (days % 7) * seconds_day
        + t.tm_hour * 60 * 60
        + t.tm_min * 60
        + t.tm_sec;
    canonicalize();
  }
  static GPS_Time now(const FloatT leap_seconds = 0) {
    time_t timer;
    struct tm t;
    
    time(&timer); // 現在時刻
#if defined(_MSC_VER)
    gmtime_s(&t, &timer); // 現在時刻を構造体に変換
#elif defined(__GNUC__)
    gmtime_r(&timer, &t); // 現在時刻を構造体に変換
#else
    t = *gmtime(&timer);
#endif
    
    return GPS_Time(t, leap_seconds);
  }
  
  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);
  }
  /**
   * 時間差を求める、戻り値はsec単位
   */
  FloatT operator-(const GPS_Time &t) const {
    FloatT res(seconds - t.seconds);
    res += (week - t.week) * seconds_week;
    return res;
  }
  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 = (int)(mod_t.seconds / 60);  t.tm_sec = (int)(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);
  }

#if ENABLE_IOSTREAM
  friend std::ostream &operator<<(std::ostream &out, const GPS_Time &t){
    out << t.week << " week " << t.seconds << " sec.";
    return out;
  }
#endif
};

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 L1_Frequency;
    static const FloatT SC2RAD;
    
  protected:
    static FloatT rad2sc(const FloatT &rad) {return rad / PI;}
    static FloatT sc2rad(const FloatT &sc)  {return sc * PI;}
    
  public:
    typedef GPS_SpaceNode<FloatT> self_t;
    typedef GPS_Time<FloatT> gps_time_t; 
    typedef System_XYZ<FloatT, WGS84> xyz_t;
    typedef System_LLH<FloatT, WGS84> llh_t;
    typedef System_ENU<FloatT, WGS84> enu_t;
  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;
  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(WGS84::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));

          // 伝播時間の考慮
          FloatT period_propagation(pseudo_range / self_t::light_speed);

          // 相対論補正
          FloatT tr(-2.0 * sqrt(WGS84::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;
          
          FloatT dt(_ephemeris.a_f0
              + _ephemeris.a_f1 * tk
              + _ephemeris.a_f2 * pow2(tk));
          
          return dt + tr - _ephemeris.t_GD;
        }
        
        struct constellation_t {
          xyz_t pos;
          xyz_t vel;
        };
        
        constellation_t constellation(
            const gps_time_t &t, const FloatT &pseudo_range = 0,
            const bool &with_velocity = true) const {
          
          constellation_t res;
          
          // 時間差を求める
          FloatT tk0(_ephemeris.period_from_time_of_ephemeris(t));
          FloatT tk(tk0);

          // 伝播時間の除去
          tk -= pseudo_range / self_t::light_speed;
          //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 - WGS84::Omega_Earth_IAU) * tk0
              - WGS84::Omega_Earth_IAU * _ephemeris.t_oe);
#else
          FloatT Omegak(_ephemeris.Omega0
              + _ephemeris.dot_Omega0 * tk                          // corrected with the time when the wave is transmitted
              - WGS84::Omega_Earth_IAU * (_ephemeris.t_oe + tk0));  // corrected with the time when the wave is received
#endif
          //cout << "Omega0 => " << _ephemeris.Omega0 << endl;
          //cout << WGS84::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;
          
          res.pos.x() = xk * Omegak_cos - yk * Omegak_sin * ik_cos;
          res.pos.y() = xk * Omegak_sin + yk * Omegak_cos * ik_cos;
          res.pos.z() = yk * ik_sin; 
          
          // 速度関係の計算 => GPS solution vol.8 (3) http://www.ngs.noaa.gov/gps-toolbox/bc_velo.htm
          if(with_velocity){
            FloatT n((sqrt(WGS84::mu_Earth) / pow3(_ephemeris.sqrt_A)) + _ephemeris.delta_n);
            FloatT Ek_dot(n / (1.0 - _ephemeris.e * cos(Ek)));
            FloatT vk_dot(sin(Ek) * Ek_dot * (1.0 + _ephemeris.e * cos(vk)) 
                / (sin(vk) * (1.0 - _ephemeris.e * cos(Ek))));
            
            FloatT pk2_sin(sin(pk * 2)), pk2_cos(cos(pk * 2));
            FloatT pk_dot(((_ephemeris.c_us * pk2_cos - _ephemeris.c_uc * pk2_sin) * 2 + 1.0) * vk_dot);
            FloatT rk_dot(pow2(_ephemeris.sqrt_A) * _ephemeris.e * sin(Ek) * n / (1.0 - _ephemeris.e * cos(Ek)) 
                + (_ephemeris.c_rs * pk2_cos - _ephemeris.c_rc * pk2_sin) * 2 * vk_dot);
            FloatT ik_dot(_ephemeris.dot_i0 + (_ephemeris.c_is * pk2_cos - _ephemeris.c_ic * pk2_sin) * 2 * vk_dot);
            
            // 軌道面内での速度(xk_dot, yk_dot)
            FloatT xk_dot(rk_dot * cos(pk) - yk * pk_dot),
                yk_dot(rk_dot * sin(pk) + xk * pk_dot);
            
            FloatT Omegak_dot(_ephemeris.dot_Omega0 - WGS84::Omega_Earth_IAU);
            
            res.vel.x() = (xk_dot - yk * ik_cos * Omegak_dot) * Omegak_cos
                - (xk * Omegak_dot + yk_dot * ik_cos - yk * ik_sin * ik_dot) * Omegak_sin;
            res.vel.y() = (xk_dot - yk * ik_cos * Omegak_dot) * Omegak_sin
                + (xk * Omegak_dot + yk_dot * ik_cos - yk * ik_sin * ik_dot) * Omegak_cos;
            res.vel.z() = yk_dot * ik_sin + yk * ik_cos * ik_dot;
          }
          
          // 結果の出力
          return res;
        }
        
        xyz_t whereis(const gps_time_t &t, const FloatT &pseudo_range = 0) const {
          return constellation(t, pseudo_range, false).pos;
        }
        
        xyz_t velocity(const gps_time_t &t, const FloatT &pseudo_range = 0) const {
          return constellation(t, pseudo_range, true).vel;
        }
    };
  public:
    typedef std::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();
    }

    /**
     * 電離層遅延補正量(単位は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;
      }
      if(amp < 0){amp = 0;}
      if(per < 72000){per = 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());
    }
};

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

template <class FloatT>
const FloatT GPS_SpaceNode<FloatT>::L1_Frequency = 1575.42E6;

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

#ifdef POW2_ALREADY_DEFINED
#undef POW2_ALREADY_DEFINED
#else
#undef pow2
#endif
#ifdef POW3_ALREADY_DEFINED
#undef POW3_ALREADY_DEFINED
#else
#undef pow3
#endif

#endif /* __GPS_H__ */