#include <iostream>
#include <sstream>
#include <cstring>

#include <map>
#include <set>

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

#include "SylphideProcessor.h"

typedef double float_sylph_t;

#include "analyze_common.h"
#include "gps_common.h"

struct Options : public GlobalOptions<float_sylph_t> {
  typedef GlobalOptions<float_sylph_t> super_t;
  
  const char *base_fname;
  std::ostream *_out_nav, *_out_obs;
  bool in_is_sylphide_log;
  Options()
      : super_t(), 
      base_fname(NULL),
      _out_nav(NULL), _out_obs(NULL), 
      in_is_sylphide_log(false) {}
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(const char *spec){
    using std::cerr;
    using std::endl;
    
    const char *value;
    if(value = get_value(spec, "base_fname", false)){
      cerr << "base_fname : ";
      base_fname = value;
      return true;
    }
    if(value = get_value(spec, "out_nav", false)){
      cerr << "out.nav : ";
      _out_nav = &(spec2ostream(value));
      return true;
    }
    if(value = get_value(spec, "out_obs", false)){
      cerr << "out.obs : ";
      _out_obs = &(spec2ostream(value));
      return true;
    }
    
    return false;
  }
  
  std::ostream &out_nav() const {return *_out_nav;}
  std::ostream &out_obs() const {return *_out_obs;}
} options;

using namespace std;

class StreamProcessor 
    : public AbstractSylphideProcessor<float_sylph_t> {
  public:
    static const unsigned int buffer_size;
  protected:
    typedef AbstractSylphideProcessor<float_sylph_t> super_t;
    typedef G_Packet_Observer<float_sylph_t> G_Observer_t;
    
    /**
     * Gページ(u-bloxのGPS)の処理用関数
     * 
     * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * {class, id} = {0x02, 0x10}のとき生情報(Carrire, 擬似距離)
     * {class, id} = {0x02, 0x31}のときエフェメリス
     * {class, id} = {0x0b, 0x02}のときヘルス、UTC、電離層パラメータ
     * 
     * @param obsrever Gページのオブザーバー
     */
    struct GHandler : public G_Observer_t  {
      bool previous_seek_next;
      
      RINEX_Nav_Writer<float_sylph_t> writer_nav;
      RINEX_Observe_Writer<float_sylph_t> writer_obs;
      int cycle_NO;
      GPS_Time<float_sylph_t> first_obs, last_obs;
      int num_of_obs;
      
      typedef map<int, set<int> > ephemeris_stock_t;
      ephemeris_stock_t ephemeris_stock; // エフェメリスの重複チェッカ
      
      GHandler(ostream &out_nav, ostream &out_obs) 
          : G_Observer_t(buffer_size),
          writer_nav(out_nav), writer_obs(out_obs),
          cycle_NO(0),
          ephemeris_stock(),
          first_obs(), last_obs(), num_of_obs(0) {
        previous_seek_next = G_Observer_t::ready();
        
        writer_obs.pgm_runby_date("ubx2rinex", " ", GPS_Time<float_sylph_t>::now().c_tm());
        writer_obs.maker_name("YOU System");
        writer_obs.observer_agency("fenrir", "YOU System");
        writer_obs.receiver_spec("0", "LEA-4T Logger", "Rev.A");
        writer_obs.antenna_spec("0", "Patch Antenna");
        writer_obs.approx_position(0, 0, 0);
        static const char *obs_types[] = {
            "L1", "C1", "D1"};
        writer_obs.types_of_obs(
            obs_types, sizeof(obs_types) / sizeof(obs_types[0]));
      }
      ~GHandler(){}
      void operator()(const G_Observer_t &observer){
        if(!observer.validate()){return;}
        
        G_Observer_t::packet_type_t
            packet_type(observer.packet_type());
        /*cerr << hex
            << (int)packet_type.mclass << ", " 
            << (int)packet_type.mid << endl;*/
        switch(packet_type.mclass){
          case 0x02: {
            switch(packet_type.mid){
              case 0x10: {
                // 観測データの抽出
                typedef RINEX_Observe<float_sylph_t>::ObservedItem item_t;
                item_t item;
                
                item.t_epoc = GPS_Time<float_sylph_t>(observer.fetch_WN(), observer.fetch_ITOW());
                item.event_flag = 0;
                item.receiver_clock_error = 0;
                cycle_NO = item.t_epoc.week / 1024;
                
                unsigned int num_of_sv((unsigned char)observer[6 + 6]);
                
                for(int i(0); i < num_of_sv; i++){
                  G_Observer_t::raw_measurement_t raw_data(observer.fetch_raw(i));
                  
                  item_t::sat_data_t::mapped_type &v(item.sat_data[raw_data.sv_number]);
                  item_t::data_t data[] = {
                      {raw_data.carrier_phase, raw_data.lock_indicator, raw_data.signal_strength / 5},
                      {raw_data.pseudo_range, 0, 0},
                      {raw_data.doppler, 0, 0}};
                  for(int i(0); i < sizeof(data) / sizeof(data[0]); i++){
                    v.push_back(data[i]);
                  }
                }
                
                writer_obs << item;
                
                if(num_of_obs++){
                  last_obs = item.t_epoc;
                }else{
                  first_obs = last_obs = item.t_epoc;
                }
                
                if(!(num_of_obs % 100)){
                  cerr << "GPS_Time : " << item.t_epoc
                      << " @ " << num_of_sv << " sats" << endl;
                }
                
                break;
              }
              case 0x31: {
                G_Observer_t::ephemeris_t ephemeris(observer.fetch_ephemeris());
                if(!ephemeris.valid){break;}
                
                // 重複チェックをする
                ephemeris_stock_t::iterator it(
                    ephemeris_stock.find(ephemeris.sv_number));
                if(it != ephemeris_stock.end()){
                  if(it->second.find(ephemeris.iodc) == it->second.end()){
                    it->second.insert(ephemeris.iodc);
                  }else{
                    break;
                  }
                }else{
                  ephemeris_stock[ephemeris.sv_number].insert(ephemeris.iodc);
                }
                
                RINEX_Nav<float_sylph_t>::SatelliteInfo info = {
                    ephemeris.sv_number,  // 衛星番号
                    ((ephemeris.how >> (24 - 17)) & 0x01FFFF) * 6,  // 送信時刻[s] (17bits * 6 in HOW, IS-GPS-200D pp.82, Fig.20-2)
                    EphemerisConverter<float_sylph_t>(ephemeris, cycle_NO)}; // エフェメリス
                
                /*cerr << hex << setfill('0') 
                    << "0x" << setw(8) << ephemeris.how
                    << ", " << dec << info.t_ot << endl;*/
                
                writer_nav << info;
                break;
              }
            }
            break;
          }
          case 0x0b: {
            switch(packet_type.mid){
              case 0x02: {
                G_Observer_t::health_utc_iono_t
                    hui(observer.fetch_health_utc_iono());
                if(!hui.iono.valid){break;}
                writer_nav.ion_alpha(
                    hui.iono.klob_a0, hui.iono.klob_a1,
                    hui.iono.klob_a2, hui.iono.klob_a3);
                writer_nav.ion_beta(
                    hui.iono.klob_b0, hui.iono.klob_b1,
                    hui.iono.klob_b2, hui.iono.klob_b3);
                break;
              }
            }
            break;
          }
        }
      }
    };
    
  public:
    StreamProcessor() : super_t() {
      
    }
    virtual ~StreamProcessor(){}
    
    /**
     * ファイル等のストリームから1ページ単位で処理を行う関数
     * 
     * @param in ストリーム
     */
    void process(istream &in){
      char buffer[PAGE_SIZE];
      int invoked(0);
      int read_count;
      stringstream str_nav, str_obs;
      GHandler g_handler(str_nav, str_obs);
      
      int buffer_offset(options.in_is_sylphide_log ? 1 : 0);
      
      while(true){
        in.read(buffer, sizeof(buffer));
        read_count = in.gcount();
        if(in.fail() || (read_count == 0)){break;}
        invoked++;
      
#if DEBUG
        cerr << "--read-- : " << invoked << " page" << endl;
        cerr << hex;
        for(int i = 0; i < read_count; i++){
          cerr 
            << setfill('0') 
            << setw(2)
            << (unsigned int)((unsigned char)buffer[i]) << ' ';
        }
        cerr << dec;
        cerr << endl;
#endif
      
        if(read_count < PAGE_SIZE){
#if DEBUG
          cerr << "--skipped-- : " << invoked << " page ; count = " << read_count << endl;
#endif
        }
        
        // Sylphideのログを直接いれた場合の使う部分の判定
        if(buffer_offset && (buffer[0] != 'G')){
          continue;
        }
        
        // 直接突っ込む
        g_handler.write(buffer + buffer_offset, read_count - buffer_offset);
        if(!g_handler.previous_seek_next){
          if(g_handler.ready()){g_handler(g_handler);}
          g_handler.previous_seek_next = g_handler.seek_next();
        }
        while(g_handler.previous_seek_next 
            && g_handler.ready()){
          g_handler(g_handler);
          g_handler.previous_seek_next = g_handler.seek_next();
        }
      }
      
      if(g_handler.num_of_obs){
        g_handler.writer_obs.first_obs(g_handler.first_obs);
        g_handler.writer_obs.last_obs(g_handler.last_obs);
        if(g_handler.num_of_obs > 1){
          g_handler.writer_obs.interval(
              (g_handler.last_obs - g_handler.first_obs) 
                  / (g_handler.num_of_obs - 1));
        }
      }
      
      options.out_nav() << g_handler.writer_nav.header() << str_nav.str();
      options.out_obs() << g_handler.writer_obs.header() << str_obs.str();
    }
};
    
const unsigned int StreamProcessor::buffer_size = 1024;

int main(int argc, char *argv[]){
  
  StreamProcessor processor;
  
  cerr << "covert from ubx format to RINEX format." << endl;
  if(argc < 2){
    cerr << "Usage: (exe) log.ubx [options]" << endl;
    return -1;
  }
  
  cerr << "in : ";
  istream &in(options.spec2istream(argv[1]));
  
  // options
  for(int i(2); i < argc; i++){
    if(options.check_spec(argv[i])){continue;}
    
    cerr << "Unknown option!! : " << argv[i] << endl;
    return -1;
  }
  
  // デフォルトの出力先の指定
  {
    string base_fname(options.base_fname ? options.base_fname : argv[1]);
    string::size_type index = base_fname.find_last_of('.');
    if(index != string::npos){
      base_fname.erase(index);
    }
    struct {
      ostream **out;
      const char *suffix;
    } list[] = {
        {&(options._out_nav), ".nav"},
        {&(options._out_obs), ".obs"}};
    for(int i(0); i < sizeof(list) / sizeof(list[0]); i++){
      if(!(*(list[i].out))){
        string fname(base_fname);
        fname.append(list[i].suffix);
        cerr << "out" << list[i].suffix << " : ";
        *(list[i].out) = &(options.spec2ostream(fname.c_str(), true));
      }
    }
  }
  
  processor.process(in);
  
  return 0;
}