#include "KLTCornerDetector.h"
#include "klt/pnmio.h"
#include "klt/klt.h"
#include <memory>


static KLT_TrackingContext tc;
static KLT_FeatureList fl;

extern "C" int KLT_verbose;

namespace
{
	int frameCount = 1;
};

KLTCornerDetector::KLTCornerDetector(  int maxCorners ) : CornerDetector( maxCorners )
{
	KLT_verbose = 0;
	_numDetectedCorners = 0;
	_reselectFeaturesOnNextFrame = true;
	
	_featureValues = new int[maxCorners];

	std::fill( _featureValues, _featureValues + maxCorners, 0 );

	tc = KLTCreateTrackingContext();
	tc->sequentialMode = TRUE;
	tc->mindist = 10;

	KLTPrintTrackingContext( tc );
	fl = KLTCreateFeatureList( maxCorners );
}

void KLTCornerDetector::detect( const unsigned char* img, int w, int h )
{
	unsigned char *imgNonConst = const_cast<unsigned char*>(img);

	if( _reselectFeaturesOnNextFrame )
	{
		KLTSelectGoodFeatures( tc, imgNonConst, w, h, fl );
		KLTTrackFeatures( tc, imgNonConst, imgNonConst, w, h, fl );
		_reselectFeaturesOnNextFrame = false;
	}
	else
	{
		KLTTrackFeatures( tc, NULL, imgNonConst, w, h, fl );
	}
	
	_numDetectedCorners = fl->nFeatures>_maxCorners?_maxCorners:fl->nFeatures;

	int i;
	for( i = 0; i < _numDetectedCorners; i++ )
	{
		_detectedCorners[2*i] = (int)fl->feature[i]->x;
		_detectedCorners[2*i + 1] = (int)fl->feature[i]->y;
		_featureValues[i] = fl->feature[i]->val;
	}

	
}