/* 
 * File:   sampling.h
 * Author: jorge
 *
 * Created on 20 de noviembre de 2013, 12:58
 */

#ifndef SAMPLING_H
#define	SAMPLING_H

#include <vector>
#include <iterator>

#include "geometry.h"

#include <Eigen/Core>

#include <pcl/point_types.h>
#include <pcl/common/distances.h>

#include <boost/shared_ptr.hpp>

#include <boost/random/uniform_real.hpp>
#include <boost/random/mersenne_twister.hpp>

//#include <gsl/gsl_multimin.h>
#include <ccgsl/multimin.hpp>

class Sampling
{
    public:
        
        typedef boost::shared_ptr<Sampling> Ptr;
        
        enum
        {
            ELLIPSE_SAMPLING,OFF_CENTER_ELLIPSE_SAMPLING,ROTATION_ONLY_SAMPLING
        };
    
        virtual void createSamples()
        {
            std::cerr<<"Sample type not specified"<<std::endl;
        }
        
        
        virtual void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr&)
        {
            std::cerr<<"Sample type not specified"<<std::endl;
        }
        
        virtual double scoreDistance(double distance)
        {
            //Rayleigh pdf
            //double b = 0.03;
            double x = distance;
            //double f = (x/(b*b))*exp((-x*x)/(2*b*b));
            
            //Weibull
            double k=weibull_k_;
            double y=weibull_delta_;
            
            double f = (k/y)*(pow(x/y,k-1))*exp(-pow(x/y,k));
            
            return std::min(f,100.0);
            //return std::min(1./distance,50.0);
        }
        
        virtual double individualScore(pcl::PointXYZRGB& l1,pcl::PointXYZRGB& l2,pcl::PointXYZRGB& p,double& distance)
        {
            distance = distancePointToLineSegment(l1,l2,p);
            return scoreDistance(distance);
        }
        
        int getBestSampleIndex()
        {
            return std::distance(scores_.begin(),std::max_element(scores_.begin(),scores_.end()));
        }
        
        void setWeibullParameters(double k,double delta)
        {
            weibull_k_ = k;
            weibull_delta_ = delta;
        }
        
        Eigen::MatrixXd distances_;
        std::vector<double> scores_;
        uint number_samples_;
        
        pcl::PointXYZRGB start_point_;
        pcl::PointXYZRGB preferencial_end_;
        pcl::PointXYZRGB preferencial_direction_;
        
        double weibull_delta_;
        double weibull_k_;
        double max_theta_;
        double max_phi_;
};
       
//Not used right now
class EllipseSampling: public Sampling
{
    public:
        EllipseSampling()
        {
            number_samples_ = 50;
            weibull_delta_ = 0.05;
            weibull_k_ = 1;
        }
        
        std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> > samples_;

        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
};

//Used in torso
class DistortedSphereSampling: public Sampling
{
    public:
        DistortedSphereSampling()
        {
            number_samples_ = 50;
            weibull_delta_ = 0.25;
            weibull_k_ = 2;
            compression_factor_= 3;
        }
        
        double compression_factor_;
        
        //Eigen::Vector3f perpendicular;
        
        std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> > samples_;

        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
        
        double scoreDistance(double distance)
        {
            //Rayleigh pdf
            //double b = 0.03;
            double x = distance;
            //double f = (x/(b*b))*exp((-x*x)/(2*b*b));
            
            //Weibull
            double k=weibull_k_;
            double y=weibull_delta_;
            
            double f1 = (k/y)*(pow(x/y,k-1))*exp(-pow(x/y,k));
            
            //Weibull
            k=10.;
            y=0.35;
            
            double f2 = (k/y)*(pow(x/y,k-1))*exp(-pow(x/y,k));
            
            double f=f1+f2*0.2;
            
            return std::min(f,100.0);
        }
};

//Not used
class EllipseOffCenterScoreSampling: public EllipseSampling
{
    public:
        EllipseOffCenterScoreSampling()
        {
        }
        
        double scoreDistance(double distance)
        {
            //Rayleigh pdf
            double b = 0.3;
            double x = distance;
            double f = (x/(b*b))*exp((-x*x)/(2*b*b));
            
            return f;
        }
};

//Used in the feet
class OffCenterEllipseSampling: public Sampling
{
    public:
        OffCenterEllipseSampling()
        {
            number_samples_ = 50;
            weibull_delta_ = 0.05;
            weibull_k_ = 2;
            period_ = 0.4;
        }
        
        std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> > samples_;

        double off_center_;
        
        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
        
        double period_;
        
        double scoreDistance(double distance)
        {
            if(distance>period_*(3./4.))
                return 0;
            else
                return std::min((1./distance)*cos(distance*(2.*M_PI/period_)),50.);
        }
};

//Used in hips
class RotationOnlySampling: public Sampling
{
    public:
        RotationOnlySampling()
        {
            number_samples_ = 10;
            weibull_delta_ = 0.05;
            weibull_k_ = 2;
        }
        
        Eigen::MatrixXf samples_;
        std::vector<double> angles_;

        double length_;
        double center_angle_;
        
        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
};

//Used in the legs
class CosineBiasedSampling: public Sampling
{
    public:
        CosineBiasedSampling()
        {
            number_samples_ = 50;
            weibull_delta_ = 0.05;
            weibull_k_ = 2;
            negative_deviation_=0;
        }
        
        std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> > samples_;
        
        double positive_max_angle_;
        double negative_max_angle_;
        double negative_deviation_;
        
        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
};

//Used in the arms
class CosineBiasedSamplingModified: public CosineBiasedSampling
{
    public:
        CosineBiasedSamplingModified()
        {
            number_samples_ = 50;
            negative_deviation_=0;
            period_ = 0.3;
        }
        
        double period_;
        
        double scoreDistance(double distance)
        {
            if(distance>period_*(3./4.))
                return 0;
            else
                return std::min((1./distance)*cos(distance*(2.*M_PI/period_)),50.);
        }
};

class OffCenterEllipseSamplingModified: public OffCenterEllipseSampling
{
    public:
        OffCenterEllipseSamplingModified()
        {
            period_ = 0.3;
        }
        
        double period_;
        
        double scoreDistance(double distance)
        {
            if(distance>period_*(3./4.))
                return 0;
            else
                return std::min((1./distance)*cos(distance*(2.*M_PI/period_)),50.);
        }
};

//Used in shoulders
class EllipseShapeSampling: public Sampling
{
    public:
        EllipseShapeSampling()
        {
            number_samples_ = 10;
            weibull_delta_ = 0.05;
            weibull_k_ = 2;
        }
        
        std::vector<Ellipse::Ptr> samples_;
        std::vector<double> angles_;
        
        double center_angle_;
        
        double a_;
        double b_;
        
        void createSamples();
        void scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud);
        double individualScore(Ellipse::Ptr& ellipse,pcl::PointXYZRGB p,double& distance);
};

using namespace std;
class SemiFixedSegmentMinimization
{
    public:
        
        double weibull_k_;
        double weibull_delta_;
        double threshold_;
        uint iterations_max_;
        uint iteration_;
        
        Eigen::Vector3f main_direction;
        Eigen::Vector3f fixed_point;
        Eigen::Vector3f preferencial_end;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
        pcl::PointXYZRGB end_point;

        std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> > trajectory;
        
        gsl_multimin_fminimizer *s;
        
        SemiFixedSegmentMinimization()
        {
            weibull_k_ = 2;
            weibull_delta_ = 0.5;
            threshold_ = 50;
            iterations_max_ = 200;
        }
        
        double scoreDistance(double distance)
        {
            double x = distance;

            //Weibull pdf
            double f = (weibull_k_/weibull_delta_)*(pow(x/weibull_delta_,weibull_k_-1))*exp(-pow(x/weibull_delta_,weibull_k_));

            return std::min(f,threshold_);
        }

        double individualScore(pcl::PointXYZRGB l1,pcl::PointXYZRGB l2,pcl::PointXYZRGB p,double& distance)
        {
            distance = distancePointToLineSegment(l1,l2,p);
            //return -scoreDistance(distance);
            
            return std::min(distance,0.15);
        }
        
        pcl::PointXYZRGB createSample(double theta, double phi)
        {
            Eigen::Vector3f vec = preferencial_end - fixed_point;
            double r = vec.norm();
            vec.normalize();

            Eigen::Vector3f perpendicular = main_direction.cross(vec);

            Eigen::Matrix3f rotMain;
            Eigen::Matrix3f rotPerpendicular;

            rotPerpendicular = rotationMatrixByAxis(theta,perpendicular);
            rotMain = rotationMatrixByAxis(phi,main_direction);

            Eigen::Vector3f vecr = rotMain*rotPerpendicular*vec;
            Eigen::Vector3f pf = fixed_point + vecr*r;
            
            return makePoint(pf);
        }
        
        
        double costFunction(gsl::vector const& v)
        {
            //Create sample from parameters
            double theta = v[0];
            double phi = v[1];

            pcl::PointXYZRGB pf = createSample(theta,phi);

            double score = 0;
            
            for(uint d=0;d<cloud->points.size();d++)
            {
                double distance;
                score += individualScore(makePoint(fixed_point),pf,cloud->at(d),distance);
                //distances_(i,d)=distance;
            }
            return score;
        }
        
        void minimize()
        {
            gsl::exception::enable();
            
            
            try
            {
                gsl::multimin::function function(*this,&SemiFixedSegmentMinimization::costFunction,size_t(2));

                gsl::vector x(2);
                x[0] = 0;
                x[1] = 0;

                gsl::vector step_size(2);
                step_size[0] = 0.1;
                step_size[1] = 0.1;

                gsl::multimin::fminimizer s(gsl::multimin::fminimizer::nmsimplex2(), 2);

                gsl::multimin::fminimizer::set(s, &function, x, step_size );
                cout << "Using " << s.name() << " to find minimum "<<endl;
                
                trajectory.clear();
                
                iteration_ = 0;
                for( double size = 1000; gsl::multimin::test::size( size, 1e-5 ) != gsl::exception::GSL_SUCCESS;s.iterate() )
                {
                    iteration_++;
                    
                    if(iteration_>iterations_max_)
                    {
                        cout<<"Maximum number of iterations reach"<<endl;
                        break;
                    }
                    
                    gsl::vector result = s.x();
                    std::cout << s.minimum() << " at (" << result[0] << "," << result[1] <<") [size = " << s.size() << "]." << std::endl;
                    size = s.size();
                    
                    pcl::PointXYZRGB p = createSample(result[0],result[1]);
                    trajectory.push_back(p);
                }
                
                gsl::vector result = s.x();
                cout << s.minimum() << " at (" << result[0] << "," << result[1] <<") [size = " << s.size() << "]." <<endl;
                
                end_point = createSample(result[0],result[1]);
                //end_point = createSample(M_PI/6.,0);
            }
            catch( exception& e )
            {
                cout << "exception caught at " << __FILE__ << "." << __LINE__ <<endl;
            }
        }
};

void excludeSamplesByProximityToLineSegment(pcl::PointXYZRGB& l1,pcl::PointXYZRGB& l2,std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> >& samples,double threshold);

#endif	/* SAMPLING_H */