#include "sampling.h"

void CosineBiasedSampling::createSamples()
{
    boost::mt19937 generator(time(0));

    pcl::PointXYZRGB psample;    
    samples_.resize(number_samples_,psample);

    Eigen::Vector3f main_direction = preferencial_direction_.getVector3fMap();

    Eigen::Vector3f vec = preferencial_end_.getVector3fMap() - start_point_.getVector3fMap();
    double r = vec.norm();
    vec.normalize();

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

    Eigen::Matrix3f rotMain;
    Eigen::Matrix3f rotPerpendicular;
    Eigen::Matrix3f rotThird = rotationMatrixByAxis(negative_deviation_,main_direction);

    double f0 = (M_PI/2.)/max_theta_;

    //Create samples around each best point
    for(uint i=0;i<number_samples_;i++)
    {
        double theta = ((random01(generator)-0.5)*2)*max_theta_;
        
        double min_limit = (cos(theta*f0)-1)*negative_max_angle_;
        double max_limit = (cos(theta*f0)-1)*negative_max_angle_+positive_max_angle_;

        double rand_value = random01(generator);
        double phi = rand_value*min_limit + (1-rand_value)*max_limit;

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

        Eigen::Vector3f vecr = rotThird*rotMain*rotPerpendicular*vec;

        Eigen::Vector3f pf = start_point_.getVector3fMap() + vecr*r;

        psample.x=pf[0];
        psample.y=pf[1];
        psample.z=pf[2];

        samples_[i]=psample;
    }

    return;
}

void CosineBiasedSampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.size(),0);

    //Create a matrix to store all distances
    distances_.resize(samples_.size(),cloud->points.size());
    distances_.setZero(samples_.size(),cloud->points.size());

    for(uint i=0;i<samples_.size();i++)
    {
        pcl::PointXYZRGB psample = samples_[i];
        
        for(uint d=0;d<cloud->points.size();d++)
        {
            double distance;
            scores_[i]+= individualScore(start_point_,psample,cloud->at(d),distance);
            distances_(i,d)=distance;
        }
    }
}

void EllipseSampling::createSamples()
{
    boost::mt19937 generator(time(0));

    pcl::PointXYZRGB psample;    
    samples_.resize(number_samples_,psample);

    Eigen::Vector3f main_direction = preferencial_direction_.getVector3fMap();

    Eigen::Vector3f vec = preferencial_end_.getVector3fMap() - start_point_.getVector3fMap();
    double r = vec.norm();
    vec.normalize();

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

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

    double R=max_phi_;
    double a=1;
    double b=1;
    double x0 = sqrt(R*R/a);

    //Create samples around each best point
    for(uint i=0;i<number_samples_;i++)
    {
        double theta = ((random01(generator)-0.5)*2)*max_theta_;

        double theta_i = theta*x0/max_theta_;

        double phi = sqrt((R*R-a*theta_i*theta_i)/b);
        phi = ((random01(generator)-0.5)*2)*phi;

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

        Eigen::Vector3f vecr = rotMain*rotPerpendicular*vec;

        Eigen::Vector3f pf = start_point_.getVector3fMap() + vecr*r;

        psample.x=pf[0];
        psample.y=pf[1];
        psample.z=pf[2];

        samples_[i]=psample;
    }

    return;
}

void EllipseSampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.size(),0);

    //Create a matrix to store all distances
    distances_.resize(samples_.size(),cloud->points.size());
    distances_.setZero(samples_.size(),cloud->points.size());

    for(uint i=0;i<samples_.size();i++)
    {
        pcl::PointXYZRGB psample = samples_[i];
        
        for(uint d=0;d<cloud->points.size();d++)
        {
            double distance;
            scores_[i]+= individualScore(start_point_,psample,cloud->at(d),distance);
            distances_(i,d)=distance;
        }
    }
}

void OffCenterEllipseSampling::createSamples()
{
    boost::mt19937 generator(time(0));

    pcl::PointXYZRGB psample;    
    samples_.resize(number_samples_,psample);

    Eigen::Vector3f main_direction = preferencial_direction_.getVector3fMap();

    Eigen::Vector3f vec = preferencial_end_.getVector3fMap() - start_point_.getVector3fMap();
    double r = vec.norm();
    vec.normalize();

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

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

    double R=max_phi_;
    double a=1;
    double b=1;
    double x0 = sqrt(R*R/a);

    Eigen::Matrix3f rotExtraPerpendicular = rotationMatrixByAxis(off_center_,perpendicular);

    //Create samples around each best point
    for(uint i=0;i<number_samples_;i++)
    {
        double theta = ((random01(generator)-0.5)*2)*max_theta_;

        double theta_i = theta*x0/max_theta_;

        double phi = sqrt((R*R-a*theta_i*theta_i)/b);
        phi = ((random01(generator)-0.5)*2)*phi;

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

        Eigen::Vector3f vecr = rotExtraPerpendicular*rotMain*rotPerpendicular*vec;

        Eigen::Vector3f pf = start_point_.getVector3fMap() + vecr*r;

        psample.x=pf[0];
        psample.y=pf[1];
        psample.z=pf[2];

        samples_[i]=psample;
    }

    return;
}

void OffCenterEllipseSampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.size(),0);

    //Create a matrix to store all distances
    distances_.resize(samples_.size(),cloud->points.size());
    distances_.setZero(samples_.size(),cloud->points.size());

    for(uint i=0;i<samples_.size();i++)
    {
        pcl::PointXYZRGB psample = samples_[i];

        for(uint d=0;d<cloud->points.size();d++)
        {
            double distance;
            scores_[i]+= individualScore(start_point_,psample,cloud->at(d),distance);
            distances_(i,d)=distance;
        }
    }
}

void DistortedSphereSampling::createSamples()
{
    boost::mt19937 generator(time(0));

    pcl::PointXYZRGB psample;    
    samples_.resize(number_samples_,psample);

    Eigen::Vector3f main_direction = preferencial_direction_.getVector3fMap();

    Eigen::Vector3f vec = preferencial_end_.getVector3fMap() - start_point_.getVector3fMap();
    double r = vec.norm();
    vec.normalize();

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

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

    double R=max_phi_;
    double a=1;
    double b=1;
    double x0 = sqrt(R*R/a);

    //Create samples around each best point
    for(uint i=0;i<number_samples_;i++)
    {
        double theta = ((random01(generator)-0.5)*2)*max_theta_;

        double theta_i = theta*x0/max_theta_;

        double phi = sqrt((R*R-a*theta_i*theta_i)/b);
        phi = ((random01(generator)-0.5)*2)*phi;

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

        Eigen::Vector3f vecr = rotMain*rotPerpendicular*vec;

        Eigen::Vector3f pf = start_point_.getVector3fMap() + vecr*r;

        psample.x=pf[0];
        psample.y=pf[1];
        psample.z=pf[2];

        samples_[i]=psample;
    }

    return;
}

void DistortedSphereSampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.size(),0);

    //Create a matrix to store all distances
    distances_.resize(samples_.size(),cloud->points.size());
    distances_.setZero(samples_.size(),cloud->points.size());

    Eigen::Vector3f v1 = preferencial_direction_.getVector3fMap();
    v1.normalize();
    
    Eigen::Vector3f v2;
    
    v2 << 1,0,0;
    if(v1(2)==0)
        v2 << 0, 0, 1;
    else
        v2(2) = (-v1(0)*v2(0) - v2(0)*v2(1))/v1(2);

    v2.normalize();
    
    Eigen::Vector3f v3 = v2.cross(v1);
    v3.normalize();
    
    for(uint i=0;i<samples_.size();i++)
    {
        pcl::PointXYZRGB psample = samples_[i];
        
        //move circle to the samples position
        //for(uint i=0;i<=circle_resolution;i++)
            //circle_pos.row(i) = circle.row(i) + psample.getVector3fMap();
        
        for(uint d=0;d<cloud->points.size();d++)
        {
            //Eigen::Vector3f ps = cloud->at(d).getVector3fMap() - psample.getVector3fMap();
            
            //Project ps on preferencial direction
            //double pref_dir = ps.dot(preferencial_direction_.getVector3fMap());
            //Project into ortogonal to pref dir
            //double ort_dir = ps.dot(perpendicular);
            //double distance = sqrt(pref_dir*pref_dir*0.8 + ort_dir*ort_dir);
            
            //double distance = pcl::euclideanDistance(psample,cloud->at(d));
            
            Eigen::Vector3f pd = psample.getVector3fMap()-cloud->at(d).getVector3fMap();
            
            double dv1 = pd.dot(v1)*compression_factor_;
            double dv2 = pd.dot(v2);
            double dv3 = pd.dot(v3);

            double distance = sqrt(dv1*dv1 + dv2*dv2 + dv3*dv3);
            
            double x = distance;
            
            double k=5;
            double y=weibull_delta_;
            
            double f = (k/y)*(pow(x/y,k-1))*exp(-pow(x/y,k));
            
            scores_[i]+=f;
            distances_(i,d)=distance;
            
            //scores_[i]+= individualScore(start_point_,psample,cloud->at(d),/distance);
            //distances_(i,d)=distance;
        }
    }
}

void RotationOnlySampling::createSamples()
{
    boost::mt19937 generator(time(0));

    Eigen::MatrixXf simple_translation;
    simple_translation.resize(1,6);
    simple_translation<<start_point_.x,start_point_.y,start_point_.z,start_point_.x,start_point_.y,start_point_.z;

    Eigen::Vector3f vertical;
    vertical << 0, 1, 0;

    Eigen::MatrixXf start_points;
    start_points.resize(3,2);

    start_points << - length_/2, length_/2,
                             0,        0,
                             0,        0;

    Eigen::MatrixXf end_points;
    end_points.resize(3,2);

    samples_.resize(number_samples_,6);
    angles_.resize(number_samples_,0);

    double dt=2*max_theta_/number_samples_;
    
    Eigen::Matrix3f rot;
    for(uint i=0;i<number_samples_;i++)
    {
        //double theta = center_angle_ + ((random01(generator)-0.5)*2)*max_theta_;
        double theta = i*dt - max_theta_ + center_angle_;
        angles_[i]=theta;
        rot = rotationMatrixByAxis(theta,vertical);

        end_points = rot*start_points;

        samples_.row(i) << end_points.col(0).transpose(), end_points.col(1).transpose();
        samples_.row(i) += simple_translation;
    }
}

void RotationOnlySampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.rows(),0);
    //Create a matrix to store all distances
    distances_.resize(samples_.rows(),cloud->points.size());
    distances_.setZero(samples_.rows(),cloud->points.size());

    for(int i=0;i<samples_.rows();i++)
    {
        pcl::PointXYZRGB p1 = makePoint(samples_(i,0),samples_(i,1),samples_(i,2));
        pcl::PointXYZRGB p2 = makePoint(samples_(i,3),samples_(i,4),samples_(i,5));

        for(uint d=0;d<cloud->points.size();d++)
        {
            double distance;
            scores_[i]+= individualScore(p1,p2,cloud->at(d),distance);
            distances_(i,d)=distance;
        }
    }
}

void EllipseShapeSampling::createSamples()
{
    boost::mt19937 generator(time(0));

    //Eigen::MatrixXf simple_translation;
    //simple_translation.resize(1,6);
    //simple_translation<<start_point_.x,start_point_.y,start_point_.z,start_point_.x,start_point_.y,start_point_.z;

    Eigen::Vector3f vertical;
    vertical << 0, 1, 0;

    samples_.resize(number_samples_);
    angles_.resize(number_samples_,0);

    double dt=2*max_theta_/number_samples_;
    
    for(uint i=0;i<number_samples_;i++)
    {
        double theta = i*dt - max_theta_ + center_angle_;
        angles_[i]=theta;
        
        Ellipse::Ptr ellipse(new Ellipse(a_,b_,-theta,0,start_point_));
        samples_[i] = ellipse;
    }
}

void EllipseShapeSampling::scoreSamples(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud)
{
    //Score samples
    scores_.resize(samples_.size(),0);
    //Create a matrix to store all distances
    distances_.resize(samples_.size(),cloud->points.size());
    distances_.setZero(samples_.size(),cloud->points.size());

    for(uint i=0;i<samples_.size();i++)
    {
        for(uint d=0;d<cloud->points.size();d++)
        {
            double distance;
            scores_[i]+= individualScore(samples_[i],cloud->at(d),distance);
            distances_(i,d)=distance;
        }
    }
}

double EllipseShapeSampling::individualScore(Ellipse::Ptr& ellipse,pcl::PointXYZRGB p,double& distance)
{
    double distance_to_center = pcl::euclideanDistance(p,ellipse->center_);
  
    if(!isValid(p) || !isValid(ellipse->center_))
    {
        distance = 1.0e6;
        return scoreDistance(distance);
    }
    
    if(distance_to_center>2*2*std::max(ellipse->a_,ellipse->b_))
    {
        distance = distance_to_center;
        return scoreDistance(distance_to_center);
    }else
    {
        distance = distancePointToEllipse(ellipse,p);
        return scoreDistance(distance);
    }
}

void excludeSamplesByProximityToLineSegment(pcl::PointXYZRGB& l1,pcl::PointXYZRGB& l2,std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> >& samples,double threshold)
{
    std::vector<pcl::PointXYZRGB,Eigen::aligned_allocator<pcl::PointXYZRGB> >::iterator it_samples;
    for(it_samples=samples.begin();it_samples!=samples.end();)
    {
        pcl::PointXYZRGB p = *it_samples;
        if(distancePointToLineSegment(l1,l2,p)<threshold)
            it_samples = samples.erase(it_samples);
        else
            it_samples++;        
    }
}