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

#include <stdio.h>
#include <io.h>
#include <fcntl.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;
    }
  }
  
  static void set_dcb(DCB &dcb){
    dcb.BaudRate = CBR_115200;
  }
  
#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);
          com_in->buffer().config_dcb(set_dcb);
          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);
          com_out->buffer().config_dcb(set_dcb);
          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;
    }
    return false;
  }
};

using namespace std;

struct Options : public GlobalOptions {
  bool save_image;
  istream *cam_config;
  ostream *raw_out;
  const char *image_type;
  const char *classifier;
  bool grey;
  int debug_level;
  
  Options() 
      : GlobalOptions(),
      save_image(false),
      cam_config(NULL), raw_out(NULL),
      image_type("jpg"),
      classifier(NULL),
      grey(false),
      debug_level(0) {}
  ~Options(){}
  
  /**
   * コマンドに与えられた設定を読み解く
   * 
   * @param spec コマンド
   * @return (bool) 解読にヒットした場合はtrue、さもなければfalse
   */
  bool check_spec(char *spec){
    char c;
    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;
    }
    if(strstr(spec, "--grey=") == spec){
      char *value(spec + strlen("--grey="));
      grey = (strcmp(value, "on") == 0);
      cerr << "grey: " << (grey ? "on" : "off") << endl;
      return true;
    }
    return GlobalOptions::check_spec(spec);
  }
} options;

class StreamProcessor {
  protected:
    int invoked;
    
  public:
    /**
     * SylphideProtocol形式で入ってくるCページ用の処理器
     * 
     */
    class HandlerI {
      protected:
        typedef HandlerI self_t;
        stringstream ss;
        bool previous_seek;
        bool got_size;
        unsigned int image_bytes;
        const char *window_id;
        cv::CascadeClassifier classifier;
        static const string header;
      public:
        unsigned int invoked;
        HandlerI(const char *id = "demo") 
            : ss(), previous_seek(false), got_size(false), invoked(0),
            image_bytes(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() {}
        
        /**
         * パケットを探し解析する関数
         * 
         * packet => [header](size in 4 bytes)(binary in (size) bytes)
         */
        void update(const char *buf, const unsigned int &size){
          ss.write(buf, size);
          
          // 頭だしを行う
          if(!previous_seek){
            size_t found_index(ss.str().find(header));
            if(found_index == string::npos){
              if(ss.str().size() >= header.size()){
                ss.str(ss.str().substr((ss.str().size() - header.size()) + 1));
                ss.clear(stringstream::goodbit);
                ss.seekp(ss.str().size());
              }
              return;
            }
            
            if(options.debug_level > 0){
              cerr << header << " : " << ss.str().size() << endl;
            }
            
            if(ss.str().size() > (found_index + header.size())){
              ss.str(ss.str().substr(found_index + header.size()));
              ss.seekp(ss.str().size());
            }else{
              ss.str("");
            }
            ss.clear(stringstream::goodbit);
            
            previous_seek = true;
          }
          
          // サイズの取得
          if(!got_size){
            if(ss.str().size() < 4){return;}
            // Little Endian
            image_bytes 
                = le_char4_2_num<unsigned int>(ss.str().c_str()[0]);
            
            if(ss.str().size() > 4){
              ss.str(ss.str().substr(4));
              ss.seekp(ss.str().size());
            }else{
              ss.str("");
            }
            ss.clear(stringstream::goodbit);
                
            got_size = true; 
          }
          
          if(options.debug_level > 0){
            cerr << image_bytes << " : " << size << " : " << ss.str().size() << endl;
          }
          
          // サイズ分データがそろったか確認する
          if(ss.str().size() < image_bytes){return;}
          
          if(image_bytes > 0){
          
            char *buf(new char [image_bytes]);
            memcpy(buf, ss.str().c_str(), image_bytes);
            
            // スキップしておく
            if(ss.str().size() > image_bytes){
              ss.str(ss.str().substr(image_bytes));
              ss.seekp(ss.str().size());
            }else{
              ss.str("");
            }
            ss.clear(stringstream::goodbit);
              
            cerr << image_bytes;
            
            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(image_bytes / 2), width, height, remain_pixels(pixels);
              for(int i(0); i < sizeof(aspects) / sizeof(aspects[0]); i++){
                int unit_length(sqrt((float_t)image_bytes / 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) + (image_bytes / 2 * 3)]; // 16bits(LE) => 24bits
              memcpy(buf, header, header_length);
              for(int offset_src(0), offset_dst(header_length); offset_src < image_bytes;){
                int pixel16(0);
                pixel16 |= (unsigned char)(buf_orig[offset_src++]);
                pixel16 |= ((int)((unsigned char)(buf_orig[offset_src++])) << 8);
                unsigned char 
                    b((pixel16 & 0xF800) >> 8),
                    g((pixel16 & 0x07E0) >> 3),
                    r((pixel16 & 0x001F) << 3);
                
                if(options.grey){
                  buf[offset_dst++] = (r + g + b) / 3; // r ?
                  buf[offset_dst++] = (r + g + b) / 3; // g
                  buf[offset_dst++] = (r + g + b) / 3; // b
                }else{
                  buf[offset_dst++] = r; // r ?
                  buf[offset_dst++] = g; // g
                  buf[offset_dst++] = b; // b
                }
              }
              delete [] buf_orig;
              image_ext = "ppm";
              image_bytes = header_length + (image_bytes / 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, image_bytes);
              }
              img_out.flush();
              img_out.close();
            }
            
            cv::Mat img_buf(
                cv::imdecode(cv::Mat(std::vector<char>(buf, buf + image_bytes)), 1));
            
            // 回転する
            try{
              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;
            }catch(exception e){
              cerr << " " << e.what();
            }
            
            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;
          }
          
          
          previous_seek = false;
          got_size = false;
          invoked++;
        }
    } handler_I;
  public:
    StreamProcessor()
        : invoked(0), handler_I() {
      
    }
    ~StreamProcessor(){}
    
    /**
     * ファイル等のストリームから1ページ単位で処理を行う関数
     * 
     * @param in ストリーム
     * @return (bool) 処理が成功したかどうか
     */
    void process(istream &in){
      char buffer[32];
      
      while(true){
        int read_count;
        in.read(buffer, sizeof(buffer));
        read_count = in.gcount();
        if(in.fail()){return;}
        if(read_count == 0){continue;}
        invoked++;
        
        if(options.raw_out){
          options.raw_out->write(buffer, read_count);
        }
      
        if(options.debug_level > 1){
          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 < sizeof(buffer)){
            cerr << "--skipped-- : " << invoked << " page ; count = " << read_count << endl;
          }
        }
        
        handler_I.update(buffer, read_count);
      }      
    }
};

const string StreamProcessor::HandlerI::header = string("Hello");

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);
  processor.process(options.spec2istream(argv[1]));
  
  return 0;
}