/* 
 * File:   geometry.h
 * Author: jorge
 *
 * Created on 19 de noviembre de 2013, 17:37
 */

#ifndef GEOMETRY_H
#define	GEOMETRY_H

#include <Eigen/Core>

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

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

#include <pcl/filters/extract_indices.h>
#include <pcl/filters/conditional_removal.h>

class Ellipse
{
    public:
        
        Ellipse(double a,double b,double theta,double phi,pcl::PointXYZRGB center);
        
        void createPoints(uint n_points);
        
        double a_;
        double b_;
        pcl::PointXYZRGB center_;
        Eigen::Vector3f u_;
        Eigen::Vector3f v_;
        
        Eigen::Vector3f a_0_;
        Eigen::Vector3f a_pi_2_;
        Eigen::Vector3f a_pi_;
        Eigen::Vector3f a_pi_32_;
        double theta_;
        double phi_;
        
        Eigen::MatrixXf points_;
        
        typedef boost::shared_ptr<Ellipse> Ptr;
        
    private:
        Eigen::Matrix3f rotPhi_;
        Eigen::Matrix3f rotTheta_;
};

pcl::PointXYZRGB makePoint(double x,double y,double z);
pcl::PointXYZRGB makePoint(Eigen::Vector3f v);
double distancePointToLineSegment(pcl::PointXYZRGB _a,pcl::PointXYZRGB _b,pcl::PointXYZRGB _p);
double distancePointToEllipse(Ellipse::Ptr ellipse,pcl::PointXYZRGB p);

pcl::PointXYZRGB operator*(double mult,pcl::PointXYZRGB p);
pcl::PointXYZRGB operator*(pcl::PointXYZRGB p,double mult);
pcl::PointXYZRGB operator/(pcl::PointXYZRGB p,double div);
pcl::PointXYZRGB operator+(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2);
pcl::PointXYZRGB operator-(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2);
pcl::PointXYZRGB normalize(pcl::PointXYZRGB p);
double norm(pcl::PointXYZRGB p);

bool isValid(pcl::PointXYZRGB p);

Eigen::Matrix3f rotationMatrixByAxis(double angle,Eigen::Vector3f axis);
Eigen::Vector3f rotateByAxis(double angle,Eigen::Vector3f axis,Eigen::Vector3f vector);

void conditionalFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,std::vector<double>&limits);
void conditionalFilter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& out_cloud,std::vector<double>&limits);
bool compareIndices(pcl::PointIndices a, pcl::PointIndices b);
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);
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);
double random01(boost::mt19937 & engine);

pcl::PointXYZRGB directionFromAxisAndRotation(pcl::PointXYZRGB start_direction,pcl::PointXYZRGB rotation_axis,double angle);

double angleBetween2Vectors(pcl::PointXYZRGB p1,pcl::PointXYZRGB p2);
double angleFromDirection(pcl::PointXYZRGB dir);

#endif	/* GEOMETRY_H */