/***************************************************************************
 * $Id: recordCalPnts.cc 76 2008-07-15 09:21:10Z kaori $
 *
 *  Copyright   Kaori Hagihara
 *  Email: kaori.hagihara@uclouvain.be
 ****************************************************************************/

#define USE_CORNERFINDER

/*
 * Record coordinates pointed on images for extCalibration
 * usage: ./recordCalPnts imagefile_name output_directory_path
 *
 * recorded file format:
 *	x1, y1
 *	x2, y2
 *	. , .
 *	. , .
 * user will add 3d point coordinates
 * modified file format:
 *	x1, y1, X1, Y1, Z1
 *	x2, y2, X2, Y2, Z2
 *	 .,  .,  .,  .,  . 
 *	 .,  .,  .,  .,  . 
 */

#include <cv.h>
#include <highgui.h>
#include <iostream>
#include <fstream>
#include <vector>

using namespace std;

char windowName[] = "Click feature points";

void on_mouse( int event, int x, int y, int flags, void* param );
void resize_window( char windowName[], char c);
void save2D32fPnts( const char *fname, vector<CvPoint2D32f> pnts);
void itoa(int val, char *s);
char* strrev(char *s);

vector<CvPoint2D32f> pnts;
vector<CvPoint> txtPos;

IplImage *origImg, *outImg;

CvFont font;

int imgW, imgH;
int winW, winH;

int main(int argc,char **argv)
{
  if(argc < 3){
    cerr << "Usage: image file name\n"
	 << "       output directory\n";
    return -1;
  }

  string outPath, imgFname, pointFname;
  
  origImg = cvLoadImage( argv[1], CV_LOAD_IMAGE_GRAYSCALE);
  
  outImg =  cvCreateImage( cvGetSize( origImg), IPL_DEPTH_8U, 3);
  for( int coi=1; coi<=3; coi++){
    cvSetImageCOI( outImg, coi );
    cvCopy( origImg, outImg);
  }
  cvSetImageCOI( outImg, 0);
  
  cvInitFont( &font, CV_FONT_HERSHEY_SIMPLEX, 0.5f, 0.5f, 0.0f, 1, CV_AA);

  imgW = (cvGetSize( origImg )).width;
  imgH = (cvGetSize( origImg )).height;
  winW = imgW;
  winH = imgH;;
  
  cvNamedWindow( windowName, 0); 
  cvShowImage( windowName, outImg);
  cvSetMouseCallback( windowName, on_mouse);
  
  char c = cvWaitKey(0);
  while(c != 27){
    resize_window( windowName, c);
    c = cvWaitKey(0);
  }
  cvDestroyWindow( windowName ); // close window
  
  outPath = argv[2];
  pointFname = outPath + "/pointsInfo.csv";
  imgFname = outPath + "/pointRefImg.jpg";

  cvSaveImage( imgFname.c_str(), outImg);

  save2D32fPnts(  pointFname.c_str(), pnts);

  cvReleaseImage( &origImg);
  cvReleaseImage( &outImg);

  return 1;
}

void on_mouse( int event, int x, int y, int flags, void* param )
{
  switch( event){
  case CV_EVENT_LBUTTONDOWN:{
    CvPoint2D32f newPnt = cvPoint2D32f( (double)x, (double)y);

    cvCircle ( outImg, cvPoint(x, y), 2, cvScalar(0,255,255), -1);
#ifdef USE_CORNERFINDER
    
    cvFindCornerSubPix( origImg, &newPnt, 1, cvSize(5,5), cvSize(-1,-1), 
			cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 10, 1.0));
#endif
    pnts.push_back( newPnt);
    cvCircle ( outImg, cvPointFrom32f( newPnt), 2, cvScalar(0,0,255), -1);
    cvShowImage( windowName, outImg);
    break;}
    
  case CV_EVENT_LBUTTONUP:{
    char cval[10];
    CvPoint txtPnt = cvPoint( x, y);
    txtPos.push_back( txtPnt);
    itoa( pnts.size(), cval);
    cvPutText ( outImg, cval, txtPnt, &font, cvScalar(0,255,0));
    cvShowImage( windowName, outImg);
    break;
  }
  case CV_EVENT_RBUTTONDOWN:
    pnts.pop_back();
    txtPos.pop_back();
    for( int coi=1; coi<=3; coi++){
      cvSetImageCOI( outImg, coi );
      cvCopy( origImg, outImg);
    }
    cvSetImageCOI( outImg, 0);
    for( int i=0; i<(int)pnts.size(); i++){
      char cval[10];
      cvCircle ( outImg, cvPointFrom32f( pnts[i]), 2, cvScalar(0,0,255), -1);
      itoa( i+1, cval);
      cvPutText( outImg, cval, txtPos[i], &font, cvScalar(0,255,0));
    }
    cvShowImage( windowName, outImg);
    break;
  }
}

void resize_window( char windowName[], char c)
{ 
  if( c == 'T'){
    winW = (int)(winW*1.1);
    winH = (int)(winH*1.1);
  }else if( c == 'Q'){
    winW = (int)(winW*0.9);
    winH = (int)(winH*0.9);
  }
  cvResizeWindow( windowName, winW, winH);
}

void save2D32fPnts( const char *fname, vector<CvPoint2D32f> pnts)
{
  cerr << "saving 2D points coordinates to " << fname<< endl;

  ofstream of( fname);
  if(!of){
    cerr << "Failed to open " << fname << endl;
    exit(-1);
  }

  for( int i=0; i<(int)pnts.size(); i++)
    of << pnts[i].x << '\t' << pnts[i].y<< endl;
  
  of.close();
}

void itoa(int val, char *s)
{
  char *t;
  int mod;

  if(val < 0) {
    *s++ = '-';
    val = -val;
  }
  t = s;

  while(val) {
    mod = val % 10;
    *t++ = (char)mod + '0';
    val /= 10;
  }

  if(s == t)
    *t++ = '0';

  *t = '\0';

  strrev(s);
}

char* strrev(char *s) 
{
  char *ret = s;
  char *t = s;
  char c;

  while( *t != '\0' )t++;
  t--;

  while(t > s) {
    c = *s;
    *s = *t;
    *t = c;
    s++;
    t--;
  }

  return ret;
}
