/**
* @file LocalFeatureSift.cpp
* @brief local feature with sift
* @author Erik Rodner
* @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;

LocalFeatureSift::LocalFeatureSift( const Config *conf )
{
    this->conf = 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 );

#ifdef NICE_USELIB_CUDASIFT
    usegpu 	     	 = conf->gB("LFSiftPP", "use_siftgpu",		false );
#else
	usegpu = false;
#endif
}

LocalFeatureSift::~LocalFeatureSift()
{
}

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) ;

    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