/// Copyright 2011 Tsuyoshi Suenaga, Robotics Lab. NAIST
/// All rights researved
///
/// The distribution policy is described in the file COPYING.txt furnished
/// with this library.
///
/// Time-stamp: <2011-02-09 20:01:46 tsuyo-s>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <AR/gsub.h>
#include <GL/glew.h>

#ifdef _DEBUG
#include <AR/video.h>
#endif  // _DEBUG

#include <utility>
#include <fstream>
#include <sstream>
#include "MarkerDetect.h"

const int kTextureWidth  = 1024;
const int kTextureHeight = 512;

/// コンストラクタ
MarkerDetect::MarkerDetect() {
  camera_param.xsize = -1;
  camera_param.ysize = -1;
  markers.clear();

  marker_info = NULL;
  marker_num  = 0;
  gl_draw_init_flag = false;
}

/// デストラクタ
MarkerDetect::~MarkerDetect() {
  ResetMarkersConfig();
}

int MarkerDetect::LoadCameraParameter(const std::string &cparam_name,
                                      const int &num) {
  if ( arParamLoad(cparam_name.c_str(), num, &camera_param) < 0 ) {
    fprintf(stderr, "Camera parameter load error. [%s]\n",
           cparam_name.c_str());
    return -1;
  }

  arInitCparam(&camera_param);

  return 0;
}

int MarkerDetect::SetCameraParameter(const int &xsize, const int &ysize,
                                     const double mat[][4],
                                     const double dist_factor[]) {
  camera_param.xsize = xsize;
  camera_param.ysize = ysize;

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 4; ++j) {
      camera_param.mat[i][j] = mat[i][j];
    }
  }

  for (int i = 0; i < 4; ++i) {
    camera_param.dist_factor[i] = dist_factor[i];
  }

  arInitCparam(&camera_param);

  return 0;
}

int MarkerDetect::SetCameraParameter(const int &xsize, const int &ysize,
                                     const double &fx, const double &fy,
                                     const double &cx, const double &cy,
                                     const double &dist_cx,
                                     const double &dist_cy,
                                     const double &dist_factor,
                                     const double &dist_scale) {
  camera_param.xsize = xsize;
  camera_param.ysize = ysize;

  memset(camera_param.mat, 0, sizeof(camera_param.mat));
  camera_param.mat[0][0] = fx;
  camera_param.mat[1][1] = fy;
  camera_param.mat[0][2] = cx;
  camera_param.mat[1][2] = cy;
  camera_param.mat[2][2] = 1.0;

  camera_param.dist_factor[0] = dist_cx;
  camera_param.dist_factor[1] = dist_cy;
  camera_param.dist_factor[2] = dist_factor;
  camera_param.dist_factor[3] = dist_scale;

  arInitCparam(&camera_param);

  return 0;
}


#if 0
int MarkerDetect::SetCameraParameter(
    const int &xsize, const int &ysize,
    const std::vector<std::vector<double> > mat,
    const std::vector<double> dist_factor) {
  camera_param.xsize = xsize;
  camera_param.ysize = ysize;

  if (mat.size() != 3) {
    fprintf(stderr, "unable to set camera parameter.\nmat is matrix 3x4.\n");
    return -1;
  } else {
    for (int i = 0; i < 3; ++i) {
      if (mat[i].size() != 4) {
        fprintf(stderr,
                "unable to set camera parameter.\nmat is matrix 3x4.\n");
        return -1;
      } else {
        for (int j = 0; j < 4; ++j)
          camera_param.mat[i][j] = mat[i][j];
      }
    }
  }

  if (dist_factor.size() != 4) {
    fprintf(stderr, "unable to set camera parameter.\n");
    fprintf(stderr, "dist_factor is vector[4].\n");
  } else {
    for (int i = 0; i < 4; ++i) {
      camera_param.dist_factor[i] = dist_factor[i];
    }
  }

  arInitCparam(&camera_param);

  return 0;
}
#endif  // if 0

void MarkerDetect::PrintCameraParameter(const std::string comment) {
  fprintf(stderr, "%s\n", comment.c_str());
  arParamDisp(&camera_param);
}

int MarkerDetect::LoadMarkersConfig(const std::string &config_filename) {
  std::ifstream fin(config_filename.c_str());
  if (!fin) {
    fprintf(stderr,
            "Could not open config file %s\n", config_filename.c_str());
    return -1;
  }
  std::string str;
  while ( getline(fin, str) ) {
    if (str.size() == 0) break;
    if (str[0] == '#' || isspace(str[0])) continue;

    std::istringstream str_stream(str);
    int id;
    std::string filename;
    str_stream >> id >> filename;
    MarkerSet value;
    if ( (value.config = arMultiReadConfigFile(filename.c_str())) == NULL ) {
      fprintf(stderr, "config data load error !! [%s]\n",
              filename.c_str());
      fin.close();
      return -2;
    }

    std::pair<std::map<int, MarkerSet>::iterator, bool> res;
    res = markers.insert(std::make_pair(id, value));
    if (!res.second) {
      fprintf(stderr, "Duplicate ID number is not allowed.\n");
      fin.close();
      return -3;
    }
  }
  fin.close();

  return 0;
}

void MarkerDetect::ResetMarkersConfig(void) {
  std::map<int, MarkerSet>::iterator it = markers.begin();
  while (it != markers.end()) {
    arMultiFreeConfig((*it).second.config);
    ++it;
  }
  markers.clear();
}

void MarkerDetect::PrintMarkersConfig(void) {
  if (markers.empty()) {
    fprintf(stderr, "No markers are registered.\n");
    return;
  }
  std::map<int, MarkerSet>::iterator it = markers.begin();
  while (it != markers.end()) {
    fprintf(stderr, "Marker ID: %d\n", (*it).first);
    fprintf(stderr, "\tMarker Num: %d\n", (*it).second.config->marker_num);

    if ((*it).second.config->prevF) {
      fprintf(stderr, "\ttrans \n");
      for (unsigned int i = 0; i < 3; ++i) {
        fprintf(stderr, "\t");
        for (unsigned int j = 0; j < 4; ++j) {
          fprintf(stderr, " %lf", (*it).second.config->trans[i][j]);
        }
        fprintf(stderr, "\n");
      }
      fprintf(stderr, "\ttransR \n");
      for (unsigned int i = 0; i < 3; ++i) {
        fprintf(stderr, "\t");
        for (unsigned int j = 0; j < 4; ++j) {
          fprintf(stderr, " %lf", (*it).second.config->transR[i][j]);
        }
        fprintf(stderr, "\n");
      }
    } else {
      fprintf(stderr, "\ttrans <none>\n");
      fprintf(stderr, "\ttransR <none>\n");
    }
    ++it;
  }
}

void MarkerDetect::GetRegisteredMarkerIDs(std::vector<int> *ids) {
  std::map<int, MarkerSet>::iterator it = markers.begin();
  ids->clear();
  while (it != markers.end()) {
    ids->push_back((*it).first);
    ++it;
  }
}

void MarkerDetect::GetDetectedMarkerIDs(std::vector<int> *ids) {
  std::map<int, MarkerSet>::iterator it = markers.begin();
  ids->clear();
  while (it != markers.end()) {
    if ((*it).second.config->prevF)
      ids->push_back((*it).first);
    ++it;
  }
}

bool MarkerDetect::QueryRegistState(const int id) {
  std::map<int, MarkerSet>::iterator it = markers.find(id);
  if (it == markers.end()) return false;
  return true;
}

bool MarkerDetect::QueryDetectState(const int id) {
  std::map<int, MarkerSet>::iterator it = markers.find(id);
  if (it == markers.end()) return false;
  if (!(*it).second.config->prevF) return false;
  return true;
}

int MarkerDetect::Detect(const unsigned char *image, const int &threshold) {
  if (arDetectMarkerLite(const_cast<unsigned char *>(image),
                         threshold, &marker_info, &marker_num) < 0 ) {
    fprintf(stderr, "Could not finish detection normally\n");
    return -1;
  }

  std::map<int, MarkerSet>::iterator it = markers.begin();
  while (it != markers.end()) {
    if ( ((*it).second.err = arMultiGetTransMat(marker_info, marker_num,
                                                (*it).second.config)) < 0 ) {
      ++it;
      continue;
    }
    ++it;
  }

  return 0;
}

inline MarkerDetect::RGBColor RgbColor(const unsigned char r,
                                       const unsigned char g,
                                       const unsigned char b) {
  MarkerDetect::RGBColor color;
  color.r = r;
  color.g = g;
  color.b = b;
  return color;
}

void MarkerDetect::DrawLine(const int x1, const int y1,
                            const int x2, const int y2,
                            const int width, const int height,
                            MarkerDetect::RGBColor color,
                            unsigned char *image) {
  int dx = x2 - x1;
  int dy = y2 - y1;
  int n = (abs(dx) > abs(dy)) ? abs(dx) : abs(dy);

  double x_step = static_cast<double>(dx)/n;
  double y_step = static_cast<double>(dy)/n;

  for (int i = 0; i < n; ++i) {
    int x = round(x1 + i*x_step);
    int y = round(y1 + i*y_step);
    image[(y*width + x)*3    ] = color.r;
    image[(y*width + x)*3 + 1] = color.g;
    image[(y*width + x)*3 + 2] = color.b;
  }
}

void MarkerDetect::DrawSquare(const double vertex[4][2],
                              const int width, const int height,
                              MarkerDetect::RGBColor color,
                              unsigned char *image) {
  DrawLine(vertex[0][0], vertex[0][1],
           vertex[1][0], vertex[1][1],
           width, height, color, image);
  DrawLine(vertex[1][0], vertex[1][1],
           vertex[2][0], vertex[2][1],
           width, height, color, image);
  DrawLine(vertex[2][0], vertex[2][1],
           vertex[3][0], vertex[3][1],
           width, height, color, image);
  DrawLine(vertex[3][0], vertex[3][1],
           vertex[0][0], vertex[0][1],
           width, height, color, image);
}

void MarkerDetect::DrawVisibleMarkers(unsigned char *image) {
  for (int i = 0; i < marker_num; ++i) {
    DrawSquare(marker_info[i].vertex,
               camera_param.xsize, camera_param.ysize,
               RgbColor(255, 0, 0),
               image);
  }
}

void MarkerDetect::SavePPMImage(unsigned char *image, char *filename) {
  FILE *fp;
  int w, h;

  w = camera_param.xsize;
  h = camera_param.ysize;

  fp = fopen(filename, "wb");

  fprintf(fp, "P6\n%d %d\n255\n", w, h);

  fwrite(image, sizeof(*image), w*h*3, fp);
  fclose(fp);
}

// 以下の処理にはglutを使用するため、glutInitを事前に呼び出す必要がある。

int MarkerDetect::FrameBufferObjectInit(void) {
  glewInit();
  if (glCheckFramebufferStatusEXT(GL_FRAMEBUFFER_EXT)
      != GL_FRAMEBUFFER_COMPLETE_EXT) {
    fprintf(stderr, "framebuffer is not complete.\n");
    return -1;
  }

  glGenFramebuffersEXT(1, &framebuffer_name);
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, framebuffer_name);
  glGenTextures(1, &texture_name);
  glBindTexture(GL_TEXTURE_2D, texture_name);
  glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, kTextureWidth, kTextureHeight,
               0, GL_RGBA, GL_UNSIGNED_BYTE, 0);
  glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT,
                            GL_TEXTURE_2D, texture_name, 0);
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);

  return 0;
}

void MarkerDetect::FrameBufferObjectFinal(void) {
  glDeleteTextures(1, &texture_name);
  glDeleteFramebuffersEXT(1, &framebuffer_name);
}

/// GUIの初期化。カメラパラメータを設定後に呼び出す必要がある
int MarkerDetect::GlDrawInit(void) {
  // 多重起動の防止
  if (gl_draw_init_flag) return 0;

  if (camera_param.xsize < 0 || camera_param.ysize < 0) {
    fprintf(stderr, "Camera parameter has not be setted yet.");
    return -1;
  }
  argInit(&camera_param, 1.0, 0, 0, 0, 0);
  glutHideWindow();

  if (FrameBufferObjectInit() < 0) {
    fprintf(stderr, "Unable to use framebuffer object.");
    return -2;
  }
  gl_draw_init_flag = true;
  return 0;
}

void MarkerDetect::GlDrawFinal(void) {
  if (gl_draw_init_flag) {
    FrameBufferObjectFinal();
    argCleanup();
  }
  gl_draw_init_flag = false;
}

static void draw(double trans1[3][4], double trans2[3][4], int mode) {
  double  gl_para[16];
  GLfloat mat_ambient[]     = {0.0, 0.0, 1.0, 1.0};
  GLfloat mat_ambient1[]    = {1.0, 0.0, 0.0, 1.0};
  GLfloat mat_flash[]       = {0.0, 0.0, 1.0, 1.0};
  GLfloat mat_flash1[]      = {1.0, 0.0, 0.0, 1.0};
  GLfloat mat_flash_shiny[] = {50.0};
  GLfloat mat_flash_shiny1[]= {50.0};
  GLfloat light_position[]  = {100.0, -200.0, 200.0, 0.0};
  GLfloat ambi[]            = {0.1, 0.1, 0.1, 0.1};
  GLfloat lightZeroColor[]  = {0.9, 0.9, 0.9, 0.1};

  argDrawMode3D();
  argDraw3dCamera(0, 0);
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);

  /* load the camera transformation matrix */
  glMatrixMode(GL_MODELVIEW);
  argConvGlpara(trans1, gl_para);
  glLoadMatrixd(gl_para);
  argConvGlpara(trans2, gl_para);
  glMultMatrixd(gl_para);

  if (mode == 0) {
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);
    glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
    glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash);
    glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny);
    glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
  } else {
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);
    glLightfv(GL_LIGHT0, GL_AMBIENT, ambi);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor);
    glMaterialfv(GL_FRONT, GL_SPECULAR, mat_flash1);
    glMaterialfv(GL_FRONT, GL_SHININESS, mat_flash_shiny1);
    glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient1);
  }
  glMatrixMode(GL_MODELVIEW);
  glTranslatef(0.0, 0.0, 25.0);
  if (!arDebug) glutSolidCube(50.0);
  else          glutWireCube(50.0);
  glDisable(GL_LIGHTING);

  glDisable(GL_DEPTH_TEST);
}

void MarkerDetect::GlDraw2D(unsigned char *image) {
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, framebuffer_name);

  argDrawMode2D();
  argDispImage(image, 0, 0);

  glColor3f(1.0, 0.0, 0.0);
  glLineWidth(1.0);
  for (int i = 0; i < marker_num; ++i) {
    argDrawSquare(marker_info[i].vertex, 0, 0);
  }

  glReadPixels(0, 0, camera_param.xsize, camera_param.ysize,
               GL_RGB, GL_UNSIGNED_BYTE, image);
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
  // 上下反転
  for (int i = 0; i < camera_param.ysize/2; ++i) {
    for (int j = 0; j < camera_param.xsize*3; ++j) {
      int top_down  = 3*i*camera_param.xsize + j;
      int bottom_up = 3*(camera_param.ysize - i - 1)*camera_param.xsize + j;
      GLubyte tmp      = image[top_down];
      image[top_down]  = image[bottom_up];
      image[bottom_up] = tmp;
    }
  }
}

void MarkerDetect::GlDraw3D(unsigned char *image) {
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, framebuffer_name);

  argDrawMode2D();
  argDispImage(image, 0, 0);

  argDrawMode3D();
  argDraw3dCamera(0, 0);
  glClearDepth(1.0);
  glClear(GL_DEPTH_BUFFER_BIT);

  std::map<int, MarkerSet>::iterator it = markers.begin();
  while (it != markers.end()) {
    if (!(*it).second.config->prevF) {
      // 不可視マーカセットを無視
      ++it;
      continue;
    }
    for (int i = 0; i < (*it).second.config->marker_num; ++i) {
      if ( (*it).second.config->marker[i].visible >= 0 )
        draw((*it).second.config->trans,
             (*it).second.config->marker[i].trans, 0);
      else
        draw((*it).second.config->trans,
             (*it).second.config->marker[i].trans, 1);
    }
    ++it;
  }
  glReadPixels(0, 0, camera_param.xsize, camera_param.ysize,
               GL_RGB, GL_UNSIGNED_BYTE, image);
  // 上下反転
  for (int i = 0; i < camera_param.ysize/2; ++i) {
    for (int j = 0; j < camera_param.xsize*3; ++j) {
      int top_down  = 3*i*camera_param.xsize + j;
      int bottom_up = 3*(camera_param.ysize - i - 1)*camera_param.xsize + j;
      GLubyte tmp      = image[top_down];
      image[top_down]  = image[bottom_up];
      image[bottom_up] = tmp;
    }
  }
  glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
}

// ******************** for DEBUG **********************
#ifdef _DEBUG
// 以下の関数群はデバッグ用関数である。
// デバイスへ接続するため、複数のインスタンスから実行することは推奨されない

// ビデオデバイスの開始処理 [デバッグ用関数]
int MarkerDetect::VideoInit(char *config) {
  if (arVideoOpen(config) < 0) return -1;
  if (arVideoCapStart() < 0)   return -2;
  return 0;
}
// ビデオ画像の取得 [デバッグ用関数]
unsigned char * MarkerDetect::VideoGetImage(void) {
  if (arVideoCapNext() < 0) return NULL;
  return (unsigned char *)arVideoGetImage();
}
// ビデオデバイスの終了処理 [デバッグ用関数]
int MarkerDetect::VideoFinal(void) {
  if (arVideoCapStop() < 0) return -2;
  if (arVideoClose() < 0)   return -1;
  return 0;
}

#endif  // _DEBUG
// ******************** for DEBUG **********************
