#include <geometry.h>

Ellipse::Ellipse(double a,double b,double theta,double phi,pcl::PointXYZRGB center)
{
    a_=a;
    b_=b;
    center_=center;
    theta_=theta;
    phi_=phi;

    createPoints(20);
}

void Ellipse::createPoints(uint n_points)
{
    Eigen::Vector3f vertical;
    vertical << 0,1,0;

    //Main axis and secondary axis direction
    u_<<1,0,0;
    v_<<0,0,-1;

    //Rotation matrixes allong primary axis and vertical direction, i don't understand way i must negate the angles
    rotPhi_ = rotationMatrixByAxis(-phi_,u_);
    rotTheta_ = rotationMatrixByAxis(-theta_,vertical);

    //Rotate direction vectors
    u_ = rotTheta_*rotPhi_*u_;
    v_ = rotTheta_*rotPhi_*v_;

    points_.resize(n_points,3);
    double dt = (2.0*M_PI)/(double)n_points;        

    for(uint i=0;i<n_points;i++)
    {
        double t=i*dt;

        Eigen::Vector3f p;
        p(0) = a_*cos(t);
        p(1) = 0;
        p(2) = -b_*sin(t);

        p=rotTheta_*rotPhi_*p;

        //Translate to center
        p+=center_.getVector3fMap();

        points_.row(i)=p;
    }
    
    a_0_ = points_.row(0);
    a_pi_2_ = points_.row(floor(n_points/4.));
    a_pi_ = points_.row(floor(n_points/2.));
    a_pi_32_ = points_.row(floor(n_points*3/4));
}
        
bool isValid(pcl::PointXYZRGB p)
{   
    if(isnan(p.x) || isnan(p.y) || isnan(p.z))
        return false;
    else
        return true;
}

pcl::PointXYZRGB makePoint(double x,double y,double z)
{
    pcl::PointXYZRGB p;
    p.x=x;
    p.y=y;
    p.z=z;
    return p;
}

pcl::PointXYZRGB makePoint(Eigen::Vector3f v)
{
    pcl::PointXYZRGB p;
    p.x=v(0);
    p.y=v(1);
    p.z=v(2);
    return p;
}

double distancePointToLineSegment(pcl::PointXYZRGB _a,pcl::PointXYZRGB _b,pcl::PointXYZRGB _p)
{
    Eigen::Vector3f a = _a.getVector3fMap();
    Eigen::Vector3f b = _b.getVector3fMap();
    Eigen::Vector3f p = _p.getVector3fMap();
    
    double pa_distance = pcl::squaredEuclideanDistance(_a,_p);
    double pb_distance = pcl::squaredEuclideanDistance(_b,_p);
    
    //Get line direction vector
    Eigen::Vector3f ab = a - b;
    ab.normalize();
    
    double distance;
    
    if(pa_distance<pb_distance)
    {
        Eigen::Vector3f pa = p-a;
        pa.normalize();
        
        double d = pa.dot(-ab);
        
        if(d>0)
            distance = sqrt(pa_distance*(1-d*d));
        else
            distance = sqrt(pa_distance);
    }else
    {
        Eigen::Vector3f pb = p-b;
        pb.normalize();
        
        double d = pb.dot(ab);
        
        if(d>0)
            distance = sqrt(pb_distance*(1-d*d));
        else
            distance = sqrt(pb_distance);
    }
    
    return distance;
}

double distancePointToEllipse(Ellipse::Ptr ellipse,pcl::PointXYZRGB _p)
{
    double a = ellipse->a_;
    double b = ellipse->b_;
    
    Eigen::Vector3f c = ellipse->center_.getVector3fMap();
    Eigen::Vector3f p = _p.getVector3fMap();
    
    Eigen::Vector3f u = ellipse->u_;
    u.normalize();
    
    Eigen::Vector3f v = ellipse->v_;
    v.normalize();
    
    //rotation matrix to put ellipse in standard form    
    Eigen::Matrix3f R;
    
    R.col(0)=u;
    R.col(1)=v;
    R.col(2)<<u.cross(v);
    
    //part of a companion matrix for a quartic
    Eigen::MatrixXf comp0(3,4);
    comp0.block<3,3>(0,0) = Eigen::MatrixXf::Identity(3,3);
    comp0.col(3) << 0,0,0;
   
    double min_dist = 0;

    //find optimal point on the ellipse
    //transform current point
    Eigen::Vector3f s = R.inverse()*(p-c);
    
    //The constants A,B and C follow from the condition dQ/dt = 0, with Q = Q(s,E,t) the XY-distance between point s and ellipse E
    double A = a*s(0);
    double B = b*s(1);
    double C = b*b - a*a;
    
    double t_hat;
    
    //we have to find [t_hat], the true anomaly on the ellipse that minimizes
    //the distance between the associated point on the ellipse [E] and the
    //point [s]. The solution depends on the value of [C]. 
    //If C = 0, the solution is easy
    //otherwise, we have to solve a quartic equation in A,B,C, which is
    //done most quickly by using EIG() on its companion matrix:
    
    if(C==0)
    {
        t_hat=atan2(B,A);
    }else
    {
        //associated companion matrix
        Eigen::Matrix4f comp;
        comp<<-2*A/C, -(A*A+B*B-C*C)/C/C,+2*A/C,(A/C)*(A/C),
                comp0;
        
        //solve this quartic (real values only)
        Eigen::VectorXcf roots = comp.eigenvalues();    
        Eigen::VectorXf roots_real(roots.size());
        roots_real = roots.real();
        
        //extract optimal point
        Eigen::VectorXf sint1(roots_real.size());
        sint1 = 1 - roots_real.array().square();
        Eigen::VectorXf sint2(roots_real.size());
        sint2 = -sint1;
        
        Eigen::MatrixXf sints(sint1.size(),2);
        sints << sint1,
                sint2;
        
        Eigen::MatrixXf costs(roots_real.size(),2);
        costs<< roots_real,
                roots_real;
        
        Eigen::MatrixXf selld1(roots_real.size(),2);
        Eigen::MatrixXf selld2(roots_real.size(),2);
        
        double s0=s(0);
        selld1 = Eigen::MatrixXf::Ones(roots_real.size(),2)*s0;
        
        double s1=s(1);
        selld2 = Eigen::MatrixXf::Ones(roots_real.size(),2)*s1;
        
        selld1 -= a*costs;
        selld2 -= b*sints;
        
        Eigen::MatrixXf selld(selld1.rows(),2);
        selld = selld1.array().square() + selld2.array().square();
        
        Eigen::VectorXf selldl(selld1.rows()*2);
        selldl << selld.col(0),
                  selld.col(1);
        
        int tind;
        selldl.minCoeff(&tind);
        
        double sinth = sints(tind);
        double costh = costs(tind);
        
        t_hat = atan2(sinth,costh);
    }
    
    //compute distance
    min_dist = sqrt( (s(0)-a*cos(t_hat))*(s(0)-a*cos(t_hat)) + (s(1)-b*sin(t_hat))*(s(1)-b*sin(t_hat)) + s(2)*s(2) );
    
    return min_dist;
}

pcl::PointXYZRGB operator*(pcl::PointXYZRGB p,double mult)
{
    pcl::PointXYZRGB pout = p;
    pout.x*=mult;
    pout.y*=mult;
    pout.z*=mult;
    return pout;
}

pcl::PointXYZRGB operator*(double mult,pcl::PointXYZRGB p)
{
    pcl::PointXYZRGB pout = p;
    pout.x*=mult;
    pout.y*=mult;
    pout.z*=mult;
    return pout;
}

pcl::PointXYZRGB operator/(pcl::PointXYZRGB p,double div)
{
    pcl::PointXYZRGB pout = p;
    pout.x/=div;
    pout.y/=div;
    pout.z/=div;
    return pout;
}

pcl::PointXYZRGB operator+(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2)
{
    pcl::PointXYZRGB pout;
    
    pout.x=p1.x+p2.x;
    pout.y=p1.y+p2.y;
    pout.z=p1.z+p2.z;
    return pout;
}

pcl::PointXYZRGB operator-(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2)
{
    pcl::PointXYZRGB pout;
    
    pout.x=p1.x-p2.x;
    pout.y=p1.y-p2.y;
    pout.z=p1.z-p2.z;
    return pout;
}

pcl::PointXYZRGB normalize(pcl::PointXYZRGB p)
{
    double n = norm(p);
    
    p.x/=n;
    p.y/=n;
    p.z/=n;
    
    return p;
}

double norm(pcl::PointXYZRGB p)
{
    return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}

Eigen::Matrix3f rotationMatrixByAxis(double angle,Eigen::Vector3f axis)
{
    axis.normalize();

    Eigen::Matrix3f S;
    S  <<   0,         axis(2),    -axis(1),
            -axis(2),  0,          axis(0) ,
            axis(1),   -axis(0),   0;
    
    Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
    R += sin(angle)*S + (1-cos(angle))*(S*S);
    return R;
}

Eigen::Vector3f rotateByAxis(double angle,Eigen::Vector3f axis,Eigen::Vector3f vector)
{
    Eigen::Matrix3f rot_matrix = rotationMatrixByAxis(angle,axis);
        
    Eigen::Vector3f out_vector = rot_matrix*vector;
    return out_vector;
}

void conditionalFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,std::vector<double>&limits)
{
    conditionalFilter(cloud,cloud,limits);    
}

void conditionalFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& out_cloud,std::vector<double>&limits)
{
    pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZRGB> ());
    
    if(limits.size()!=6)
    {
        std::cout<<"Incorrect limits size in ConditionalFilter"<<std::endl;
        return;
    }
    
    double xmin = limits[0];
    double xmax = limits[1];
    double ymin = limits[2];
    double ymax = limits[3];
    double zmin = limits[4];
    double zmax = limits[5];
    
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::GT, zmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("z", pcl::ComparisonOps::LT, zmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::GT, xmin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("x", pcl::ComparisonOps::LT, xmax)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::GT, ymin)));
    range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB> ("y", pcl::ComparisonOps::LT, ymax)));

    // build the filter
    pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem(range_cond);
    condrem.setInputCloud(cloud);

    //pcl::PointIndices::Ptr inliers (new pcl::PointIndices);

    condrem.setKeepOrganized (false);
    condrem.filter(*out_cloud);
}

bool compareIndices(pcl::PointIndices a, pcl::PointIndices b) 
{
    return a.indices.size() < b.indices.size();
}

void removeExplainedPointsExcludingPointNeighborhood(pcl::PointXYZRGB& conditioning_point, Eigen::MatrixXd& distances,int min_index,double threshold,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& input_cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& reduced_cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& removed_cloud)
{
    //Remove points from the point cloud that are explained by the previous sample
    pcl::PointIndices::Ptr explained_indices(new pcl::PointIndices);

    if(distances.cols() != input_cloud->size())
    {
        std::cerr<<"Error! distance matrix does not match input point cloud"<<std::endl;
        return;
    }
    for(int i=0;i<distances.cols();i++)
    {
        //Remove this point from the cloud
        if(distances(min_index,i)<threshold)
        {
            if(pcl::euclideanDistance(input_cloud->at(i),conditioning_point)>threshold)
                explained_indices->indices.push_back(i);
        }
    }

    pcl::ExtractIndices<pcl::PointXYZRGB> indicesExtraction;
    
    indicesExtraction.setInputCloud (input_cloud);
    indicesExtraction.setIndices (explained_indices);
    
    //Extract the removed points
    indicesExtraction.setNegative(false);
    indicesExtraction.filter(*removed_cloud);
    
    indicesExtraction.setInputCloud (input_cloud);
    indicesExtraction.setIndices (explained_indices);
    
    //Extract the remaing points
    indicesExtraction.setNegative(true);
    indicesExtraction.filter(*reduced_cloud);
}

void removeExplainedPoints(Eigen::MatrixXd& distances,int min_index,double threshold,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& input_cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& reduced_cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& removed_cloud)
{
    //Remove points from the point cloud that are explained by the previous sample
    pcl::PointIndices::Ptr explained_indices(new pcl::PointIndices);

    if(distances.cols() != input_cloud->size())
    {
        std::cerr<<"Error! distance matrix does not match input point cloud"<<std::endl;
        return;
    }
    
    for(int i=0;i<distances.cols();i++)
    {
        //Remove this point from the cloud
        if(distances(min_index,i)<threshold)
        {
            explained_indices->indices.push_back(i);
        }
    }
    
    pcl::ExtractIndices<pcl::PointXYZRGB> indicesExtraction;
    
    indicesExtraction.setInputCloud (input_cloud);
    indicesExtraction.setIndices (explained_indices);

    //Extract the removed points
    indicesExtraction.setNegative(false);
    indicesExtraction.filter(*removed_cloud);
    
    //Extract the remaing points
    indicesExtraction.setNegative(true);
    indicesExtraction.filter(*reduced_cloud);
}

double random01(boost::mt19937 & engine)
{
    boost::uniform_real<double> u01; // same as u01(0.0, 1.0); see Note below
    return u01(engine);
}

pcl::PointXYZRGB directionFromAxisAndRotation(pcl::PointXYZRGB start_direction,pcl::PointXYZRGB rotation_axis,double angle)
{
    Eigen::Vector3f sd = start_direction.getVector3fMap();
    Eigen::Vector3f rot_axis = rotation_axis.getVector3fMap();
    
    Eigen::Matrix3f rotation_matrix = rotationMatrixByAxis(angle,rot_axis);
    
    Eigen::Vector3f result = rotation_matrix*sd;
    result.normalize();
    
    return makePoint(result);
}

double angleBetween2Vectors(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2)
{
    Eigen::Vector3f a = p1.getVector3fMap();
    Eigen::Vector3f b = p2.getVector3fMap();
    a.normalize();
    b.normalize();
    
    //double r= a.dot(b);
    //return acos(r);
    
    Eigen::Vector3f cr= a.cross(b);
    double norm_cr = cr.norm();
    
    double angle = atan2(norm_cr,a.dot(b));
    return angle;
}

double angleFromDirection(pcl::PointXYZRGB dir)
{
    return atan2(dir.x,-dir.z);
}