/** 
* @file LocalFeatureSift.cpp
* @brief local feature with sift
* @author Erik Rodner
* @date 02/05/2008
*/
// #ifdef NICE_USELIB_ICE
//   #include <image_nonvis.h>
// #endif
#include <iostream>

#include "vislearning/features/localfeatures/sift.h"
#include "vislearning/features/localfeatures/LocalFeatureSift.h"

using namespace OBJREC;
using namespace std;
using namespace NICE;

LocalFeatureSift::LocalFeatureSift( const Config *conf )
{
    octaves          = conf->gI("LFSiftPP", "octaves", 			6);
    levels           = conf->gI("LFSiftPP", "levels", 			3);
    first_octave     = conf->gI("LFSiftPP", "first_octave", 		-1);
    normalizeFeature = conf->gB("LFSiftPP", "normalize_feature", 	true );
    magnif 	     = conf->gD("LFSiftPP", "magnif", 			3 );
    deletemode       = conf->gB("LFSiftPP", "deletemode", 		true );	
    integerValues    = conf->gB("LFSiftPP", "integer_values", 		true );
}

LocalFeatureSift::~LocalFeatureSift()
{
}

void LocalFeatureSift::sortPositions(VVector & positions) const
{
	// < Key  , Val >
	map<double, bool> scales;

	// in der Map entpsrechende Skalewerte auf true setzten, jeder Skalewert ist dabei einzigartig
	for( vector< NICE::Vector >::iterator i  = positions.begin(); 
			i != positions.end();i++)
	{
		const NICE::Vector & pos = *i;
		scales[pos[2]] = true;
	}
	
	VVector newpositions;
	
	map<double,bool>::iterator iter;
	// Map durchlaufen
	for( iter = scales.begin(); iter != scales.end(); ++iter ) 
	{
		// alle Positionen durchlaufen
		for( vector< NICE::Vector >::iterator i  = positions.begin(); 
				   i != positions.end();i++)
		{
			
			const NICE::Vector & pos = *i;
			if(pos[2] == iter->first)
			{
				newpositions.push_back(pos);
			}
		}
	}
	positions = newpositions;
}

void LocalFeatureSift::computeDesc( const NICE::Image & img, VVector & positions, VVector & descriptors ) const
{
	
	int         O      = octaves ;
	int const   S      = levels ;
	int const   omin   = first_octave;
	float const sigman = .5 ; //.5
	float const sigma0 = 1.6 * powf(2.0f, 1.0f / S) ;

	if (O < 1) 
	{
		O = std::max(int(std::floor(log2
				(std::min(img.width(),img.height()))) - omin - 3), 1) ;
	}
	
	const unsigned char *blockimg = (unsigned char*) img.getPixelPointer();
	float *blockimgfl = new float[img.width() * img.height()];
	for ( int k = 0 ; k < img.width() * img.height() ; k++ )
		blockimgfl[k] = blockimg[k];

	VL::Sift sift( blockimgfl, img.width(), img.height(),
				   sigman, sigma0, O, S, omin, -1, S+1) ;

	sift.process ( blockimgfl, img.width(), img.height() );

	sift.setMagnification ( magnif );
	sift.setNormalizeDescriptor ( normalizeFeature );

	const int descr_size = 128;
	VL::float_t *descr_pt = new VL::float_t [descr_size];
	VL::float_t angles[4] ;

	NICE::Vector feature (descr_size);
	NICE::Vector pos     ( 4 );


	for( vector< NICE::Vector >::iterator i  = positions.begin(); 
			i != positions.end();)
	{
		const NICE::Vector & pos = *i;
		double x = pos[0];
		double y = pos[1];
		assert(pos[0] < img.width());
		assert(pos[1] < img.height());
		double s = pos[2];
		bool deleteFeature = false;
		VL::Sift::Keypoint kp = sift.getKeypoint (x,y,s);

		double angle = 0.0;

		if ( pos.size() < 4 ) 
		{
			int nangles = sift.computeKeypointOrientations(angles, kp);

			if ( nangles > 0 )
			{
				angle = angles[0]; 
			} 
			else 
			{
				if(deletemode)
					deleteFeature = true;
				else
					angle = 0;
			}
		} 
		else 
		{
			angle = pos[3];
		}

		if ( ! deleteFeature ) 
		{
			sift.computeKeypointDescriptor ( descr_pt, kp, angle ); 		
			for ( int j = 0 ; j < descr_size ; j++ )
				// Umwandlung in Integer moegl.
				feature[j] = (integerValues ? (int)(512*descr_pt[j]) : descr_pt[j]);

			descriptors.push_back ( feature );

			i++;
		} 
		else 
		{
			i = positions.erase(i);
		}
	}

	delete [] blockimgfl;
	delete [] descr_pt;
 
}

int LocalFeatureSift::getDescriptors ( const NICE::Image & img, VVector & positions, VVector & descriptors ) const
{
	sortPositions(positions);
	
	computeDesc(img, positions, descriptors);
	
    return 0;
}

void LocalFeatureSift::visualizeFeatures ( NICE::Image & mark,
				 const VVector & positions,
				 size_t color ) const
{
	fthrow("LocalFeatureSift::visualizeFeatures -- not yet implemented due to old ICE version.");
//TODO check this!
// #ifdef NICE_USELIB_ICE
//     ice::Image mark_ice = ice::NewImg ( mark.width(), 
// 	mark.height(), 255 );
//     for ( size_t k = 0 ; k < positions.size() ; k++ )
//     {
// 		const NICE::Vector & pos = positions[k];
// 		ice::Matrix points ( 0, 2 );
// 		const int size = 6;
// 		points.Append ( ice::Vector(-size, -size) );
// 		points.Append ( ice::Vector(-size, size) );
// 		points.Append ( ice::Vector(size, size) );
// 		points.Append ( ice::Vector(size, -size) );
// 
// 		ice::Trafo tr;
// 
// 		tr.Scale ( 0, 0, pos[2] );
// 		tr.Rotate ( 0, 0, pos[3] );
// 		tr.Shift ( pos[0], pos[1] );
// 
// 		ice::TransformList(tr, points);
// 
// 		for ( int j = 0 ; j < points.rows(); j++ )
// 		{
// 			if (points[j][0] < 0 ) 
// 			points[j][0] = 0;
// 			if (points[j][0] >= mark_ice->xsize) 
// 			points[j][0] = mark_ice->xsize - 1;
// 			if (points[j][1] < 0 ) 
// 			points[j][1] = 0;
// 			if (points[j][1] >= mark_ice->ysize) 
// 			points[j][1] = mark_ice->ysize - 1;
// 		}
// 		ice::DrawPolygon ( points, color, mark_ice );
//     }
// 
//     for ( unsigned int y = 0 ; y < mark.height(); y++ )
// 		for ( unsigned int x = 0 ; x < mark.width(); x++ )
// 			mark.setPixel(x,y, GetVal(mark_ice,x,y));
// #else
// 	cerr << "uses ice visualization, please install ice or change to NICE visualization" << endl;
// #endif
}