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

// STL includes
#include <iostream>
#include <sstream>

// nice-core includes
#include <core/basics/Exception.h>

// #ifdef NICE_USELIB_ICE
//   #include <image_nonvis.h>
// #endif

// nice-vislearning includes
#include "vislearning/features/localfeatures/LFSiftPP.h"

//TODO move this to separate input or source folder
#include "vislearning/features/localfeatures/sift.h"


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

///////////////////// ///////////////////// /////////////////////
//                   CONSTRUCTORS / DESTRUCTORS
///////////////////// ///////////////////// /////////////////

LFSiftPP::LFSiftPP() : LocalFeatureRepresentation () 
{
    this->threshold        = 0.0;
    this->edgeThreshold    = 10.0;
    this->octaves          = 6;
    this->first_octave     = -1;
    this->levels           = 3;
    
    this->minScale         = 1;
    this->maxScale         = 4;
    this->numScales        = 10;
    this->numAngles        = 6;
    
    this->descriptorAlignment = DALGIN_DETECTOR;
    this->normalizeFeature = false;
}

LFSiftPP::LFSiftPP( const Config * _conf )
{
  this->initFromConfig( _conf );
}

LFSiftPP::~LFSiftPP()
{
}

void LFSiftPP::initFromConfig(const NICE::Config * _conf, const std::string & _confSection)
{
    this->threshold      = _conf->gD(_confSection, "threshold", 0.0);
    this->edgeThreshold  = _conf->gD(_confSection, "edge_threshold", 10.0);
    this->octaves        = _conf->gI(_confSection, "octaves", 6);
    this->first_octave   = _conf->gI(_confSection, "first_octave", -1);
    this->levels         = _conf->gI(_confSection, "levels", 3);
    
    this->minScale       = _conf->gD(_confSection, "min_scale", 1);
    this->maxScale       = _conf->gD(_confSection, "max_scale", 4);
    this->numScales      = _conf->gI(_confSection, "num_scales", 10);
    this->numAngles      = _conf->gI(_confSection, "num_angles", 6 );

    std::string descriptorAlignment_s = _conf->gS(_confSection, "descriptor_alignment", "detector" );

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

    this->normalizeFeature = _conf->gB(_confSection, "normalize_feature", false );

    //TODO check whether this os is still needed here.
    std::ostringstream os;
    os << "siftpp_" << "l" << levels << "_o" 
       << octaves << "_m" << minScale << "_t" 
       << threshold << "_e" << edgeThreshold;

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

///////////////////// ///////////////////// /////////////////////
//                      FEATURE STUFF
///////////////////// ///////////////////// /////////////////

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 ( NICE::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 ( NICE::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(Exception, "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!
}


///////////////////// INTERFACE PERSISTENT /////////////////////
// interface specific methods for store and restore
///////////////////// INTERFACE PERSISTENT ///////////////////// 

void LFSiftPP::restore ( std::istream & is, int format )
{
  //delete everything we knew so far...
  this->clear();
  
  
  if ( is.good() )
  {
    
    std::string tmp;
    is >> tmp; //class name 
    
    if ( ! this->isStartTag( tmp, "LFSiftPP" ) )
    {
      std::cerr << " WARNING - attempt to restore LFSiftPP, but start flag " << tmp << " does not match! Aborting... " << std::endl;
      throw;
    }   
    
    bool b_endOfBlock ( false ) ;
    
    while ( !b_endOfBlock )
    {
      is >> tmp; // start of block 
      
      if ( this->isEndTag( tmp, "LFSiftPP" ) )
      {
        b_endOfBlock = true;
        continue;
      }      
      
      tmp = this->removeStartTag ( tmp );   

      
      if ( tmp.compare("threshold") == 0 )
      {
        is >> this->threshold;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("edgeThreshold") == 0 )
      {
        is >> this->edgeThreshold;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }      
      else if ( tmp.compare("octaves") == 0 )
      {
        is >> this->octaves;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("first_octave") == 0 )
      {
        is >> this->first_octave;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      } 
      else if ( tmp.compare("levels") == 0 )
      {
        is >> this->levels;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("minScale") == 0 )
      {
        is >> this->minScale;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("maxScale") == 0 )
      {
        is >> this->maxScale;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("numScales") == 0 )
      {
        is >> this->numScales;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("numAngles") == 0 )
      {
        is >> this->numAngles;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("descriptorAlignment") == 0 )
      {
        unsigned int ui_descriptorAlignment;        
        is >> ui_descriptorAlignment;        
        this->descriptorAlignment = static_cast<DESCRIPTORALIGNMENT> ( ui_descriptorAlignment ) ;
        
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      } 
      else if ( tmp.compare("normalizeFeature") == 0 )
      {
        is >> this->normalizeFeature;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }       
      else
      {
      std::cerr << "WARNING -- unexpected LFSiftPP object -- " << tmp << " -- for restoration... aborting" << std::endl;
      throw;
      }
    }
  }
  else
  {
    std::cerr << "LFSiftPP::restore -- InStream not initialized - restoring not possible!" << std::endl;
    throw;
  }
}

void LFSiftPP::store ( std::ostream & os, int format ) const
{ 
  if (os.good())
  {
    // show starting point
    os << this->createStartTag( "LFSiftPP" ) << std::endl; 
    
    os << this->createStartTag( "threshold" ) << std::endl;
    os << this->threshold << std::endl;
    os << this->createEndTag( "threshold" ) << std::endl;  

    os << this->createStartTag( "edgeThreshold" ) << std::endl;
    os << this->edgeThreshold << std::endl;
    os << this->createEndTag( "edgeThreshold" ) << std::endl;
    
    os << this->createStartTag( "octaves" ) << std::endl;
    os << this->octaves << std::endl;
    os << this->createEndTag( "octaves" ) << std::endl;  

    os << this->createStartTag( "first_octave" ) << std::endl;
    os << this->first_octave << std::endl;
    os << this->createEndTag( "first_octave" ) << std::endl; 
    
    os << this->createStartTag( "levels" ) << std::endl;
    os << this->levels << std::endl;
    os << this->createEndTag( "levels" ) << std::endl;    
    
    os << this->createStartTag( "minScale" ) << std::endl;
    os << this->minScale << std::endl;
    os << this->createEndTag( "minScale" ) << std::endl;  

    os << this->createStartTag( "maxScale" ) << std::endl;
    os << this->maxScale << std::endl;
    os << this->createEndTag( "maxScale" ) << std::endl;
    
    os << this->createStartTag( "numScales" ) << std::endl;
    os << this->numScales << std::endl;
    os << this->createEndTag( "numScales" ) << std::endl;  

    os << this->createStartTag( "numAngles" ) << std::endl;
    os << this->numAngles << std::endl;
    os << this->createEndTag( "numAngles" ) << std::endl; 
    
    os << this->createStartTag( "descriptorAlignment" ) << std::endl;
    os << this->descriptorAlignment << std::endl;
    os << this->createEndTag( "descriptorAlignment" ) << std::endl;
    
    os << this->createStartTag( "normalizeFeature" ) << std::endl;
    os << this->normalizeFeature << std::endl;
    os << this->createEndTag( "normalizeFeature" ) << std::endl; 
    
    
    // done
    os << this->createEndTag( "LFSiftPP" ) << std::endl;    
  }
  else
  {
    std::cerr << "OutStream not initialized - storing not possible!" << std::endl;
  }
}

void LFSiftPP::clear ()
{
}