/** 
* @file LFSiftPP.cpp
* @brief Sift++ interface
* @author Erik Rodner
* @date 11/19/2007

*/
#include <iostream>
#include <sstream>

// #ifdef NICE_USELIB_ICE
//   #include <image_nonvis.h>
// #endif
#include "vislearning/features/localfeatures/LFSiftPP.h"


using namespace std;

using namespace NICE;



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

using namespace OBJREC;


LFSiftPP::LFSiftPP( const Config *conf )
{
    threshold      = conf->gD("LFSiftPP", "threshold", 0.0);
    edgeThreshold  = conf->gD("LFSiftPP", "edge_threshold", 10.0);
    octaves        = conf->gI("LFSiftPP", "octaves", 6);
    first_octave   = conf->gI("LFSiftPP", "first_octave", -1);
    levels         = conf->gI("LFSiftPP", "levels", 3);
    
    minScale       = conf->gD("LFSiftPP", "min_scale", 1);
    maxScale       = conf->gD("LFSiftPP", "max_scale", 4);
    numScales      = conf->gI("LFSiftPP", "num_scales", 10);
    numAngles      = conf->gI("LFSiftPP", "num_angles", 6 );

    std::string descriptorAlignment_s = conf->gS("LFSiftPP", "descriptor_alignment", "detector" );

    if ( descriptorAlignment_s == "detector" )
		descriptorAlignment = DALGIN_DETECTOR;
    else if ( descriptorAlignment_s == "multiple" )
		descriptorAlignment = DALIGN_MULTIPLE;
    else {
		fprintf (stderr, "LFSiftPP: descriptor alignment method unknown !\n");
		exit(-1);
    }

    normalizeFeature = conf->gB("LFSiftPP", "normalize_feature", false );

    std::ostringstream os;
    os << "siftpp_" << "l" << levels << "_o" 
       << octaves << "_m" << minScale << "_t" 
       << threshold << "_e" << edgeThreshold;

    if ( ! normalizeFeature )
		os << "_nd";
}

LFSiftPP::~LFSiftPP()
{
}

int LFSiftPP::getDescSize () const
{
    return 128;
}

int LFSiftPP::extractFeatures ( const NICE::Image & img, VVector & features,
			        VVector & positions ) const
{
    int         O      = octaves ;
    int const   S      = levels ;
    int const   omin   = first_octave;

    float const sigman = .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();
    if ( blockimg == NULL ) {
	fprintf (stderr, "FATAL ERROR: do not use subimages !!\n");
	exit(-1);
    }
    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() );
    // compute keypoints
    sift.detectKeypoints(threshold, edgeThreshold) ;

    const int descr_size = 128;
    VL::float_t *descr_pt = new VL::float_t [descr_size];
    VL::float_t angles[4] ;
    int keypointCount = 0;
    const int maxKeyPoints = std::numeric_limits<int>::max();

    NICE::Vector feature (descr_size);
    for( VL::Sift::KeypointsConstIter iter = sift.keypointsBegin() ;
	 iter != sift.keypointsEnd() ; ++iter, keypointCount++ )
    {
		if ( keypointCount >= maxKeyPoints ) break;

		if ( descriptorAlignment == DALGIN_DETECTOR )
		{
			if ( iter->s < minScale ) continue;

			int nangles = sift.computeKeypointOrientations(angles, *iter);
			for ( int i = 0 ; i < nangles ; i++ )
			{
				sift.computeKeypointDescriptor ( descr_pt, *iter, angles[i] ); 		
				for ( int j = 0 ; j < descr_size ; j++ )
				{
					if ( isnan(descr_pt[j]) ) {
					fprintf (stderr, "Descriptor with NAN values !!\n");
					exit(-1);
					} else {
					feature[j] = descr_pt[j];
					}
				}
				NICE::Vector p (4);
				p[0] = iter->x;
				p[1] = iter->y;
				p[2] = iter->s;
				p[3] = angles[i];
				positions.push_back(p);
				if ( normalizeFeature )
					feature.normalizeL2();
				features.push_back ( feature );
			}
		} else if ( descriptorAlignment == DALIGN_MULTIPLE ) {
			
			double angle_step = 2 * M_PI / numAngles;
			double scale_step = (maxScale - minScale) / numScales;

			for ( int j = 0 ; j < numAngles ; j++ )
			{
				double scale = j * scale_step;
				for ( int i = 0 ; i < numAngles ; i++ )
				{
					double angle = angle_step * i;
					sift.computeKeypointDescriptor ( descr_pt, *iter, angle ); 		
					for ( int j = 0 ; j < descr_size ; j++ )
					{
						if ( isnan(descr_pt[j]) ) {
							fprintf (stderr, "Descriptor with NAN values !!\n");
							exit(-1);
						} else {
							feature[j] = descr_pt[j];
						}
					}
					NICE::Vector p (4);
					p[0] = iter->x;
					p[1] = iter->y;
					p[2] = iter->s;
					p[3] = angles[i];
					positions.push_back(p);

					if ( normalizeFeature )
					feature.normalizeL2();
					features.push_back ( feature );
				}
			}
		}
    }

    fprintf (stderr, "LFSiftpp::convert: Number of Keypoints = %d\n", keypointCount );

    delete [] blockimgfl;
    delete [] descr_pt;
    if ( keypointCount <= 0 )
		fprintf (stderr, "FATAL ERROR: no keypoints found !!\n");

    return 0;
}

	

void LFSiftPP::visualizeFeatures ( NICE::Image & mark,
				 const VVector & positions,
				 size_t color ) const
{
	fthrow("LFSiftPP::visualizeFeatures -- not yet implemented due to old ICE version.");
// #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
//TODO check this!
}