/**
 * 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 0

#define IS_LITTLE_ENDIAN 1
#include "../070423/SylphideStream.h"
#include "../070423/SylphideProcessor.h"

typedef double float_sylph_t;

#include "../070423/analyze_common.h"

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

#include "minimal_rtklib.h"

#define rcsid rcsid_rinex
#define obscodes obscodes_rinex
#include "rtklib_2.4.2/src/rinex.c"
#undef rcsid
#undef obscodes

using namespace std;

#ifdef WITHOUT_FILE
void rtkprintstat(int level, const char *str, ...){}

int trace_vprintf(const char *format, va_list ap){
  int res;
  res = vfprintf(stderr, format, ap);
  fflush(stderr);
  return res;
}

int uncompress(const char *file, char *uncfile){return 0;}
#endif

struct Options : public GlobalOptions {
  typedef GlobalOptions super_t;
  Options() : super_t() {}
  ~Options(){}
  /**
   * コマンドに与えられた設定を読み解く
   *
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(char *spec){
    static const char *available_options[] = {
        "--start-gpst=",
        "--end-gpst=",
        "--out=",
        "--in_sylphide="};
    for(int i(0);
        i < sizeof(available_options) / sizeof(available_options[0]);
        i++){
      if(std::strstr(spec, available_options[i]) == spec){
        return super_t::check_spec(spec);
      }
    }
    return false;
  }
} options;

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_RTKLIB {

  public:
    typedef System_XYZ<> xyz_t;
    typedef System_LLH<> llh_t;
    typedef System_ENU<> enu_t;
    
  protected:
    bool has_true;
    xyz_t _loc_true;
    bool _always_reset;
    bool _ephemeris_lock;
    bool _valid_iono_param;
    
    unsigned _solve_invoked;
  
    // RTKLIB instances
    nav_t nav;
    rtk_t rtk;
    prcopt_t opt;

    eph_t _eph[MAXSAT * 2];
    seph_t _seph[NSATSBS * 2];
    sbssat_t _sbssat;
    sbsion_t _sbsion[MAXBAND + 1];

  public:
    G_Page_Solver_RTKLIB()
        : has_true(false), _loc_true(),
          _always_reset(false), _ephemeris_lock(false), _valid_iono_param(false),
          _solve_invoked(0),
          nav(), rtk(), opt(prcopt_default) {

      ///< @see init_raw() of rcvraw.c
      memset(&nav, 0, sizeof(nav_t));

      nav.eph = _eph;
      nav.nmax = MAXSAT * 2;
      nav.seph = _seph;
      nav.ns = nav.nsmax = NSATSBS * 2;
      nav.sbssat = &_sbssat;
      nav.sbsion = _sbsion;

      static const double lam_glo[] = {
          CLIGHT/FREQ1_GLO, CLIGHT/FREQ2_GLO};
      for (int i(0); i < sizeof(nav.lam) / sizeof(nav.lam[0]); ++i){
        switch(satsys(i + 1, NULL)){
          case SYS_NONE: continue;
          case SYS_GLO: memcpy(nav.lam[i], lam_glo, sizeof(nav.lam[i])); break;
          default: memcpy(nav.lam[i], lam_carr, sizeof(nav.lam[i])); break;
        }
      }

      opt.mode = PMODE_PPP_KINEMA; // PMODE_SINGLE;
      opt.dynamics = 1;

      rtkinit(&rtk, &opt);
    }
    ~G_Page_Solver_RTKLIB(){
      rtkfree(&rtk);
    }

    nav_t &space_node(){
      return nav;
    }
  
    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 regist_iono_param(const precision_t (&param)[8]){
      for(int i(0); i < sizeof(param) / sizeof(param[0]); ++i){
        nav.ion_gps[i] = param[i];
      }
      return _valid_iono_param = true;
    }

  public:
    void solve_user_position(const obs_t &obs) {
      // 電離層パラメータが有効かチェック
      if(!_valid_iono_param){
        trace(4, "rtkpos warning: using default iono paramter.\n");
      }

      if(rtkpos(&rtk, obs.data, obs.n, &nav) == 0){
        return;
      }

      static const char *stat_str[] = {"NONE", "FIX", "FLOAT", "SBAS", "DGPS", "SINGLE", "PPP", "DR"};
      trace(3, "rtkpos solved (stat=%s)\n", stat_str[rtk.sol.stat]);

      double *xyz(
          (rtk.opt.mode >= PMODE_DGPS)
            ? ((rtk.na >= 6) ? rtk.xa : rtk.x) // fixed or float
            : rtk.sol.rr);
      xyz_t user_position_xyz(xyz[0], xyz[1], xyz[2]);
      llh_t user_position_llh(user_position_xyz.llh());
      enu_t vel_enu(enu_t::relative_rel(
          xyz_t(rtk.sol.rr[3], rtk.sol.rr[4], rtk.sol.rr[5]),
          user_position_llh));

      {
        int wn;
        double sec(time2gpst(rtk.sol.time, &wn));
        options.out() << wn << ", "
             << sec << ", "
             << (unsigned)(rtk.sol.stat) << ", ";
      }
      options.out() << rad2deg(user_position_llh.latitude()) << ", "
           << rad2deg(user_position_llh.longitude()) << ", "
           << user_position_llh.height() << ", ";

      { ///< @see outnmea_gsa() of solution.c, compute DOP
        double azels[MAXOBS * 2];
        union{
          struct {
            double gdop, pdop, hdop, vdop;
          } s;
          double v[4];
        } dop;
        int ns(0);
        for(int i(0), j(0); i < sizeof(rtk.ssat) / sizeof(rtk.ssat[0]); ++i){
          if ((!rtk.ssat[i].vs)
              || (rtk.ssat[i].azel[1] <= 0.0)
              || (rtk.ssat[i].sys == SYS_GLO)){continue;}
          azels[j++] = rtk.ssat[i].azel[0];
          azels[j++] = rtk.ssat[i].azel[1];
          ns++;
        }
        dops(ns, azels, opt.elmin, dop.v);
        double tdop(sqrt((dop.s.gdop * dop.s.gdop) - (dop.s.pdop * dop.s.pdop)));
        options.out() << dop.s.gdop << ", " // GDOP
             << dop.s.pdop << ", " // PDOP
             << dop.s.hdop << ", " // HDOP
             << dop.s.vdop << ", " // VDOP
             << tdop << ", ";
      }

      options.out() << vel_enu.north() << ", "
           << vel_enu.east() << ", "
           << vel_enu.up() << endl;

#if 0
      // 初期解の設定
      if(_always_reset){
        _user_position.x() = _user_position.y() = _user_position.z() = 0;
      }
#endif
    }

    void regist_epehemris(const eph_t &eph){
      ///< @see uniqnav() of rktcmn.c, this is another keeping buffer version.
      ///< @see add_eph of rinex.c

      // ttrが若い順、toeが若い順、satが若い順に並んでいる
      int insert_index(nav.n - 1);
      for(; insert_index >= 0; insert_index--){
        if(nav.eph[insert_index].ttr.time < eph.ttr.time){
          break;
        }else if(nav.eph[insert_index].ttr.time > eph.ttr.time){
          continue;
        }

        if(nav.eph[insert_index].toe.time < eph.toe.time){
          break;
        }else if(nav.eph[insert_index].toe.time > eph.toe.time){
          continue;
        }

        if(nav.eph[insert_index].sat < eph.sat){
          break;
        }else if(nav.eph[insert_index].sat > eph.sat){
          continue;
        }
        return; // 重複チェック
      }
      insert_index++;

      if(nav.nmax <= nav.n){
        // rotate
        int i(0), j(1);
        for(; j < nav.nmax; i++, j++){
          if(nav.eph[i].ttr.time < nav.eph[j].ttr.time){
            break;
          }
        }
        if(insert_index <= i){return;} // 古すぎるデータ
        nav.n = nav.nmax - j;
        insert_index -= j;
        memmove(&nav.eph[0], &nav.eph[j], sizeof(eph_t) * nav.n);
      }

      if(insert_index < nav.n){
        memmove(&nav.eph[insert_index + 1], &nav.eph[insert_index], sizeof(eph_t) * (nav.n - insert_index));
      }
      nav.eph[insert_index] = eph;
      nav.n++;

      tracenav(4, &nav);
    }
} solver;

/**
 * Gページ(u-bloxのGPS)の処理用関数
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * {class, id} = {0x02, 0x10}のとき生情報(Carrier, 擬似距離)
 * {class, id} = {0x02, 0x11}のとき生情報(サブフレーム、SBASのみ利用)
 * {class, id} = {0x02/0x0b, 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);
  static gtime_t t_last_observation;
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  switch(packet_type.mclass){
    case 0x02: { 
      switch(packet_type.mid){
        case 0x10:
          ///< @see decode_rxmraw() of ublox.c
          cycle_NO = observer.fetch_WN() >> 10;
          gtime_t t_epoc(gpst2time(observer.fetch_WN(), observer.fetch_ITOW()));
          t_last_observation = t_epoc;

          // check minimum 4 ephemris available
          if(solver.space_node().n < 4){return;}

          obs_t obs;
          obs.n = (unsigned char)observer[6 + 6];
          obs.data = new obsd_t[obs.n];
          obs.nmax = obs.n;

          for(int i(0); i < obs.n; ++i){
            G_Observer_t::raw_measurement_t raw_data(observer.fetch_raw(i));

            obs.data[i].time = t_epoc;
            obs.data[i].rcv = 1;
            obs.data[i].L[0] = raw_data.carrier_phase;
            obs.data[i].P[0] = raw_data.pseudo_range;
            obs.data[i].D[0] = raw_data.doppler;
            obs.data[i].SNR[0] = (4.0 * raw_data.signal_strength) + 0.5;
            obs.data[i].LLI[0] = raw_data.lock_indicator;
            obs.data[i].code[0] = CODE_L1C;
            obs.data[i].sat = satno(
                (MINPRNSBS <= raw_data.sv_number ? SYS_SBS : SYS_GPS), raw_data.sv_number);

            for (int j(1); j < NFREQ; ++j){
              obs.data[i].L[j] = obs.data[i].P[j] = 0;
              obs.data[i].D[j] = 0.0;
              obs.data[i].SNR[j] = obs.data[i].LLI[j] = 0;
              obs.data[i].code[j] = CODE_NONE;
            }
          }

          sortobs(&obs);
          solver.solve_user_position(obs);

          delete [] obs.data;
          return;
        case 0x11: { // Get SBAS info from sub frame buffer. @see decode_rxmsfrb() of ublox.c
          unsigned char id;
          observer.inspect((char *)&id, sizeof(id), 6 + 1);
          if(satsys(id, NULL) != SYS_SBS){break;}

          // @see sbsdecodemsg() of sbas.c
          sbsmsg_t msg;
          msg.tow = (int)(time2gpst(t_last_observation, &msg.week) + DTTOL);
          msg.prn = id;
          unsigned char buf[40];
          observer.inspect((char *)buf, sizeof(buf), 6 + 2);
          for(int i(0), j(4), dword_index(0); dword_index < 7; j += 8, dword_index++){
            for(int k(0); k < 4; k++){
              msg.msg[i++] = buf[--j]; // endian swap.
            }
          }
          msg.msg[28] = buf[31] << 6; // data bit position => dword_LE[7][25..24] = buf_LE[(7*4)+3][1..0]

          // parity check
          {
            unsigned char buf2[29];
            for(int i(sizeof(buf2) - 1), j(i - 1); i > 0; i--, j--){
              buf2[i] = ((msg.msg[i] >> 6) & 0x03) | ((msg.msg[j] << 2) & 0xFC);
            }
            buf2[0] = ((msg.msg[0] >> 6) & 0x03);
            unsigned int gen_parity(crc24q(buf2, sizeof(buf2))), orig_parity(0);

            // parity bit position => dword_LE[7][23..0] = buf_LE[28] | (buf_LE[29] << 8) | (buf_LE[30] << 16)
            orig_parity |= buf[30]; orig_parity <<= 8;
            orig_parity |= buf[29]; orig_parity <<= 8;
            orig_parity |= buf[28];

            if(gen_parity != orig_parity){
              trace(3, "sbas(%d) subframe parity error: 0x%08x vs 0x%08x\n", msg.prn, gen_parity, orig_parity);
              break;
            }
            trace(5, "sbas(%d) subframe parity checked: 0x%08x\n", msg.prn, orig_parity);
          }

          switch(sbsupdatecorr(&msg, &solver.space_node())){
            case 9: tracehnav(4, &solver.space_node()); break;
          }
          return;
        }
      }
      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){
            precision_t param[] = {
                hui.iono.klob_a0, hui.iono.klob_a1, hui.iono.klob_a2, hui.iono.klob_a3,
                hui.iono.klob_b0, hui.iono.klob_b1, hui.iono.klob_b2, hui.iono.klob_b3};
            solver.regist_iono_param(param);
          }
          return;
        }
      }
      break;
    }
  }

  if((packet_type.mid == 0x31) // RXM-EPH or AID-EPH
      && ((packet_type.mclass == 0x02) || (packet_type.mclass == 0x0B))){
    G_Observer_t::ephemeris_t ephemeris(observer.fetch_ephemeris());
    if(!ephemeris.valid){return;}

#define get_subframe_head_index(subframe) (6 + 8 + ((subframe - 1) * 32))
#define get_byte(subframe, index) \
(observer[get_subframe_head_index(subframe) \
+ (((index / 30) - 2) * 4) + (2 - ((index % 30) / 8))]) << ((index % 30) % 8)

    /* Index check
     * [ 0.. 3] =>  60.. 67,  68.. 75,  76.. 83
     * [ 4.. 7] =>  90.. 97,  98..105, 106..113
     * [ 8..11] => 120..127, 128..135, 136..143
     * [12..15] => 150..157, 158..165, 166..173
     * [16..19] => 180..187, 188..195, 196..203
     * [20..23] => 210..217, 218..225, 226..233
     * [24..27] => 240..247, 248..255, 256..263
     * [28..31] => 270..277, 278..285, 286..293
     */
    eph_t eph = {0};
    if(ephemeris.sv_number > MAXPRNGPS){return;}
    eph.sat = satno(SYS_GPS, ephemeris.sv_number); // satellite number

    ///< @see decode_subfrm1() of rcvraw.c
    eph.week = ephemeris.wn + (1024 * cycle_NO); // GPS/QZS: gps week (GAL: galileo week)
    eph.code = get_byte(1, 70) >> (8 - 2); // 2bits, GPS/QZS: code on L2 (GAL/CMP: data sources)
    eph.sva = ephemeris.ura; // SV accuracy (URA index)
    eph.svh = ephemeris.sv_health; // SV health (0:ok)
    eph.iodc = ephemeris.iodc;
    eph.flag = get_byte(1, 90) >> (8 - 1); // 1bit, GPS/QZS: L2 P data flag (CMP: nav type)
    eph.tgd[0] = ephemeris.t_gd; // GPS/QZS:tgd[0] = TGD
    eph.toc = gpst2time(eph.week, ephemeris.t_oc);
    eph.f2 = ephemeris.a_f2; // SV clock parameters
    eph.f1 = ephemeris.a_f1;
    eph.f0 = ephemeris.a_f0;

    {
      unsigned long tow_count(ephemeris.how >> (24 - 17)); // Hand Over Word[b0..b16] => TOW-Count @see IS-GPS-200D
      eph.ttr = gpst2time(eph.week, tow_count * 6);
    }

    ///< @see decode_subfrm2() of rcvraw.c
    eph.iode = ephemeris.iode;
    eph.crs = ephemeris.c_rs;
    eph.deln = ephemeris.delta_n;
    eph.M0 = ephemeris.m_0;
    eph.cuc = ephemeris.c_uc;
    eph.e = ephemeris.e;
    eph.cus = ephemeris.c_us;
    eph.A = ephemeris.root_a * ephemeris.root_a;
    eph.toes = ephemeris.t_oe; // Toe (s) in week
    eph.toe = gpst2time(eph.week, ephemeris.t_oe);
    eph.fit = ephemeris.fit; // fit interval (h)

    ///< @see decode_subfrm3() of rcvraw.c
    eph.cic = ephemeris.c_ic;
    eph.OMG0 = ephemeris.omega_0;
    eph.cis = ephemeris.c_is;
    eph.i0 = ephemeris.i_0;
    eph.crc = ephemeris.c_rc;
    eph.omg = ephemeris.omega;
    eph.OMGd = ephemeris.omega_0_dot;
    eph.idot = ephemeris.i_0_dot;

    solver.regist_epehemris(eph);
#undef get_byte
#undef get_subframe_head_index
  }
}

/**
 * ファイル等のストリームからページ単位で切り出す関数
 * 
 * @param in ストリーム
 */
void stream_processor(istream &in){
  char buffer[PAGE_SIZE];
  
  cerr.precision(10);
  options.out().precision(12);

  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);

    cerr.flush();
    options.out().flush();
  }
}

int main(int argc, char *argv[]){

#ifndef WITHOUT_FILE
  traceopen("");
#endif

  cerr << "G_Page solver." << endl;
  if(argc < 2){
    cerr << "Usage: (exe) log.dat [options]" << endl;
    return -1;
  }
  
  // ログファイルの指定
  istream &in(options.spec2istream(argv[1]));
  
  // option check...
  cerr << "Option checking..." << endl;
  for(int arg_index(2); arg_index < argc; arg_index++){
    
    if(options.check_spec(argv[arg_index])){continue;}

    // 真値の設定 --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_RTKLIB::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航法ファイルの読み込み
      if(readrnx(spec, 1, "", NULL, &solver.space_node(), NULL) == 0){
        return -1;
      }
      uniqnav(&solver.space_node());
      
      solver.lock_ephemeris(true);
      cerr << "use RINEX Nav Ephemeris" << endl;
      continue;
    }
    // デバックレベル
    if(strstr(argv[arg_index], "--debug=") == argv[arg_index]){
      char *spec(argv[arg_index] + strlen("--debug="));
      int level(atoi(spec));
      tracelevel(level);
      cerr << "debug_level: " << level << endl;
      continue;
    }
    cerr << "Unknown command: " << argv[arg_index] << endl;
    exit(-1);
  }

  // SylphideProtocolへの対応
  if(options.in_sylphide){
    SylphideIStream sylphide_in(in, PAGE_SIZE);
    stream_processor(sylphide_in);
  }else{
    stream_processor(in);
  }
  
  return 0;
}