/**
 * G_Pageの単独測位計算プログラム
 * 
 */
 
#include <iostream>
#include <sstream>
#include <fstream>

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

#include <algorithm>
#include <utility>
#include <map>
#include <queue>
#include <vector>

#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <ctime>
#include <cmath>

#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "SylphideProcessor.h"

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

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

#include "gps_common.h"

using namespace std;

typedef SylphideProcessor<> Processor_t;
typedef Processor_t::A_Observer_t A_Observer_t;
typedef Processor_t::G_Observer_t G_Observer_t;

#define BUFFER_SIZE (PAGE_SIZE * 1) // 32
#define OBSERVER_SIZE (PAGE_SIZE * 32) // 1024

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

typedef double precision_t;

class G_Page_Solver_SP {

  public:
    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 vector<pair<int, precision_t> > sat_range_t;
    typedef map<int, vector<space_node_t::Satellite::Ephemeris> > nav_data_t;
    typedef pair<precision_t, gps_time_t> range_time_t; 
    typedef map<int, range_time_t> sat_range_time_t;
    
    struct solve_1step_return_t {
      matrix_t G; // デザイン行列 衛星はsat_range.iterator()の順に対応
      matrix_t W; // 重み行列 衛星はsat_range.iterator()の順に対応
      xyz_t delta_user_position;
      precision_t delta_receiver_error; 
    };
    
  protected:
    space_node_t _space_node;
    nav_data_t nav_data;
    bool has_true;
    xyz_t _loc_true;
    bool _always_reset;
    bool _ephemeris_lock;
    bool _valid_iono_param;
    
    xyz_t _user_position;
    precision_t _receiver_error;
    unsigned _solve_invoked;
    
    sat_range_time_t latest_sat_range_time;
  
  public:
    G_Page_Solver_SP() 
        : _space_node(), nav_data(),
          has_true(false), _loc_true(), 
          _always_reset(false), _ephemeris_lock(false), _valid_iono_param(false),
          _user_position(), _receiver_error(0), _solve_invoked(0),
          latest_sat_range_time() {}
    ~G_Page_Solver_SP(){}
    
    space_node_t &space_node(){return _space_node;}
  
    xyz_t &loc_true(bool set_true = true){
      has_true = set_true;
      return _loc_true;
    }
    
    bool set_reset_mode(bool always_reset){
      return _always_reset = always_reset;
    }
    
    bool lock_ephemeris(bool flag){
      return _ephemeris_lock = flag;
    }
    
    bool valid_iono_param(bool flag){
      return _valid_iono_param = flag;
    }
  
  protected:  
    /**
     * Iterationの1step
     * 
     * @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 sat_range_t &sat_range,
        const gps_time_t &target_time,
        xyz_t &user_position,
        precision_t &receiver_error,
        bool is_coarse_mode = true) throw (exception){
      
      //cerr << "usr => " << user_position << endl;
      
      gps_time_t target_time_est(target_time);
      target_time_est -= (receiver_error / space_node_t::light_speed); 
      
      matrix_t G(sat_range.size(), 4);
      matrix_t W(matrix_t::getI(sat_range.size()));;      
      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;
          
          // 重みの設定
          if(delta_r(i, 0) > 30.0){
            // 残差が大きい衛星は重みを小さくして計算に使用されないようにする
            W(i, i) = 1E-8;
            cerr << "PRN(" << it->first << ") is neglected, delta_r = " << delta_r(i, 0) << endl;
          }else{
            // GPS実用プログラミングより
            W(i, i) *= pow2(sin(relative_pos.elevation())/0.8);
            W(i, i) = std::max(W(i, i), 1E-3); 
          } 
        }
      }
      //cerr << "delta_r:" << delta_r << endl;
      //cerr << "G:" << G << endl;
      
      try{
        // 最小二乗法の適用
        matrix_t delta_x(
            (G.transpose() * W * G).inverse() * G.transpose() * W * delta_r);
            
        solve_1step_return_t res;
        {
          res.G = G;
          res.W = W;
          res.delta_user_position = xyz_t(delta_x.partial(3, 1, 0, 0));
          res.delta_receiver_error = delta_x(3, 0);
        }
        
        user_position += res.delta_user_position;
        receiver_error += res.delta_receiver_error;
        
        return res;
      }catch(exception e){throw e;}
    }
      
  public:
    struct solve_user_velocity_return_t {
      xyz_t user_velocity;
      precision_t dot_receiver_error; 
    };
  
    solve_user_velocity_return_t solve_user_velocity(
        const sat_range_t &sat_range,
        const gps_time_t &target_time,
        const precision_t &receiver_error,
        const matrix_t &design_matrix_G,
        const matrix_t &weight_matrix_W) throw (exception) {
      
      matrix_t delta_range(sat_range.size(), 1);
      matrix_t G(design_matrix_G.rows(), design_matrix_G.columns());
      matrix_t W(weight_matrix_W.rows(), weight_matrix_W.columns());
      
      gps_time_t target_time_est(target_time - receiver_error / space_node_t::light_speed);
      
      // 擬似距離の変化率を求める
      int i1(0), i2(0);
      for(sat_range_t::const_iterator it(sat_range.begin());
          it != sat_range.end();
          ++it, ++i2){
        
        precision_t geo_range_est(it->second - receiver_error);
      
        // 前回の擬似距離情報があるか調べる
        sat_range_time_t::iterator it2(latest_sat_range_time.find(it->first));
        if(it2 != latest_sat_range_time.end()){
          // 擬似距離差 / 時間差 で正規化
          delta_range(i1, 0)
              = (geo_range_est - it2->second.first) 
                  / it2->second.second.interval(target_time_est);
          //cerr << "delta_range(" << it->first << "): "
          //     << delta_range(i1, 0) << endl;
          
          //cerr << "delta_t(" << it->first << "): "
          //     << it2->second.second.interval(target_time_est) << endl;
          
          for(int j(0); j < design_matrix_G.columns(); j++){
            G(i1, j) = const_cast<matrix_t &>(design_matrix_G)(i2, j);
          }
          W(i1, i1) = const_cast<matrix_t &>(weight_matrix_W)(i2, i2);
          
          // さらに衛星の速度ベクトルを算出(xyz_t)
          space_node_t::Satellite &sat(
              const_cast<space_node_t *>(&_space_node)->satellite(it->first));
          
          xyz_t sat_vel(sat.velocity(target_time_est, it->second));
          //cerr << "sat_vel(" << it->first << "): "
          //     << sat_vel << endl;
          
          // デザイン行列にある視線ベクトルを利用して、
          // 擬似距離変化率から衛星の速度ベクトルに由来する部分を除去
          delta_range(i1, 0) += (G(i1, 0) * sat_vel.x()
              + G(i1, 1) * sat_vel.y()
              + G(i1, 2) * sat_vel.z());
          
          //cerr << "delta_range(" << it->first << "): "
          //     << delta_range(i1, 0) << endl;
          
          i1++;
        }
      
        // 最新の擬似距離情報を記録
        latest_sat_range_time[it->first] = range_time_t(geo_range_est, target_time_est);
      }
      G = G.partial(i1, design_matrix_G.columns(), 0, 0);
      W = W.partial(i1, i1, 0, 0);
      delta_range = delta_range.partial(i1, delta_range.columns(), 0, 0);
      
      // デザイン行列から速度を求める
      try{
        solve_user_velocity_return_t res;
        
        matrix_t sol((G.transpose() * W * G).inverse() * G.transpose() * W * delta_range);
        res.user_velocity.x() = sol(0, 0);
        res.user_velocity.y() = sol(1, 0);
        res.user_velocity.z() = sol(2, 0);
        res.dot_receiver_error = sol(3, 0);
        
        return res;
      }catch(exception e){throw e;}
    }
    
    void solve_user_position(
        const sat_range_t &sat_range,
        const gps_time_t &target_time) {
      
      // 電離層パラメータが有効かチェック
      if(!_valid_iono_param){
        cerr << "Valid iono paramter required!!" << endl;
        return;
      }
      
      // 初期解の設定
      if(_always_reset){
        _user_position.x() = _user_position.y() = _user_position.z() = 0;
      }
      
      sat_range_t available_sat_range;
      
      for(sat_range_t::const_iterator it(sat_range.begin());
          it != sat_range.end();
          ++it){
        
        int prn(it->first);
        precision_t pseudo_range(it->second);
        
        cerr << "PRN: " << prn << " => " << pseudo_range;
        
        bool available(false);
        bool search_ephemeris(true);
        if(_space_node.has_satellite(it->first)){
          available = true;
          if(!_space_node.satellite(prn).ephemeris().maybe_newer_one_avilable(target_time)){
            search_ephemeris = false;
          }
        }
        
        // エフェメリスの設定
        
        // 前にエフェメリスがない場合、
        // あるいは新しいものが利用可能である場合は設定する
        if(search_ephemeris){
          
          // エフェメリスがあるか
          if(nav_data.find(prn) != nav_data.end()){
          
            // エフェメリスを検索
            struct comparator {
              const gps_time_t &_t;
              comparator(const gps_time_t &t) : _t(t) {} 
              bool operator()(
                  const space_node_t::Satellite::Ephemeris &eph1,
                  const space_node_t::Satellite::Ephemeris &eph2){
                return _abs(eph1.period_from_time_of_clock(_t)) 
                    < _abs(eph2.period_from_time_of_clock(_t));
              }
            };
            _space_node.satellite(prn).ephemeris()
                = *min_element(nav_data[prn].begin(), nav_data[prn].end(), 
                    comparator(target_time));
            available = true;
          }
        }
        
        // エフェメリスがないものについては無効とする
        if(!available){
          cerr << endl;
          continue;
        }
        
        cerr << " @ Ephemeris: t_oc => "
             << _space_node.satellite(prn).ephemeris().WN << " week " 
             << _space_node.satellite(prn).ephemeris().t_oc
             << endl;
        
        /*
        cerr << _space_node.satellite(prn).ephemeris().a_f0 << ", "
             << _space_node.satellite(prn).ephemeris().a_f1 << ", "
             << _space_node.satellite(prn).ephemeris().a_f2 << ", "
             << _space_node.satellite(prn).ephemeris().iode << ", "
             << _space_node.satellite(prn).ephemeris().c_rs << ", "
             << _space_node.satellite(prn).ephemeris().delta_n << ", "
             << _space_node.satellite(prn).ephemeris().m0 << ", "
             << _space_node.satellite(prn).ephemeris().c_uc << ", "
             << _space_node.satellite(prn).ephemeris().e << ", "
             << _space_node.satellite(prn).ephemeris().c_us << ", "
             << _space_node.satellite(prn).ephemeris().sqrt_A << ", "
             << _space_node.satellite(prn).ephemeris().t_oe << ", "
             << _space_node.satellite(prn).ephemeris().c_ic << ", "
             << _space_node.satellite(prn).ephemeris().Omega0 << ", "
             << _space_node.satellite(prn).ephemeris().c_is << ", "
             << _space_node.satellite(prn).ephemeris().i0 << ", "
             << _space_node.satellite(prn).ephemeris().c_rc << ", "
             << _space_node.satellite(prn).ephemeris().omega << ", "
             << _space_node.satellite(prn).ephemeris().dot_Omega0 << ", "
             << _space_node.satellite(prn).ephemeris().dot_i0 << ", "
             << _space_node.satellite(prn).ephemeris().t_GD << endl;
        */
             
        
        // 擬似距離の再設定
        available_sat_range.push_back(sat_range_t::value_type(prn, pseudo_range));
      }
      
      if(available_sat_range.size() < 4){
        cerr << "Minimum satellite not available!! : sat => " << available_sat_range.size() << endl;
        return;
      }
      
      // 航法計算の実行
      try{
        matrix_t C;
        matrix_t G;
        matrix_t W;
        
        for(unsigned i(0); i < 10; i++){
          solve_1step_return_t res(
              solve_1step(
                available_sat_range,
                target_time,
                _user_position,
                _receiver_error,
                (((!_always_reset) && _solve_invoked == 0) ? (i < 3) : (i < 1)) // 初回だけは荒く計算する回数を増やす
              ));
          
          //cerr << "loop(" << i << ") => "
          //     << "user: " << _user_position
          //     << "; receiver_error: " << _receiver_error
          //     << endl;
          
          if(res.delta_user_position.dist() <= 1E-6){
            G = res.G;
            W = res.W;
            C = (G.transpose() * G).inverse();
            break;
          }
        }
        
        llh_t &user_position_llh(_user_position.llh());
        cerr << "user(llh):" << user_position_llh << endl;
        cerr << "clock_bias:" << _receiver_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)));
        
        // CSV形式で吐き出しておく
        
        /*
        struct tm t_epoc_tm(target_time.c_tm()); //leap_seconds));
        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 << ", ";
        */
        
        cout << target_time.week << ", "
             << target_time.seconds << ", "
             << (_receiver_error / space_node_t::light_speed) << ", ";
        cout << rad2deg(user_position_llh.latitude()) << ", "
             << rad2deg(user_position_llh.longitude()) << ", "
             << user_position_llh.height() << ", "
             << gdop << ", "
             << pdop << ", "
             << hdop << ", "
             << vdop << ", "
             << tdop << ", ";
        
        // 速度の計算
        enu_t vel_enu;
        try{
          solve_user_velocity_return_t vel(solve_user_velocity(
              available_sat_range, target_time, _receiver_error, G, W));
          vel_enu = enu_t::relative_rel(vel.user_velocity, user_position_llh);
        }catch(exception e){
          cerr << "No velocity solution!!" << endl;
        }
        
        cout << vel_enu.north() << ", "
             << vel_enu.east() << ", "
             << vel_enu.up() << endl;

        if(has_true){
          enu_t error(enu_t::relative(_user_position, _loc_true));
          cerr << "error e, n, u => " << error << endl;
        }
         
        _solve_invoked++;
      }catch(exception e){
        cerr << "No solution!!" << endl;
        //cerr << e.what() << endl;
      }
    }
    
    void regist_epehemris(
        const space_node_t::Satellite::Ephemeris &eph){
              
      // 衛星順、時刻順に並べる
      if(nav_data[eph.svid].empty()){
        nav_data[eph.svid].push_back(eph);
      }else{
        for(nav_data_t::mapped_type::reverse_iterator it(nav_data[eph.svid].rbegin()); 
            it != nav_data[eph.svid].rend(); 
            ++it){
          if(gps_time_t(eph.WN, eph.t_oc) >= gps_time_t(it->WN, it->t_oc)){
            if(!_ephemeris_lock){
              if(eph.t_oc != it->t_oc){
                nav_data[eph.svid].insert(it.base(), eph);
              }else{
                (*it) = eph;
              }
            }
            break;
          }
        }
      }
      
      cerr << "NAV data : " << nav_data.size()
           << " ; " << eph.svid << " => " 
           << nav_data[eph.svid].size() << ", " << eph.WN << ", " << eph.t_oc
           << endl;
    }
    
    void regist_epehemris(
        const G_Observer_t::ephemeris_t &ephemeris,
        const int cycle_NO = 0){
      
      regist_epehemris(
          (space_node_t::Satellite::Ephemeris)EphemerisConverter<precision_t>(
              ephemeris, cycle_NO));
    }
} solver;

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x02, 0x10}のとき生情報(Carrire, 擬似距離)
 * {class, id} = {0x02, 0x31}のときエフェメリス
 * {class, id} = {0x0b, 0x02}のときヘルス、UTC、電離層パラメータ
 * 
 * @param obsrever Gページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  if(!observer.validate()){return;}
  
  static int cycle_NO(0);
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  switch(packet_type.mclass){
    case 0x02: { 
      switch(packet_type.mid){
        case 0x10: {
          G_Page_Solver_SP::sat_range_t sat_range;
          G_Page_Solver_SP::gps_time_t t_epoc(observer.fetch_WN(), observer.fetch_ITOW());
          cycle_NO = t_epoc.week / 1024;
          unsigned int num_of_sv((unsigned char)observer[6 + 6]);
          cerr << "GPS_Time : " << t_epoc << endl;
          //cerr << "Number of satellites : " << num_of_sv << endl;
          for(int i = 0; i < num_of_sv; i++){
            G_Observer_t::raw_measurement_t raw_data(observer.fetch_raw(i));
            
            // 擬似距離の設定
            sat_range.push_back(G_Page_Solver_SP::sat_range_t::value_type(
                raw_data.sv_number, raw_data.pseudo_range));
          }
          
          solver.solve_user_position(sat_range, t_epoc);
          
          break;
        }
        case 0x31: {
          G_Observer_t::ephemeris_t ephemeris(observer.fetch_ephemeris());
          if(!ephemeris.valid){break;}
          
          solver.regist_epehemris(ephemeris, cycle_NO);
        }
      }
      break;
    }
    case 0x0b: {
      switch(packet_type.mid){
        case 0x02: {  // GPS Health, UTC and ionosphere parameters
          G_Observer_t::health_utc_iono_t
              hui(observer.fetch_health_utc_iono());
          if(hui.iono.valid){
            solver.space_node().iono_coef().alpha0 = hui.iono.klob_a0;
            solver.space_node().iono_coef().alpha1 = hui.iono.klob_a1;
            solver.space_node().iono_coef().alpha2 = hui.iono.klob_a2;
            solver.space_node().iono_coef().alpha3 = hui.iono.klob_a3;
            solver.space_node().iono_coef().beta0 = hui.iono.klob_b0;
            solver.space_node().iono_coef().beta1 = hui.iono.klob_b1;
            solver.space_node().iono_coef().beta2 = hui.iono.klob_b2;
            solver.space_node().iono_coef().beta3 = hui.iono.klob_b3;
            solver.valid_iono_param(true);
          }
          break;
        }
      }
      break;
    }
    default:
      break;
  }
}

/**
 * ファイル等のストリームからページ単位で切り出す関数
 * 
 * @param in ストリーム
 */
void stream_processor(istream &in){
  char buffer[PAGE_SIZE];
  
  Processor_t processor(OBSERVER_SIZE);       // ストリーム処理機を生成
  processor.set_g_handler(g_packet_handler);  // Gページの際の処理を登録
  
  int count(0);

  while(!in.eof()){
    count++;
    int read_count;
    in.read(buffer, PAGE_SIZE);
    read_count = in.gcount();

    if(read_count < PAGE_SIZE){
      continue;
    }

    processor.process(buffer, read_count);
  }
}

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

  cerr << "G_Page solver." << endl;
  if(argc < 2){
    cerr << "Usage: (exe) log.dat [options]" << endl;
    return -1;
  }
  
  // ログファイルの読み込み
  fstream fin_log(argv[1], ios::in | ios::binary);    
  if(fin_log.fail()){
    cout << "Not found!! : " << argv[1] << endl;
    return -1;
  }
  
  // option check...
  cerr << "Option checking..." << endl;
  for(int arg_index(3); arg_index < argc; arg_index++){
    
    // 真値の設定 --true=lat[deg],long[deg],height[m]
    if(strstr(argv[arg_index], "--true=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--true="));
      cerr << "true: " << spec << endl;
      precision_t lat_deg(0), long_deg(0), height_m(0);
      if(*spec != '\0'){lat_deg = strtod(spec, &spec);}
      if(*spec != '\0'){long_deg = strtod(++spec, &spec);}
      if(*spec != '\0'){height_m = strtod(++spec, &spec);}
      solver.loc_true() = G_Page_Solver_SP::llh_t(
          deg2rad(lat_deg), deg2rad(long_deg), height_m).xyz();
      cerr << "set true: " << solver.loc_true() << endl;
      continue;
    }
    // 観測ごとにユーザ位置をゼロリセットするかどうか
    if(strstr(argv[arg_index], "--reset=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--reset="));
      bool reset_mode((*spec == 't') || (*spec == 'T'));
      cerr << "reset_mode: " << solver.set_reset_mode(reset_mode) << endl;
      continue;
    }
    // RINEX NAV 情報を使用するかどうか
    if(strstr(argv[arg_index], "--use_rinex=") == argv[arg_index]){
      
      char *spec(argv[arg_index] + strlen("--use_rinex="));
      
      // RINEX航法ファイルの読み込み
      fstream fin_n(spec, ios::in | ios::binary);
      if(fin_n.fail()){
        cerr << "Not found!! : " << spec << endl;
        return -1;
      }
      
      // 航法ファイルの処理
      typedef RINEX_Nav_Reader<> reader_n_t;
      reader_n_t reader_n(fin_n);
      
      // 電離層遅延係数の処理
      if(!reader_n.extract_iono_coef(solver.space_node())){
        cerr << "Iono coef not found!!" << endl;
        return -1;
      }else{
        solver.valid_iono_param(true);
      }
      
      // すべての航法情報の取得を行い、登録
      while(reader_n.has_next()){
        reader_n_t::SatelliteInfo info(reader_n.next());
        if(info.ephemeris.t_oc == 374400){continue;}
        solver.regist_epehemris(info.ephemeris);
      }
      solver.lock_ephemeris(true);
      cerr << "use RINEX Nav Ephemeris" << endl;
      continue;
    }
  }
  
  stream_processor(fin_log);
  
  return 0;
}