#include <iostream>
#include <fstream>
#include <iomanip>
#include <string>
#include <exception>
#include <string>

//#define DEBUG 1

#define IS_LITTLE_ENDIAN 1
#include "util/comstream.h"
#include "SylphideProcessor.h"
#include "SylphideStream.h"
#include "util/endian.h"
#include "util/crc.h"

using namespace std;

typedef G_Packet_Observer<> G_Observer_t;
typedef A_Packet_Observer<> A_Observer_t;
typedef F_Packet_Observer<> F_Observer_t;
typedef M_Packet_Observer<> M_Observer_t;
typedef P_Packet_Observer<> P_Observer_t;
typedef N_Packet_Observer<> N_Observer_t;

ostream *out_gps(NULL);
/**
 * Gページ(u-bloxのGPS)の処理用関数
 * 
 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Gページのオブザーバー
 */
void g_packet_handler(const G_Observer_t &observer){
  cerr << "G";
  
  G_Observer_t::packet_type_t
      packet_type(observer.packet_type());
  packet_type.mclass;
  packet_type.mid;
  
  unsigned int packet_size(observer.current_packet_size());
  if(observer.validate()){
    // 有効なデータを処理する
    if(out_gps){
      char *buf(new char[packet_size]);
      observer.inspect(buf, packet_size);
      out_gps->write(buf, packet_size);
      out_gps->flush();
      delete [] buf;
    }
  }else{
    // エラー!!
    //cerr << "!" << (int)(packet_type.mclass) 
    //     << "+" << (int)(packet_type.mid) << " ";
    cerr << hex;
    for(int i = 0; i < packet_size; i++){
      cerr 
        << setfill('0') 
        << setw(2)
        << (unsigned int)((unsigned char)observer[i]) << ' ';
    }
    cerr << dec;
    cerr << endl;
    return;
  }
  cerr << " ";
}

/**
 * Aページ(AD変換結果)の処理用関数
 * 
 * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Aページのオブザーバー
 */
void a_packet_handler(const A_Observer_t &observer){
  cerr << "A";
  if(!observer.validate()){
    // エラー!!
    //cerr << "! ";
    return;
  }
  cerr << " ";
}

/**
 * Fページの処理用関数
 * 
 * Fページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Fページのオブザーバー
 */
void f_packet_handler(const F_Observer_t &observer){
  cerr << "F";
  if(!observer.validate()){
    // エラー!!
    //cerr << "! ";
    return;
  }
  cerr << " ";
}

/**
 * Mページの処理用関数
 * 
 * Mページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Mページのオブザーバー
 */
void m_packet_handler(const M_Observer_t &observer){
  cerr << "M";
  if(!observer.validate()){
    // エラー!!
    //cerr << "! ";
    return;
  }
  cerr << " ";
}

/**
 * Pページの処理用関数
 * 
 * Pページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Pページのオブザーバー
 */
void p_packet_handler(const P_Observer_t &observer){
  cerr << "P";
  if(!observer.validate()){
    // エラー!!
    //cerr << "! ";
    return;
  }
  cerr << " ";
}

/**
 * Nページの処理用関数
 * 
 * Nページの内容が正しいかvalidateで確認した後、実処理を行うこと。
 * 
 * @param obsrever Nページのオブザーバー
 */
void n_packet_handler(const N_Observer_t &observer){
  cerr << "N";
  if(!observer.validate()){
    // エラー!!
    //cerr << "! ";
    return;
  }
  cerr << " ";
}

class NAV2Bridge {
  protected:
    ComportStream com_in;
    ComportStream com_gps_out;
    
    static void set_dcb_default(DCB &dcb){
      dcb.BaudRate = CBR_9600;
      dcb.ByteSize = 8;
      dcb.Parity = NOPARITY;
      dcb.fParity = FALSE;
      dcb.StopBits = ONESTOPBIT;
      dcb.fBinary = TRUE; // バイナリモード
      dcb.fNull = FALSE;  // NULLバイトは破棄しない
      dcb.fOutX = FALSE;  // XONなし
      dcb.fInX = FALSE;   // XOFFなし
      dcb.fOutxCtsFlow = FALSE;   // CTSフロー制御なし
      dcb.fOutxDsrFlow = FALSE;   // DSRフロー制御なし
      dcb.fDtrControl = DTR_CONTROL_DISABLE;         // DSR制御なし
      dcb.fDsrSensitivity = FALSE;
      dcb.fRtsControl = RTS_CONTROL_DISABLE;
      dcb.fAbortOnError = FALSE;
    }
  public:
    NAV2Bridge(const char *port_in, const char *port_gps_out) 
        : com_in(port_in),
          com_gps_out(port_gps_out) {
    
      cerr << "In: " << port_in << endl;
      com_in.buffer().config_dcb(set_dcb_default);
      
      cerr << "GPS Out: " << port_gps_out << endl;
      com_gps_out.buffer().config_dcb(set_dcb_default);
      out_gps = &com_gps_out;
    }
    ~NAV2Bridge() {}
    
    void run() {
      SylphideProcessor<double> processor(1024);
      processor.set_g_handler(g_packet_handler);
      processor.set_a_handler(a_packet_handler);
      processor.set_f_handler(f_packet_handler);
      processor.set_m_handler(m_packet_handler);
      processor.set_p_handler(p_packet_handler);
      processor.set_n_handler(n_packet_handler);
      SylphideIStream sylph_in(com_in, PAGE_SIZE);
      char buf[PAGE_SIZE];
      int previous_sequence_num(0);
      while(sylph_in.good() && com_gps_out.good()){
        sylph_in.read(buf, sizeof(buf));

#if 0
        //cerr << "--read-- : " << received << " bytes" << endl;
        cerr << hex;
        for(int i = 0; i < received; i++){
          cerr 
            << setfill('0') 
            << setw(2)
            << (unsigned int)((unsigned char)buf[i]) << ' ';
        }
        cerr << dec;
        cerr << endl;
#endif
        
        unsigned int sequence_num(sylph_in.current_sequence());
        if(((previous_sequence_num + 1) & 0xFFFF) != sequence_num){
          cerr << "! ";
        }
        previous_sequence_num = sequence_num; 
          
        //cerr << sequence_num << " ";
        processor.process(buf, sizeof(buf));
      }
    }
};

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

  cerr << "starting NAV2 bridge ..." << endl;
  if(argc < 3){
    cerr << "Usage: (exe) in_port gps_out_port" << endl;
    return -1;
  }
  
  try{
    NAV2Bridge bridge(argv[1], argv[2]);
    bridge.run();
  }catch(ios_base::failure &e){
    cerr << e.what() << endl;
  }
  
  return 0;
}