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

#include <stdio.h>
#include <io.h>
#include <fcntl.h>

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

typedef double float_sylph_t;
#include "analyze_common.h"
#include "mavlink/common/mavlink.h"

using namespace std;

struct Options : public GlobalOptions<float_sylph_t> {

  typedef GlobalOptions<float_sylph_t> super_t;
  int debug_level;
  mavlink_system_t mavlink_system;
  
  Options() 
      : super_t(),
      debug_level(0) {
    mavlink_system.sysid = 0;
    mavlink_system.compid = MAV_COMP_ID_UART_BRIDGE;
    mavlink_system.type = MAV_TYPE_GENERIC;
  }
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(const char *spec){
    const char *value;
    // 互換性のため、in_sylphideと等価のdirect_sylphideを残しておく
    if(value = get_value(spec, "direct_sylphide")){
      in_sylphide = is_true(value);
      cerr << "direct_sylphide: " << (in_sylphide ? "on" : "off") << endl;
      return true;
    }
    if(value = get_value(spec, "debug", false)){
      debug_level = atoi(value);
      cerr << "debug_level: " << debug_level << endl;
      return true;
    }
    return super_t::check_spec(spec);
  }
} options;

class StreamProcessor : public SylphideProcessor<float_sylph_t> {
  protected:
    int invoked;
    typedef SylphideProcessor<float_sylph_t> super_t;
  
  public:
    /**
     * Aページ(AD変換結果)の処理用関数
     * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Aページのオブザーバー
     */
    struct HandlerA {
      HandlerA() {}
      void operator()(const super_t::A_Observer_t &observer){
        if(!observer.validate()){return;}
      }
    } handler_A;
    
    /**
     * Gページ(u-bloxのGPS)の処理用関数
     * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * {class, id} = {0x01, 0x02}のとき位置情報が得られる
     * {class, id} = {0x01, 0x12}のとき速度情報が得られる
     * 
     * @param obsrever Gページのオブザーバー
     */
    struct HandlerG {
      unsigned int itow_ms_0x0102, itow_ms_0x0112;
      super_t::G_Observer_t::position_t position;
      super_t::G_Observer_t::position_acc_t position_acc;
      super_t::G_Observer_t::velocity_t velocity;
      super_t::G_Observer_t::velocity_acc_t velocity_acc;
      HandlerG() 
          : itow_ms_0x0102(0), itow_ms_0x0112(0),
          position(0, 0, 0), position_acc(0, 0),
          velocity(0, 0, 0), velocity_acc(0) {
        
      }
      ~HandlerG(){}
      
      void operator()(const super_t::G_Observer_t &observer){
        if(!observer.validate()){return;}
        
        bool change_itow(false);
        super_t::G_Observer_t::packet_type_t
            packet_type(observer.packet_type());
        switch(packet_type.mclass){
          case 0x01: {
            switch(packet_type.mid){
              case 0x02: {
                position = observer.fetch_position();
                position_acc = observer.fetch_position_acc();
                
                itow_ms_0x0102 = observer.fetch_ITOW_ms();
                change_itow = true;
                
                break;
              }
              case 0x12: {
                velocity = observer.fetch_velocity();
                velocity_acc = observer.fetch_velocity_acc();
                
                itow_ms_0x0112 = observer.fetch_ITOW_ms();
                change_itow = true;
                
                break;
              }
            }
            break;
          }
          default:
            break;
        }
        
        if(change_itow
            && (itow_ms_0x0102 == itow_ms_0x0112)){
          
          float_sylph_t current(1E-3 * itow_ms_0x0102);
          if(!options.is_time_in_range(current)){return;}
          
          position.latitude;
          position.longitude;
          position.altitude;
          position_acc.horizontal;
          position_acc.vertical;
          velocity.north;
          velocity.east;
          velocity.down;
          velocity_acc.acc;
        }
      }
    } handler_G;
    
    /**
     * Fページ(FPGAサブシステムからの情報)の処理用関数
     * Fページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Fページのオブザーバー
     */
    struct HandlerF {
      int count;
      HandlerF() : count(0) {}
      void operator()(const F_Observer_t &observer){
        if(!observer.validate()){return;}
        
        float_sylph_t current(observer.fetch_ITOW());
        if(!options.is_time_in_range(current)){return;}
        
        F_Observer_t::values_t values(observer.fetch_values());
        for(int i = 0; i < 8; i++){
          values.servo_in[i];
          values.servo_out[i];
        }
      }
    } handler_F;
    
    struct HandlerP {
      float_sylph_t previous_itow;
      HandlerP() : previous_itow(0) {}
      
      /**
       * Pページ(エアデータセンサからの情報)の処理用関数
       * Pページの内容が正しいかvalidateで確認した後、実処理を行うこと。
       * 
       * @param obsrever Fページのオブザーバー
       */
      void operator()(const P_Observer_t &observer){
        if(!observer.validate()){return;}
        
        float_sylph_t current(observer.fetch_ITOW());
        if(!options.is_time_in_range(current)){return;}
        
        P_Observer_t::values_t values(observer.fetch_values());
        for(int i(0), j(-3); i < 4; i++, j++){
          values.air_speed[i];
          values.air_alpha[i];
          values.air_beta[i];
        }
      }
    } handler_P;
    
    /**
     * Mページ(磁気センサからの情報)の処理用関数
     * Mページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Mページのオブザーバー
     */
    struct HandlerM {
      HandlerM() {}
      void operator()(const M_Observer_t &observer){
        if(!observer.validate()){return;}
        
        float_sylph_t current(observer.fetch_ITOW());
        if(!options.is_time_in_range(current)){return;}
        
        M_Observer_t::values_t values(observer.fetch_values());
        for(int i(0), j(-3); i < 4; i++, j++){
          values.x[i];
          values.y[i];
          values.z[i];
        }
      }
    } handler_M;
    
    /**
     * Nページ(航法情報)の処理用関数
     * Nページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Nページのオブザーバー
     */
    struct HandlerN {
      void operator()(const N_Observer_t &observer){
        if(!observer.validate()){return;}
        
        float_sylph_t current(observer.fetch_ITOW());
        if(!options.is_time_in_range(current)){return;}

        N_Observer_t::navdata_t values(observer.fetch_navdata());

        mavlink_message_t msg;
        uint8_t buf[MAVLINK_MAX_PACKET_LEN];

        { // heartbeat
          mavlink_msg_heartbeat_pack(options.mavlink_system.sysid, options.mavlink_system.compid, &msg,
              MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_SAFETY_ARMED, 0, MAV_STATE_STANDBY);
          uint16_t len(mavlink_msg_to_send_buffer(buf, &msg));
          options.out().write((char *)buf, len);
        }

        { // attitude
          mavlink_msg_attitude_pack(options.mavlink_system.sysid, options.mavlink_system.compid, &msg,
              observer.fetch_ITOW_ms(), deg2rad(values.roll), deg2rad(values.pitch), deg2rad(values.heading), 0, 0, 0);
          uint16_t len(mavlink_msg_to_send_buffer(buf, &msg));
          options.out().write((char *)buf, len);
        }

        { // gps_raw_int
          float_sylph_t v_horizontal(pow(pow(values.v_north, 2) + pow(values.v_east, 2), 0.5));
          mavlink_msg_gps_raw_int_pack(options.mavlink_system.sysid, options.mavlink_system.compid, &msg,
              observer.fetch_ITOW_ms() * 1000, 3,
              values.latitude * 1E7, values.longitude * 1E7, values.altitude * 1E3,
              0xFFFF, 0xFFFF,
              v_horizontal * 1E2,
              v_horizontal > 5 ? (uint16_t)(atan2(values.v_east, values.v_north) * 1E2) : 0xFFFF,
              0xFF);
          uint16_t len(mavlink_msg_to_send_buffer(buf, &msg));
          options.out().write((char *)buf, len);
        }
      }
    } handler_N;
    
#if 0
    /**
     * SylphideProtocol形式で入ってくるCページ用の処理器
     * 
     */
    class SylphideStreamHandler 
        : public Sylphide_Packet_Observer<float_sylph_t> {
      protected:
        typedef Sylphide_Packet_Observer<float_sylph_t> super_t;
        typedef SylphideStreamHandler self_t;
      public:
        bool previous_seek;
        unsigned int invoked;
        SylphideStreamHandler() 
            : super_t(PAGE_SIZE),
            previous_seek(false), invoked(0) {}
        ~SylphideStreamHandler() {}
        
        /**
         * パケットが見つかった際に呼び出される関数
         * 
         */
        void operator()(const self_t &observer){
          if(!observer.validate()){return;}
          
        }
    } handler_C;
#endif
    
  public:
    StreamProcessor()
        : super_t(), invoked(0) {
      
    }
    ~StreamProcessor(){}
    
    /**
     * ファイル等のストリームから1ページ単位で処理を行う関数
     * 
     * @param in ストリーム
     * @return (bool) 処理が成功したかどうか
     */
    void process(istream &in){
      char buffer[PAGE_SIZE];
      
      while(true){
        int read_count;
        in.read(buffer, PAGE_SIZE);
        read_count = in.gcount();
        if(in.fail() || (read_count == 0)){return;}
        invoked++;
      
        if(options.debug_level){
          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;
          
          if(read_count < PAGE_SIZE){
            cerr << "--skipped-- : " << invoked << " page ; count = " << read_count << endl;
          }
        }
      
        switch(buffer[0]){
#define assign_case(type, mark) \
case mark: { \
  super_t::process_packet( \
      buffer, read_count, \
      observer_ ## type , previous_seek_next_ ## type, handler_ ## type); \
} \
break;
          assign_case(A, 'A');
          assign_case(G, 'G');
          assign_case(F, 'F');
          assign_case(P, 'P');
          assign_case(M, 'M');
          assign_case(N, 'N');
#undef assign_case
#if 0
          case 'C': {
            super_t::process_packet(
                buffer, read_count,
                handler_C, handler_C.previous_seek, handler_C);
          }
          break;
#endif
          default: {

          }
        }
      }      
    }
};

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

  if(argc < 2){
    cerr << "Usage: " << argv[0] << " log_in [options]" << endl;
    return -1;
  }
  
  StreamProcessor processor;
  
  int arg_index(2);
  
  // オプションによる出力の設定
  for(; arg_index < argc; arg_index++){
    if(options.check_spec(argv[arg_index])){continue;}
    
    cerr << "Unknown option!! : " << argv[arg_index] << endl;
    return -1;
  }
  
  if(options.in_sylphide){
    SylphideIStream sylph_in(options.spec2istream(argv[1]), PAGE_SIZE);
    processor.process(sylph_in);
  }else{
    processor.process(options.spec2istream(argv[1]));
  }
  
  return 0;
}