/**
* @file LocalFeatureSift.cpp
* @brief local feature with sift
* @author Erik Rodner, Alexander Freytag
* @date 03/10/2012
*/
#include <iostream>

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


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

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

LocalFeatureSift::LocalFeatureSift() : LocalFeature () 
{
  this->octaves          = 6;
  this->levels           = 3;
  this->first_octave     = -1;
  this->normalizeFeature = true;
  this->magnif           = 3;
  this->deletemode       = true;
  this->integerValues    = true;
  this->usegpu           = false;
}

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

LocalFeatureSift::~LocalFeatureSift()
{
}

void OBJREC::LocalFeatureSift::initFromConfig( const NICE::Config* _conf, const std::string& _confSection )
{
  //NOTE previous confSection defaultet to LFSiftPP!
  this->octaves          = _conf->gI(_confSection, "octaves",           6);
  this->levels           = _conf->gI(_confSection, "levels",            3);
  this->first_octave     = _conf->gI(_confSection, "first_octave",      -1);
  this->normalizeFeature = _conf->gB(_confSection, "normalize_feature", true );
  this->magnif           = _conf->gD(_confSection, "magnif",            3 );
  this->deletemode       = _conf->gB(_confSection, "deletemode",        true );
  this->integerValues    = _conf->gB(_confSection, "integer_values",    true );

#ifdef NICE_USELIB_CUDASIFT
  this->usegpu           = _conf->gB(_confSection, "use_siftgpu",       false );
#else
  this->usegpu = false;
#endif
}

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

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

    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;
    for ( iter = scales.begin(); iter != scales.end(); ++iter )
    {
        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
{
    if ( usegpu ) {
        try {
#ifdef NICE_USELIB_CUDASIFT
            withGPU( img, positions, descriptors );
#endif
        }
        catch ( runtime_error& rte ) {
            cerr << "[err] LocalFeatureSift: " << rte.what() << endl;
            clog << "[log] use SIFTPP implementation:" << endl;
            withPP( img, positions, descriptors );
        }
    }
    else {
        withPP( img, positions, descriptors );
    }
}

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
{
	/* TODO: switch to NICE instead of 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));*/
}

void LocalFeatureSift::withPP( 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) ;
    //calling sift.process(.) already done in VL::Sift constructor, so omit the following process function
    //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;
}

#ifdef NICE_USELIB_CUDASIFT
void LocalFeatureSift::withGPU(const NICE::Image& img, VVector& positions, VVector& descriptors) const
{
    // fill the parameter for
    char* argv[] = {
        // First octave to detect DOG keypoints
        "-fo" , const_cast<char*> (StringTools::convertToString<int> (first_octave).c_str()),
        // Maximum number of Octaves
        "-no" , const_cast<char*> (StringTools::convertToString<int> (octaves).c_str()),
        // Number of DOG levels in an octave.
        "-d"  , const_cast<char*> (StringTools::convertToString<int> (levels).c_str()),
        // Write unnormalized descriptor if specified.
        const_cast<char*> (normalizeFeature ? "" : "-unn"),
        // Descriptor grid size factor (magnif ??)
        "-dw" , const_cast<char*> (StringTools::convertToString<float> (magnif).c_str()),
        // verbose levels
        "-v", "0"
    };
    int argc = sizeof (argv) / sizeof (char*);

    // sift Instanz
    SiftGPU sift;

    // give parameter to sift
    sift.ParseParam (argc, argv);

    // check, whether siftgpu is full supported
    int support = sift.CreateContextGL();
    if( support != SiftGPU::SIFTGPU_FULL_SUPPORTED )
      throw runtime_error( "SiftGPU-support is not given by your device.");

    // set keypoints
    const int numberOfKeypoints = positions.size();
    SiftGPU::SiftKeypoint keys[numberOfKeypoints];

    // copy the NICEKeypoints into SiftKeypoints
    for (int i = 0; i < numberOfKeypoints; i++) {
        keys[i].x = positions[i][0];
        keys[i].y = positions[i][1];
        keys[i].s = positions[i][2];
        keys[i].o = positions[i][3];
    }
    sift.SetKeypointList (numberOfKeypoints, keys);

    // run SIFT
    const int imageWidth  = img.width();
    const int imageHeight = img.height();
    const unsigned char* rawImageData = img.getPixelPointer();

    sift.RunSIFT (imageWidth, imageHeight, rawImageData, GL_LUMINANCE, GL_UNSIGNED_BYTE);

    // get descriptors
    const int descr_size = 128;
    const int numberOfDescriptors = sift.GetFeatureNum();
    float desc[descr_size * numberOfDescriptors];

    sift.GetFeatureVector (NULL, desc);

    Vector localDesc (descr_size);
    // copy the SiftDescriptors into NICEDescriptors
    for (int i = 0; i < numberOfDescriptors; i++) {
        for (int j = 0; j < descr_size; j++) {
            localDesc[j] = (integerValues ? (int) (512 * desc[i*descr_size+j]) : desc[i*descr_size+j]);
        }
        descriptors.push_back (localDesc);
    }
}
#endif
  
///////////////////// INTERFACE PERSISTENT /////////////////////
// interface specific methods for store and restore
///////////////////// INTERFACE PERSISTENT ///////////////////// 

void LocalFeatureSift::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, "LocalFeatureSift" ) )
    {
      std::cerr << " WARNING - attempt to restore LocalFeatureSift, 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, "LocalFeatureSift" ) )
      {
        b_endOfBlock = true;
        continue;
      }      
      
      tmp = this->removeStartTag ( tmp );    
      
      if ( tmp.compare("octaves") == 0 )
      {
        is >> this->octaves;
        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("first_octave") == 0 )
      {
        is >> this->first_octave;
        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 if ( tmp.compare("magnif") == 0 )
      {
        is >> this->magnif;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("deletemode") == 0 )
      {
        is >> this->deletemode;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }      
      else if ( tmp.compare("integerValues") == 0 )
      {
        is >> this->integerValues;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }
      else if ( tmp.compare("usegpu") == 0 )
      {
        is >> this->usegpu;
        is >> tmp; // end of block 
        tmp = this->removeEndTag ( tmp );
      }      
      else
      {
      std::cerr << "WARNING -- unexpected LocalFeatureSift object -- " << tmp << " -- for restoration... aborting" << std::endl;
      throw;
      }
    }
  }
  else
  {
    std::cerr << "LocalFeatureSift::restore -- InStream not initialized - restoring not possible!" << std::endl;
    throw;
  }
}

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

    os << this->createStartTag( "levels" ) << std::endl;
    os << this->levels << std::endl;
    os << this->createEndTag( "levels" ) << 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( "normalizeFeature" ) << std::endl;
    os << this->normalizeFeature << std::endl;
    os << this->createEndTag( "normalizeFeature" ) << std::endl; 
    
    os << this->createStartTag( "magnif" ) << std::endl;
    os << this->magnif << std::endl;
    os << this->createEndTag( "magnif" ) << std::endl;
    
    os << this->createStartTag( "deletemode" ) << std::endl;
    os << this->deletemode << std::endl;
    os << this->createEndTag( "deletemode" ) << std::endl;  

    os << this->createStartTag( "integerValues" ) << std::endl;
    os << this->integerValues << std::endl;
    os << this->createEndTag( "integerValues" ) << std::endl;     
    
    os << this->createStartTag( "usegpu" ) << std::endl;
    os << this->usegpu << std::endl;
    os << this->createEndTag( "usegpu" ) << std::endl;     
    
    // done
    os << this->createEndTag( "LocalFeatureSift" ) << std::endl;    
  }
  else
  {
    std::cerr << "OutStream not initialized - storing not possible!" << std::endl;
  }
}

void LocalFeatureSift::clear ()
{
}