#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 "CatCAMProcessor.h"

typedef double float_t;

#include <map>

#include <cstdio>
#include <cstring>
#include <cstdlib>
#include <cfloat>
#include <io.h>
#include <fcntl.h>

#include "util/util.h"
#include "util/comstream.h"
#include "util/endian.h"

// OpenCV
#include "opencv\cv.h"
#include "opencv\highgui.h"

#ifdef _DEBUG
    //Debugモードの場合
    #pragma comment(lib,"cv210d.lib")
    #pragma comment(lib,"cxcore210d.lib")
    #pragma comment(lib,"cvaux210d.lib")
    #pragma comment(lib,"highgui210d.lib")
#else
    //Releaseモードの場合
    #pragma comment(lib,"cv210.lib")
    #pragma comment(lib,"cxcore210.lib")
    #pragma comment(lib,"cvaux210.lib")
    #pragma comment(lib,"highgui210.lib")
#endif

struct GlobalOptions {
  
  std::ostream *_out; //< 出力先、out()で参照をとるべき
  bool in_sylphide;   //< trueのとき、入力がSylphideプロトコル
  bool out_sylphide;  //< trueのとき、出力がSylphideプロトコル
  typedef std::map<const char *, std::iostream *> iostream_pool_t;
  iostream_pool_t iostream_pool;
  
  GlobalOptions()
      : _out(&(std::cout)),
      in_sylphide(false), out_sylphide(false),
      iostream_pool() {};
  virtual ~GlobalOptions(){
    for(iostream_pool_t::iterator it(iostream_pool.begin());
        it != iostream_pool.end();
        ++it){
      it->second->flush();
      delete it->second;
    }
  }
  
#ifdef _WIN32
#define COMPORT_PREFIX "COM"
#else
#define COMPORT_PREFIX "/dev/tty"
#endif
  
  std::istream &spec2istream(
      const char *spec, 
      const bool force_fstream = false){
    if(!force_fstream){
      if(strcmp(spec, "-") == 0){
        // 標準入力は'-'で指定
        std::cerr << "[std::cin]" << std::endl;
#if defined(_MSC_VER)
      setmode(fileno(stdin), O_BINARY);
#endif
        return std::cin;
      }else if(strstr(spec, COMPORT_PREFIX) == spec){
        // COMポート
        // 先に登録されていないか確認する
        if(iostream_pool.find(spec) == iostream_pool.end()){
          ComportStream *com_in = new ComportStream(spec);
          iostream_pool[spec] = com_in;
          return *com_in;
        }else{
          return *(iostream_pool[spec]);
        }
      }
    }
    
    std::cerr << spec;
    std::fstream *fin(new std::fstream(spec, std::ios::in | std::ios::binary));    
    if(fin->fail()){
      std::cerr << " => File not found!!" << std::endl;
      exit(-1);
    }
    std::cerr << std::endl;
    iostream_pool[spec] = fin;
    return *fin;
  }
  
  std::ostream &spec2ostream(
      const char *spec,
      const bool force_fstream = false){
    if(!force_fstream){
      if(strcmp(spec, "-") == 0){
        // 標準出力は'-'で指定
        std::cerr << "[std::cout]" << std::endl;
#if defined(_MSC_VER)
      setmode(fileno(stdout), O_BINARY);
#endif
        return std::cout;
      }else if(strstr(spec, COMPORT_PREFIX) == spec){
        // COMポート
        // 先に登録されていないか確認する
        if(iostream_pool.find(spec) == iostream_pool.end()){
          ComportStream *com_out = new ComportStream(spec);
          iostream_pool[spec] = com_out;
          return *com_out;
        }else{
          return *(iostream_pool[spec]);
        }
      }
    }
    
    std::cerr << spec;
    std::fstream *fout(new std::fstream(spec, std::ios::out | std::ios::binary));
    std::cerr << std::endl;
    iostream_pool[spec] = fout;
    return *fout;
  }
  
  std::ostream &out() const {return *_out;}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  virtual bool check_spec(char *spec){
    using std::cerr;
    using std::endl;
    
    if(strstr(spec, "--out=") == spec){
      char *value(spec + strlen("--out="));
      cerr << "out: ";
      _out = &(spec2ostream(value));
      return true;
    }
    if(strstr(spec, "--in_sylphide=") == spec){
      char *value(spec + strlen("--in_sylphide="));
      in_sylphide = (strcmp(value, "on") == 0);
      cerr << "in_sylphide: " << (in_sylphide ? "on" : "off") << endl;
      return true;
    }
    if(strstr(spec, "--out_sylphide=") == spec){
      char *value(spec + strlen("--out_sylphide="));
      out_sylphide = (strcmp(value, "on") == 0);
      cerr << "out_sylphide: " << (out_sylphide ? "on" : "off") << endl;
      return true;
    }
    return false;
  }
};

using namespace std;

struct Options : public GlobalOptions {
  bool out_A;
  bool out_G;
  bool out_I;
  bool save_image;
  istream *cam_config;
  ostream *raw_out;
  const char *image_type;
  const char *classifier;
  int debug_level;
  
  Options() 
      : GlobalOptions(),
      out_A(false), out_G(false), out_I(false),
      save_image(false),
      cam_config(NULL), raw_out(NULL),
      image_type("jpg"),
      classifier(NULL), 
      debug_level(0) {}
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(char *spec){
    char c;
    if((strstr(spec, "--out=") == spec) 
        && (c = *(spec + strlen("--out=")))){
      switch(c){
        case 'A': out_A = true; break;
        case 'G': out_G = true; break;
        case 'I': out_I = true; break;
        default: return false;
      }
      return true;
    }
    // 互換性のため、in_sylphideと等価のdirect_sylphideを残しておく
    if(strstr(spec, "--direct=") == spec){
      char *value(spec + strlen("--direct="));
      in_sylphide = (strcmp(value, "on") == 0);
      cerr << "direct: " << (in_sylphide ? "on" : "off") << endl;
      return true;
    }
    if(strstr(spec, "--save_image=") == spec){
      char *value(spec + strlen("--save_image="));
      save_image = (strcmp(value, "on") == 0);
      cerr << "save_image: " << (save_image ? "on" : "off") << endl;
      return true;
    }
    if(strstr(spec, "--debug=") == spec){
      char *value(spec + strlen("--debug="));
      debug_level = atoi(value);
      cerr << "debug_level: " << debug_level << endl;
      return true;
    }
    if(strstr(spec, "--cam_config=") == spec){
      char *value(spec + strlen("--cam_config="));
      cam_config = &spec2istream(value);
      cerr << "cam_config: " << value << endl;
      return true;
    }
    if(strstr(spec, "--raw_out=") == spec){
      char *value(spec + strlen("--raw_out="));
      raw_out = &spec2ostream(value);
      cerr << "raw_out: " << value << endl;
      return true;
    }
    if(strstr(spec, "--image_type=") == spec){
      char *value(spec + strlen("--image_type="));
      image_type = value;
      cerr << "image_type: " << value << endl;
      return true;
    }
    if(strstr(spec, "--classifier=") == spec){
      char *value(spec + strlen("--classifier="));
      classifier = value;
      cerr << "classifier: " << classifier << endl;
      return true;
    }
    return GlobalOptions::check_spec(spec);
  }
} options;

class StreamProcessor : public CatCAMProcessor<float_t> {
  protected:
    int invoked;
    typedef CatCAMProcessor<float_t> super_t;
  
  public:
    /**
     * Aページ(AD変換結果)の処理用関数
     * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。
     * 
     * @param obsrever Aページのオブザーバー
     */
    struct HandlerA {
      int count;
      HandlerA() : count(0) {}
      void operator()(const super_t::A_Observer_t &observer){
        if(!observer.validate()){return;}
        
        options.out() << (count++) << ","
             << observer.fetch_ITOW() << ",";
        
        A_Observer_t::values_t values(observer.fetch_values());
        options.out() 
            << values.values[0].x << ","
            << values.values[0].y << ","
            << values.values[0].z << endl;
      }
    } 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)){
          
          options.out() << (1E-3 * itow_ms_0x0102) << ", "
              << position.latitude << ", "
              << position.longitude << ", "
              << position.altitude << ", "
              << position_acc.horizontal << ", "
              << position_acc.vertical << ", "
              << velocity.north << ", "
              << velocity.east << ", "
              << velocity.down << ", "
              << velocity_acc.acc << endl;
        }
      }
    } handler_G;
    
    /**
     * SylphideProtocol形式で入ってくるCページ用の処理器
     * 
     */
    class HandlerI 
        : public I_Packet_Observer<float_t> {
      protected:
        typedef I_Packet_Observer<float_t> super_t;
        typedef HandlerI self_t;
      public:
        bool previous_seek;
        unsigned int invoked;
        const char *window_id;
        cv::CascadeClassifier classifier;
        HandlerI(const char *id = "demo") 
            : super_t(1 << 20, 0),
            previous_seek(false), invoked(0),
            window_id(id), classifier() { 
          cv::namedWindow(window_id, CV_WINDOW_AUTOSIZE);
          if(options.classifier){
            classifier.load(options.classifier);
            if(!classifier.empty()){
              cerr << "Classifier(" << options.classifier << ") activated!" << endl;
            }
          }
        }
        ~HandlerI() {}
        
        /**
         * パケットが見つかった際に呼び出される関数
         * 
         */
        void operator()(const self_t &observer){
          if(!observer.validate()){return;}
          
          int length(observer.fetch_data_length());
          char *buf(new char [length]);
          
          if(length == observer.fetch_data(buf, length)){
            
            cerr 
                << observer.fetch_ITOW_ms() << ": "
                << length;
            
            char *image_ext("jpg");
            
            if(strstr(options.image_type, "rgb565") == options.image_type){
              char *buf_orig(buf);
              // 多分4:3か5:4か16:9か11:9画像
              const int aspects[][2] = {{4, 3}, {5, 4}, {16, 9}, {11, 9}};
              int pixels(length / 2), width, height, remain_pixels(pixels);
              for(int i(0); i < sizeof(aspects) / sizeof(aspects[0]); i++){
                int unit_length(sqrt((float_t)length / 2 / (aspects[i][0] * aspects[i][1])));
                int width_new(unit_length * aspects[i][0]), height_new(unit_length * aspects[i][1]),
                    remain_pixels_new(pixels - (width_new * height_new));
                if((remain_pixels_new >= 0) && (remain_pixels > remain_pixels_new)){
                  width = width_new;
                  height = height_new;
                  remain_pixels = remain_pixels_new;
                  if(remain_pixels == 0){break;}
                }
              }
              char header[128];
              sprintf(header, "P6\n%d %d\n255\n", width, height);
              int header_length = strlen(header);
              buf = new char[sizeof(header) + (length / 2 * 3)]; // 16bits(LE) => 24bits
              memcpy(buf, header, header_length);
              for(int offset_src(0), offset_dst(header_length); offset_src < length;){
                int pixel16(0);
                pixel16 |= ((int)(buf_orig[offset_src++]) << 8);
                pixel16 |= buf_orig[offset_src++];
                buf[offset_dst++] = (unsigned char)((pixel16 & 0xF800) >> 8); // r ?
                buf[offset_dst++] = (unsigned char)((pixel16 & 0x07E0) >> 3); // g
                buf[offset_dst++] = (unsigned char)((pixel16 & 0x001F) << 3); // b
              }
              delete [] buf_orig;
              image_ext = "ppm";
              length = header_length + (length / 2 * 3);
            }
            
            if(options.save_image){
              char fname[32];
              sprintf(fname, "img_%05d.%s", invoked, image_ext);
              cerr << " => " << fname;
              fstream img_out(fname, std::ios::out | std::ios::binary);
              if(!img_out.fail()){
                img_out.write(buf, length);
              }
              img_out.flush();
              img_out.close();
            }
            
            cv::Mat img_buf(
                cv::imdecode(cv::Mat(std::vector<char>(buf, buf + length)), 1));
            
            // 回転する
            {
              cv::Mat img_buf_rotated(cv::Size(img_buf.rows, img_buf.cols), CV_32FC4);
              const cv::Point2f in_points[] = {
                  cv::Point2f(0., 0.), 
                  cv::Point2f(0., img_buf.rows), 
                  cv::Point2f(img_buf.cols, 0.)};
              const cv::Point2f out_points[] = {
                  cv::Point2f(0, img_buf.cols), 
                  cv::Point2f(img_buf.rows, img_buf.cols),
                  cv::Point2f(0, 0)};
              cv::Mat rotator(
                  cv::getAffineTransform(
                      in_points, out_points));
              /*cv::Mat rotator(
                  cv::getRotationMatrix2D(
                      cv::Point(img_buf.cols / 2, img_buf.rows / 2), 90, 1.0));*/
              cv::warpAffine(img_buf, img_buf_rotated, rotator, 
                  img_buf_rotated.size());
              img_buf = img_buf_rotated;
            }
            
            if(img_buf.data){
              cerr << " " << img_buf.cols << " x " << img_buf.rows;
              
              // 画像認識をここでする
              if(!classifier.empty()){
                // グレースケール画像の生成
                cv::Mat grey_buf(img_buf.rows, img_buf.cols, CV_8U);
                cv::cvtColor(img_buf, grey_buf, CV_BGR2GRAY);
                vector<cv::Rect> res;
                classifier.detectMultiScale(grey_buf, res, 1.1, 3, 0, cv::Size());
                cerr << " detetct: " << res.size() <<  " objects";
                
                for(int i(0); i < res.size(); i++){
                  /*cv::rectangle(img_buf, 
                      cv::Point(res[i].x, res[i].y), 
                      cv::Point(res[i].x + res[i].width, res[i].y + res[i].height),
                      cv::Scalar(0, 0, 255), 3);*/ // 矩形
                  cv::ellipse(img_buf, 
                      cv::Point(res[i].x + res[i].width / 2, res[i].y + res[i].height / 2),
                      cv::Size(res[i].width, res[i].height),
                      0., 0., 360., 
                      cv::Scalar(0, 0, 255), 2, CV_AA); // 楕円
                }
              }
              
              cv::Mat disp_buf(cv::Size(480, 640), CV_32FC4);
              cv::resize(img_buf, disp_buf, cv::Size(disp_buf.cols, disp_buf.rows));
              cv::imshow(window_id, disp_buf);
              //cv::imshow(window_id, img_buf);
              cv::waitKey(1);
            }
            
            cerr << endl;
          }
          
          delete [] buf;
          invoked++;
        }
    } handler_I;
    
  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.raw_out){
          options.raw_out->write(buffer, read_count);
        }
      
        if(options.debug_level){
            //|| ((buffer[0] == 'I') && ((buffer[1] == 'H') || (buffer[1] == 'F')))){
          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: if(options.out_ ## type){ \
  super_t::process_packet( \
      buffer, read_count, \
      observer_ ## type , previous_seek_next_ ## type, handler_ ## type); \
} \
break;
          assign_case(A, 'A');
          assign_case(G, 'G');
          case 'I': if(options.out_I){
            super_t::process_packet(
                buffer, read_count,
                handler_I, handler_I.previous_seek, handler_I);
          }
          break;
#undef assign_case
        }
      }      
    }
};

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

  if(argc < 2){
    cerr << "Usage: " << argv[0] << " log.dat [options] [--out=(A|G|I)]" << endl;
    return -1;
  }
  
  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;
  }
  
  StreamProcessor processor;
  
  // カメラの設定データを送信
  if(options.cam_config){
    char buf[128];
    options.cam_config->read(buf, sizeof(buf));
    int read_bytes = options.cam_config->gcount();
    ostream &cam_config_out(options.spec2ostream(argv[1]));
    cam_config_out.write(buf, read_bytes);
    cam_config_out.flush();
    cerr << "cam_config: " << read_bytes << " bytes transmitted!" << endl;
  }
  
  options.out().precision(10);
  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;
}