/*!
  \file
  \brief URG のレーザを表示する ARToolKit の利用例

  ARToolKit 付属の simpleTest2.c を改造して作成

  \author Satofumi KAMIMURA

  $Id: UrgLasers.cpp 1191 2009-07-30 05:52:49Z satofumi $
*/

#include "UrgCtrl.h"
#include "convert2d.h"
#include "Point.h"
#ifdef _WIN32
#include <windows.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#ifndef __APPLE__
#include <GL/gl.h>
#include <GL/glut.h>
#else
#include <OpenGL/gl.h>
#include <GLUT/glut.h>
#endif
#include <AR/gsub.h>
#include <AR/video.h>
#include <AR/param.h>
#include <AR/ar.h>
#include <vector>

using namespace qrk;
using namespace std;


/* set up the video format globals */
namespace
{
#ifdef _WIN32
  char *vconf = "Data\\WDM_camera_flipV.xml";
#else
  char *vconf = "-dev=/dev/video0 -channel=0 -debug -palette=YUV420P -width=640 -height=480";
#endif

  int xsize, ysize;
  int thresh = 100;
  int fps_count = 0;

  int mode = 1;

  char *cparam_name = "camera_para.dat";
  ARParam cparam;

  char *patt_name = "patt.hiro";
  int patt_id;
  double patt_width = 80.0;
  double patt_center[2] = {0.0, 0.0};
  double patt_trans[3][4];

  UrgCtrl* urg_ = NULL;
  vector<long> data_;
  Position<long> rotate_offset_(0, 0, deg(90));
}


/* cleanup function called when program exits */
static void cleanup(void)
{
  arVideoCapStop();
  arVideoClose();
  argCleanup();
}


static void keyEvent( unsigned char key, int x, int y)
{
  static_cast<void>(x);
  static_cast<void>(y);


  /* quit if the ESC key is pressed */
  if(key == 0x1b) {
    printf("*** %f (frame/sec)\n", (double)fps_count/arUtilTimer());
    cleanup();
    exit(0);
  }

  if( key == 'c' ) {
    printf("*** %f (frame/sec)\n", (double)fps_count/arUtilTimer());
    fps_count = 0;

    mode = 1 - mode;
    if( mode ) printf("Continuous mode: Using arGetTransMatCont.\n");
    else printf("One shot mode: Using arGetTransMat.\n");
  }
}


static void draw( double trans[3][4] )
{
  double gl_para[16];

  // URG データの更新
  // !!! Top-URG のデータ更新の方が Linux でのカメラ FPS より早いので、
  // !!! 現状では毎回データが取得できる
  urg_->capture(data_);

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

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

  glMatrixMode(GL_MODELVIEW);

  // URG の場合、上面からの高さを Z 軸方向にずらす
  glTranslatef(0.0, 0.0, -25.0);

  // URG データの描画
  vector<Point<long> > point_data;
  convert2d(point_data, urg_, data_, rotate_offset_);

  glColor4d(1.0, 0.3, 0.3, 0.5);
  glPointSize(2);
  glBegin(GL_POINTS);
  for (vector<Point<long> >::iterator it = point_data.begin();
       it != point_data.end(); ++it) {
    glVertex2d(it->x, it->y);
  }
  glEnd();

  glDisable( GL_DEPTH_TEST );
}


/* main loop */
static void mainLoop(void)
{
  static int contF = 0;
  ARUint8 *dataPtr;
  ARMarkerInfo *marker_info;
  int marker_num;
  int j, k;

  /* grab a vide frame */
  if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) {
    arUtilSleep(2);
    return;
  }
  if (fps_count == 0 ) {
    arUtilTimerReset();
  }
  fps_count++;

  argDrawMode2D();
  argDispImage( dataPtr, 0,0 );

  /* detect the markers in the video frame */
  if( arDetectMarker(dataPtr, thresh, &marker_info, &marker_num) < 0 ) {
    cleanup();
    exit(0);
  }

  arVideoCapNext();

  /* check for object visibility */
  k = -1;
  for( j = 0; j < marker_num; j++ ) {
    if( patt_id == marker_info[j].id ) {
      if( k == -1 ) k = j;
      else if( marker_info[k].cf < marker_info[j].cf ) k = j;
    }
  }
  if( k == -1 ) {
    contF = 0;
    argSwapBuffers();
    return;
  }

  /* get the transformation between the marker and the real camera */
  if( mode == 0 || contF == 0 ) {
    arGetTransMat(&marker_info[k], patt_center, patt_width, patt_trans);
  }
  else {
    arGetTransMatCont(&marker_info[k], patt_trans, patt_center, patt_width, patt_trans);
  }
  contF = 1;

  draw(patt_trans);

  argSwapBuffers();
}


static void init( void )
{
  ARParam wparam;

  /* open the video path */
  if( arVideoOpen( vconf ) < 0 ) exit(0);
  /* find the size of the window */
  if( arVideoInqSize(&xsize, &ysize) < 0 ) exit(0);
  printf("Image size (x,y) = (%d,%d)\n", xsize, ysize);

  /* set the initial camera parameters */
  if( arParamLoad(cparam_name, 1, &wparam) < 0 ) {
    printf("Camera parameter load error !!\n");
    exit(0);
  }
  arParamChangeSize( &wparam, xsize, ysize, &cparam );
  arInitCparam( &cparam );
  printf("*** Camera Parameter ***\n");
  arParamDisp( &cparam );

  if( (patt_id=arLoadPatt(patt_name)) < 0 ) {
    printf("pattern load error !!\n");
    exit(0);
  }

  /* open the graphics window */
  argInit( &cparam, 1.0, 0, 0, 0, 0 );
}


int main(int argc, char **argv)
{
  urg_ = new UrgCtrl;
  if (! urg_->connect("/dev/ttyACM0")) {
    printf("UrgCtrl::connect: %s\n", urg_->what());
    return 1;
  }
  urg_->setCaptureMode(AutoCapture);

  glutInit(&argc, argv);
  init();

  arVideoCapStart();
  argMainLoop( NULL, keyEvent, mainLoop );
  return 0;
}
