#include "FASTTrackerProcessor.h"
#include "FASTTrackerPropertiesWidget.h"
#include "FASTCornerDetector.h"
#include <im_convert.h>
#include <im_process.h>
#include <memory>
#include <QAction>
#include <QtOpenGL>

FASTTrackerProcessor::FASTTrackerProcessor()
{
	_widget = NULL;
	_actionDetectFeatures = new QAction( QIcon(":/icons/interpret.png"), "Detect Features", this );
	_actionDetectFeatures->setObjectName( "actionDetectFeatures" );

	QMetaObject::connectSlotsByName( this );

	_enableCanny = false;
	_cannyStdDev = 1.0f;
	_cannyLowerThreshold = 30;
	_cannyUpperThreshold = 60;

	_grayImage = imImageCreate( 320, 240, IM_GRAY, IM_BYTE );
	_binaryImage = imImageCreate( 320, 240, IM_BINARY, IM_BYTE );

	_cornerDetector = new FASTCornerDetector( MAX_FEATURES );
}

FASTTrackerProcessor::~FASTTrackerProcessor()
{
	imImageDestroy( _grayImage );
	imImageDestroy( _binaryImage );
}

void FASTTrackerProcessor::resize( int width, int height )
{
	_width	= width;
	_height = height;
	imImageReshape( _grayImage, width, height );
	imImageReshape( _binaryImage, width, height );
}

void FASTTrackerProcessor::process( const unsigned char* original, unsigned char* processed )
{
	if( _enableCanny )
	{
		// create IM images
		imImage* originalImg = imImageInit( _width, _height, IM_GRAY, IM_BYTE, const_cast<unsigned char*>( original ), 0, 0 );
		imImage* processedImg = imImageInit( _width, _height, IM_GRAY, IM_BYTE, processed, 0, 0 );

		// use the Canny edge detector
		imProcessCanny( originalImg, _grayImage, _cannyStdDev );
		imProcessHysteresisThreshold( _grayImage, _binaryImage, _cannyLowerThreshold, _cannyUpperThreshold );
		imConvertColorSpace( _binaryImage, processedImg );

		// destroy IM images
		originalImg->data[0] = 0;
		processedImg->data[0] = 0;
		imImageDestroy( originalImg );
		imImageDestroy( processedImg );
	}
	else
	{
		memcpy( processed, original, _width * _height );
	}
	
	_cornerDetector->detect( processed, _width, _height );
}

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

void FASTTrackerProcessor::setupToolBar( QToolBar* toolBar )
{
	toolBar->addAction( _actionDetectFeatures );
}

void FASTTrackerProcessor::decorateGL( Canvas* canvas )
{
	if( _cornerDetector->getNumCornersDetected() == 0 )
		return;

	glPointSize( 3.0f );
	glBegin( GL_POINTS );
	glColor3f( 0.0, 1.0, 0.0 );

	int numDetectedCorners	= _cornerDetector->getNumCornersDetected();
	int *detectedCorners	= _cornerDetector->getDetectedCorners();
	for( int i = 0; i < numDetectedCorners; ++i )
	{
		glVertex2iv((GLint*)(detectedCorners + 2*i) );
	}

	glEnd();
}

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

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

void FASTTrackerProcessor::on_actionDetectFeatures_triggered()
{
	// empty
}
