#include "RuinsReconstructorProcessor.h"
#include "RuinsReconstructorPropertiesWidget.h"
#include "Canvas.h"
#include "tcgVisionGeom.h"
#include "tcgVisionCamera.h"
#include "kalman/kalman_camera.h"
#include <QAction>
#include <QtOpenGL>
#include "tecvr/Matrix4d.hpp"
namespace
{
	const float modelTransparency		= 0.0;
	const double CUBE_DIM				= 2.4;
	const int NUM_POINTS				= 39;
	const float DIFFUSE[4]				= {1,1,1,1};
	const float POSITION[4]				= {5,30,10,0};
		
	const int REFERENCE_INITIAL_POINTS[2*NUM_POINTS] = 
	{ 
		41,		135,
		52,		144,
		42,		151,
		93,		116,
		104,	123,
		104,	139,
		118,	119,
		118,	133,
		118,	102,
		109,	95	,
		94,		100,
		104,	108,
		130,	142,
		144,	136,
		145,	122,
		131,	127,
		134,	114,
		135,	99	,
		135,	81	,
		124,	74	,
		110,	78	,
		120,	85	,
		158,	131,
		161,	97	,
		163,	80	,
		176,	88	,
		191,	83	,
		156,	147,
		169,	156,
		168,	171,
		182,	163,
		183,	149,
		197,	142,
		199,	159,
		212,	184,
		214,	170,
		227,	163,
		52,		158,
		175,	106		
	};

	const double MODEL_POINTS[3*NUM_POINTS] =
	{
		0*CUBE_DIM,1*CUBE_DIM,6*CUBE_DIM,
		1*CUBE_DIM,1*CUBE_DIM,6*CUBE_DIM,
		0*CUBE_DIM,0*CUBE_DIM,6*CUBE_DIM,
		0*CUBE_DIM,1*CUBE_DIM,3*CUBE_DIM,
		1*CUBE_DIM,1*CUBE_DIM,3*CUBE_DIM,
		1*CUBE_DIM,0*CUBE_DIM,3*CUBE_DIM,
		1*CUBE_DIM,1*CUBE_DIM,2*CUBE_DIM,
		1*CUBE_DIM,0*CUBE_DIM,2*CUBE_DIM,
		1*CUBE_DIM,2*CUBE_DIM,2*CUBE_DIM,
		0*CUBE_DIM,2*CUBE_DIM,2*CUBE_DIM,
		0*CUBE_DIM,2*CUBE_DIM,3*CUBE_DIM,
		1*CUBE_DIM,2*CUBE_DIM,3*CUBE_DIM,
		2*CUBE_DIM,0*CUBE_DIM,2*CUBE_DIM,
		2*CUBE_DIM,0*CUBE_DIM,1*CUBE_DIM,
		2*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		2*CUBE_DIM,1*CUBE_DIM,2*CUBE_DIM,
		1*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		1*CUBE_DIM,2*CUBE_DIM,1*CUBE_DIM,
		1*CUBE_DIM,3*CUBE_DIM,1*CUBE_DIM,
		0*CUBE_DIM,3*CUBE_DIM,1*CUBE_DIM,
		0*CUBE_DIM,3*CUBE_DIM,2*CUBE_DIM,
		1*CUBE_DIM,3*CUBE_DIM,2*CUBE_DIM,
		3*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		3*CUBE_DIM,3*CUBE_DIM,1*CUBE_DIM,
		3*CUBE_DIM,4*CUBE_DIM,1*CUBE_DIM,
		4*CUBE_DIM,4*CUBE_DIM,1*CUBE_DIM,
		4*CUBE_DIM,4*CUBE_DIM,0*CUBE_DIM,
		4*CUBE_DIM,1*CUBE_DIM,2*CUBE_DIM,
		5*CUBE_DIM,1*CUBE_DIM,2*CUBE_DIM,
		5*CUBE_DIM,0*CUBE_DIM,2*CUBE_DIM,
		5*CUBE_DIM,0*CUBE_DIM,1*CUBE_DIM,
		5*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		5*CUBE_DIM,1*CUBE_DIM,0*CUBE_DIM,
		6*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		7*CUBE_DIM,0*CUBE_DIM,1*CUBE_DIM,
		7*CUBE_DIM,1*CUBE_DIM,1*CUBE_DIM,
		7*CUBE_DIM,1*CUBE_DIM,0*CUBE_DIM,
		1*CUBE_DIM,0*CUBE_DIM,6*CUBE_DIM,
		4*CUBE_DIM,3*CUBE_DIM,1*CUBE_DIM		
	};

	tecvr::Matrix4d aux;
	tcgVisionPoint3D pobs;
	tcgVisionPoint3D pref;
	tcgVisionPoint3D vup;
	tcgVisionPoint3D pw[100];
	tcgVisionPoint2D pf[100];
	double Q1[3][4];
	double Q2[3][4];
	double visionAngle; 
	double modelview[16];
	double projection[16];
	double R[9];
	KalmanStruct *kf, *kfc;
};

int tcgVisionCameraTransfPoint3Dto2D( double *H, tcgVisionPoint3D p1, tcgVisionPoint2D *p2 )
{
	double z;

	z = H[8]*p1.x + H[9]*p1.y + H[10]*p1.z + H[11];
	if (z) {
		p2->x = (H[0]*p1.x + H[1]*p1.y + H[2]*p1.z + H[3])/z;
		p2->y = (H[4]*p1.x + H[5]*p1.y + H[6]*p1.z + H[7])/z;
		return 1;
	}
	p2->x = (H[0]*p1.x + H[1]*p1.y + H[2]*p1.z + H[3])/z;
	p2->y = (H[4]*p1.x + H[5]*p1.y + H[6]*p1.z + H[7])/z;
	return 0;
}

void mult( tcgVisionPoint3D *v, double *m /*3x3*/ )
{
	v->x = -m[0]*-v->x + -m[1]*-v->y  -m[2]*-v->z;
	v->y = m[3]*-v->x + m[4]*-v->y + m[5]*-v->z;
	v->z = -m[6]*-v->x + -m[7]*-v->y - m[8]*-v->z;
}

void getOGLModelViewMatrix( tcgVisionPoint3D *T, double r[9], double mv[16] )
{
	mult( T, r );
	mv[0] =  -r[0]; mv[4] =  -r[1]; mv[8]  = -r[2]; mv[12] =  -T->x;
	mv[1] =  r[3]; mv[5] =  r[4]; mv[9]  = r[5]; mv[13] =  -T->y;
	mv[2] = -r[6]; mv[6] = -r[7]; mv[10] =  -r[8]; mv[14] = -T->z;
	mv[3] = 0.000; mv[7] = 0.000; mv[11] = 0.000; mv[15] = 1.000;
}

void getOGLProjectionMatrix( double f, double np, double fp, int width, int height, double pr[16] )
{
	double b, t, l, r;
	t = np*height/2.0/f;
	b = -t;
	r = np*width/2.0/f;
	l = -r;

	pr[0] = 2*np/(r-l); pr[4] =        0.0; pr[8]  =      (r+l)/(r-l); pr[12] =              0.0;
	pr[1] =        0.0; pr[5] = 2*np/(t-b); pr[9]  =      (t+b)/(t-b); pr[13] =              0.0;
	pr[2] =        0.0; pr[6] =        0.0; pr[10] = -(fp+np)/(fp-np); pr[14] = -2*fp*np/(fp-np);
	pr[3] =        0.0; pr[7] =        0.0; pr[11] =             -1.0; pr[15] =              0.0;
}

void getRFromMatrix4d( double *r, tecvr::Matrix4d &m )
{
	r[0] = m(0,0);
	r[1] = m(0,1);
	r[2] = m(0,2);
	r[3] = m(1,0);
	r[4] = m(1,1);
	r[5] = m(1,2);
	r[6] = m(2,0);
	r[7] = m(2,1);
	r[8] = m(2,2);
}

void setRFromMatrix4d( double *r, tecvr::Matrix4d &m )
{
	r[0] = m(0,0);
	r[1] = m(0,1);
	r[2] = m(0,2);
	r[3] = m(1,0);
	r[4] = m(1,1);
	r[5] = m(1,2);
	r[6] = m(2,0);
	r[7] = m(2,1);
	r[8] = m(2,2);
}

tcgVisionPoint3D crossProduct( tcgVisionPoint3D *v1, tcgVisionPoint3D *v2 ) 
{
	tcgVisionPoint3D out;
	
	out.x =  v1->y*v2->z - v1->z*v2->y;
	out.y = v1->z*v2->x - v1->x*v2->z;
	out.z = v1->x*v2->y - v1->y*v2->x;
	
	return out;
}

// v2-v1
tcgVisionPoint3D vecSub( tcgVisionPoint3D *v1, tcgVisionPoint3D *v2 ) 
{
	tcgVisionPoint3D out;

	out.x = v2->x - v1->x;
	out.y = v2->y - v1->y;
	out.z = v2->z - v1->z;

	return out;
}

void normalize( tcgVisionPoint3D *v )
{
	double norm = sqrt( v->x*v->x + v->y*v->y + v->z*v->z );

	v->x/=norm;
	v->y/=norm;
	v->z/=norm;
}
void getRotation( tcgVisionPoint3D *vup, tcgVisionPoint3D *center, tcgVisionPoint3D *pos, double *r )
{
	tcgVisionPoint3D zAxis = vecSub( pos, center );
	normalize( &zAxis );
	tcgVisionPoint3D xAxis = crossProduct( vup, &zAxis );
	normalize( &xAxis );
	tcgVisionPoint3D yAxis = crossProduct( &zAxis, &xAxis );
	normalize( &yAxis );

	r[0] = xAxis.x;
	r[1] = xAxis.y;
	r[2] = xAxis.z;

	r[3] = yAxis.x;
	r[4] = yAxis.y;
	r[5] = yAxis.z;

	r[6] = zAxis.x;
	r[7] = zAxis.y;
	r[8] = zAxis.z;
}

RuinsReconstructorProcessor::RuinsReconstructorProcessor( Canvas* canvas ) 
: _trackedPoints( NUM_POINTS )
{
	_actionStartInitialPointsSelection = new QAction( QIcon(":/icons/interpret.png"), "Start/Stop Initial Points Selection", this );
	_actionStartInitialPointsSelection->setObjectName( "actionStartInitialPointsSelection" );
	_actionStartInitialPointsSelection->setCheckable( true );

	_canvasPointGrabber = new CanvasPointGrabber( canvas );

	QMetaObject::connectSlotsByName( this );

	_widget					= NULL;
	_modelPoints			= NULL;

	_isCameraCurrentlyCalibrated		= false;
	_hasLastCameraCalibration			= false;
	_automaticInitialPointsMode			= false;
	_useKalmanFilter					= false;
	_kalmanInitialized					= false;	

	_referenceImage.load( "CalibrationFrame.png" );
	setModel( MODEL_POINTS, NUM_POINTS );

	//_cornerDetector = new FASTCornerDetector( CanvasPointGrabber::MAX_POINTS );
	_cornerDetector = new KLTCornerDetector( CanvasPointGrabber::MAX_POINTS );
	_cornerTracker = new CornerTracker();

	// Build menu
	QAction *action = _menuInitialPointsGetMode.addAction( "Automatic Initial Points" );
	action->setData( 0 );
	action = _menuInitialPointsGetMode.addAction( "Manual Initial Points" );
	action->setData( 1 );
	
	// TEST BEGIN *****************************************
	// Test calibration passing pre-defined calibration points (instead of grabbing from mouse clicking)
// 	_trackedPoints.setNumPoints( NUM_POINTS );
// 	int *p = _trackedPoints.getPoints();
// 	for( int i = 0 ; i < NUM_POINTS ; i ++ )
// 	{
// 		p[2*i] = REFERENCE_INITIAL_POINTS[2*i];
// 		p[2*i + 1] = REFERENCE_INITIAL_POINTS[2*i + 1];
// 	}
//   		
// 	_canvasPointGrabber->forceReady();
	// TEST END *******************************************
}

RuinsReconstructorProcessor::~RuinsReconstructorProcessor()
{
	// empty
}

void RuinsReconstructorProcessor::resize( int width, int height )
{
	_width	= width;
	_height = height;
}

void RuinsReconstructorProcessor::process( const unsigned char* original, unsigned char* processed )
{
	memcpy( processed, original, _width * _height );

	if( _automaticInitialPointsMode ) return;

	if( _trackedPoints.isEmpty() ) // Must feed application with initial points
	{
		// CanvasPointGrabber is only used in manual initial points selection
		
		switch( _canvasPointGrabber->getState() )
		{
		
		case CanvasPointGrabber::STATE_EMPTY:
			return;

		case CanvasPointGrabber::STATE_GRABBING:
			_trackedPoints.clear();
			_isCameraCurrentlyCalibrated = false;
			_hasLastCameraCalibration = false;
			return;

		case CanvasPointGrabber::STATE_READY:
			
			transferGrabberPoints( _canvasPointGrabber, &_trackedPoints );
			break;
		}	

	}
	// Once initial points had been given, the detection can be done
	_cornerDetector->detect( processed, _width, _height );
	int* points = _trackedPoints.getPoints();
	_cornerTracker->trackCorners( points, _cornerDetector->getDetectedCorners(), _trackedPoints.getNumPoints(), _cornerDetector->getNumCornersDetected(), &_trackedPoints );

	calibrateCamera();
}

QWidget* RuinsReconstructorProcessor::getPropertiesWidget( QWidget* parent )
{
	if( !_widget )
	{
		_widget = new RuinsReconstructorPropertiesWidget( this, parent, Qt::Tool );
	}
	return _widget;
}

void RuinsReconstructorProcessor::setupToolBar( QToolBar* toolBar )
{
	toolBar->addAction( _actionStartInitialPointsSelection );
}

void RuinsReconstructorProcessor::decorateGL( Canvas* canvas )
{
	
	if( canvas->getNumber() == 0 )
	{
		if( _canvasPointGrabber->isGrabbing() )
		{
			_canvasPointGrabber->decorateGL( canvas );
		}
		else if( _automaticInitialPointsMode )  // User set automatic initial points mode. This is a guide
												// to the place the model must be 
		{
			glMatrixMode( GL_PROJECTION );
			glPushMatrix();
			glMatrixMode( GL_MODELVIEW );
			glPushMatrix();

			calibrateCamera();
			loadCameraMatrices();

			glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
			glEnable( GL_BLEND ); 
			glBlendFunc ( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );

			drawer.drawRuin(true,0.8f);

			glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
			glDisable( GL_BLEND ); 

			glMatrixMode( GL_PROJECTION );
			glPopMatrix();
			glMatrixMode( GL_MODELVIEW );
			glPopMatrix();

			canvas->renderText( _width*0.1, _height*0.9, "MATCH VIRTUAL AND REAL MODELS" );
		}
	}
	else
	{
		// Use only canvas 0 on this state
		if( _automaticInitialPointsMode ) return;

		if( _canvasPointGrabber->isGrabbing() )
		{
			glEnable( GL_TEXTURE_2D );
			canvas->bindTexture( _referenceImage );
			glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE );
			glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR );
			glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR_MIPMAP_LINEAR );
			glBegin( GL_QUADS );
			glTexCoord2f( 0, 0 );
			glVertex2f( 0, 0 );
			glTexCoord2f( 1, 0 );
			glVertex2f( _width, 0 );
			glTexCoord2f( 1, 1 );
			glVertex2f( _width, _height );
			glTexCoord2f( 0, 1 );
			glVertex2f( 0, _height );
			glEnd();
			glDisable( GL_TEXTURE_2D );

			for( int i = 0 ; i < NUM_POINTS ; i ++ )
			{
				canvas->renderText( (int)REFERENCE_INITIAL_POINTS[2*i], (int)REFERENCE_INITIAL_POINTS[2*i + 1], QString::number( i ) );
			}
		}
		else 
		{	
			// Save the current matrices state
			glMatrixMode( GL_PROJECTION );
			glPushMatrix();
			glMatrixMode( GL_MODELVIEW );
			glPushMatrix();

			loadCameraMatrices();

			if( _isCameraCurrentlyCalibrated )
			{
				glPushAttrib( GL_LIGHTING_BIT );

				/* Setup light */
				glEnable( GL_LIGHTING );
				glEnable( GL_LIGHT0 );
				glLightfv( GL_LIGHT0, GL_POSITION, POSITION );
				glLightfv( GL_LIGHT0, GL_DIFFUSE, DIFFUSE );

				glEnable(GL_COLOR_MATERIAL);
				glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE, DIFFUSE );
			
// 				glEnable ( GL_POLYGON_OFFSET_FILL );
/*				glPolygonOffset (1.0, 1.0);*/
				glColorMask( GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE );
				
				drawer.drawRuin();

				glColorMask( GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

				glEnable( GL_BLEND ); 
				glBlendFunc ( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );

				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
				drawer.drawReconstructedRuin( false, modelTransparency );
				glDisable( GL_BLEND ); 

// 				glDisable(GL_DEPTH_FUNC);
// 				glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
// 				drawer.drawReconstructedRuin( true, 1.0 );
// 				glEnable(GL_DEPTH_FUNC);

//				glDisable ( GL_POLYGON_OFFSET_FILL );

				/* TEST CALIBRATION!!! ******/
				// 				glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
				// 				drawer.drawRuin();
				// 
				// 				glDepthFunc(GL_LESS);
				// 				glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
				// 
				// 				drawer.drawRuin(true);
				/* TEST CALIBRATION!!! ******/
				glDisable( GL_COLOR_MATERIAL );
				glPopAttrib();

			}
			else if( _hasLastCameraCalibration )
			{
				// Draw wire frame guide to match real model again
// 				glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
// 
// 				drawer.drawRuin( true );
// 
// 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
			}

			// Restore last matrices state
			glMatrixMode( GL_PROJECTION );
			glPopMatrix();
			glMatrixMode( GL_MODELVIEW );
			glPopMatrix();
		}
	}
}

bool RuinsReconstructorProcessor::onMousePress( Canvas* canvas, int x, int y )
{
	return true;
}

bool RuinsReconstructorProcessor::onMouseRelease( Canvas* canvas, int x, int y )
{
	return false;
}

void RuinsReconstructorProcessor::calibrateCamera()
{
	if( !_modelPoints || _trackedPoints.getNumPoints() < 8 )
	{
		_isCameraCurrentlyCalibrated = false;
		return;
	}

	int i,j, lastValidPoint;
	
	int np = _trackedPoints.getNumPoints();

	int *points = _trackedPoints.getPoints();

	lastValidPoint = 0;
	/* convert pairs of model and image points to the lib format */
	for (i=0; i<np; i++) 
	{
		j=lastValidPoint; // use only valid points
		while( j < np && _trackedPoints.isPointValid(j) == false )
		{
			j++;
		}
		lastValidPoint = j+1;
	
		pw[i].x = _modelPoints[3*j];
		pw[i].y = _modelPoints[3*j + 1];
		pw[i].z = _modelPoints[3*j + 2];
		pf[i].x = points[2*j] - _width*0.5;
		pf[i].y = points[2*j + 1] - _height*0.5;
	
		if( lastValidPoint >= np  ) 
		{ 
			break; 
		}
	}

	np = i;

	if( _useKalmanFilter )
	{
		if( !_kalmanInitialized )
		{
 			if( _hasLastCameraCalibration ) // use calman from the second calibration on
			{
				kf = init_kf_pointsTcg2D(np, pf, 10.0);
				aux.makeLookAt( tecvr::Vector3d(pobs.x, pobs.y, pobs.z), tecvr::Vector3d(pref.x,pref.y,pref.z), tecvr::Vector3d(vup.x, vup.y, vup.z ) );
				getRFromMatrix4d( R, aux );
 				kfc = init_kf_camera(0.5, 0.5, 1, R, &pobs, &visionAngle);
				_kalmanInitialized = true;
			}
		}

		// Adjust pf points using kalman filter
		//step_kf_pointsTcg2D(np, pf, 0.1,  1.0, kf);
	}

	if ( np <= 8 || !tcgVisionCameraCalibration( 0 , NULL, NULL, np, pw, pf, Q1, Q2, &pobs, &pref, &vup ) ) 
	{
		_isCameraCurrentlyCalibrated = false;
		return;
	}
	visionAngle = tcgVisionCameraFindVisionAngle( _height, Q2, pobs, pref );
		
	_isCameraCurrentlyCalibrated = true;
	_hasLastCameraCalibration = true;
}

void RuinsReconstructorProcessor::loadCameraMatrices()
{
	// No reason for loading matrices, the camera was never calibrated
	if( !_hasLastCameraCalibration && !_isCameraCurrentlyCalibrated && !_automaticInitialPointsMode )
	{
		return;
	}

	if( _useKalmanFilter && _kalmanInitialized )
	{
		aux.makeLookAt( tecvr::Vector3d(pobs.x, pobs.y, pobs.z), tecvr::Vector3d(pref.x,pref.y,pref.z), tecvr::Vector3d(vup.x, vup.y, vup.z ) );
		getRFromMatrix4d( R, aux );
		step_kf_camera(1, 0.1, 50, R, &pobs, &visionAngle, 1.0, kfc);
		aux.makeLookAt( tecvr::Vector3d(pobs.x, pobs.y, pobs.z), tecvr::Vector3d(pobs.x+R[6],pobs.y+R[7],pobs.z+R[8]), tecvr::Vector3d(R[3], R[4], R[5] ) );
	}
	else
	{
		aux.makeLookAt( tecvr::Vector3d(pobs.x, pobs.y, pobs.z), tecvr::Vector3d(pref.x,pref.y,pref.z), tecvr::Vector3d(vup.x, vup.y, vup.z ) );
	}


 	glMatrixMode( GL_MODELVIEW );
	glLoadMatrixd( aux.data() );

	glMatrixMode( GL_PROJECTION );
	glLoadIdentity();
	gluPerspective( visionAngle, (double)_width/_height, 1, 100 );
}

void RuinsReconstructorProcessor::setModel( const double *modelPoints, int numPoints )
{
	if( !_modelPoints )
	{
		_modelPoints = new double[3*numPoints];
	}

	_maxPointsModel = numPoints;

	std::copy( modelPoints, modelPoints+3*numPoints, _modelPoints );
}

void RuinsReconstructorProcessor::on_actionStartInitialPointsSelection_toggled( bool on )
{
	if( on == true )
	{
		QAction *choosen = _menuInitialPointsGetMode.exec( QCursor::pos() );
		if( !choosen )
		{
			on_actionStartInitialPointsSelection_toggled( false );
			return;
		}
		int optId = choosen->data().toInt();

		if( optId == 0 ) // Automatic mode
		{
			_automaticInitialPointsMode		= true;		
			_hasLastCameraCalibration		= false;
			_isCameraCurrentlyCalibrated	= false;
			
			_trackedPoints.setNumPoints( NUM_POINTS );

			int* points = _trackedPoints.getPoints();
			for( int i = 0; i < NUM_POINTS; ++i )
			{
				points[2*i] = REFERENCE_INITIAL_POINTS[2*i];
				points[2*i + 1] = _height - REFERENCE_INITIAL_POINTS[2*i+1];
			}
			// Reset all points trusty rate
		}
		else
		{
			_canvasPointGrabber->startGrabbing();
		}
		_actionStartInitialPointsSelection->setIcon( QIcon(":/icons/right.png" ) );
	}
	else
	{
		_actionStartInitialPointsSelection->setIcon( QIcon(":/icons/interpret.png" ) );
		if( _automaticInitialPointsMode  )
		{
			_automaticInitialPointsMode = false;
		}
		else
		{
			_canvasPointGrabber->stopGrabbing();
		}
	}
	_trackedPoints.resetTrustyRate();
	_trackedPoints.resetValidState();	
	_cornerDetector->refreshDetection();
}
