/* 
 * File:   PclPointCloud.h
 * Author: carlos
 *
 * Created on 26 de octubre de 2012, 15:45
 */

#ifndef PCLPOINTCLOUD_H
#define	PCLPOINTCLOUD_H

#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/mutex.hpp>
#include <vector>

using namespace std;

class PclPointCloud {
public:
    
    enum Cloud_ID {original, cond_removal, shadow_pts, sor, triangulation, difNormals, mls, voxel_grid, convolution, nss, ror, tmp};
    enum Filter_ID {shapeContext1980, ESFSignature640 };
    static const int NUM_CLOUDS = 20;
    
    PclPointCloud();
    PclPointCloud(const PclPointCloud& orig);
    virtual ~PclPointCloud();
    
    int setCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int cloudId);
    int getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, int cloudId);
    
    int rotTransCloud(int idCloudIn, int idCloudOut, double pitch, double roll, double yaw, double x, double y, double z);
    
    int startPclVisualizerThread(string windowName);
    int stopPclVisualizerThread(string windowName);
    int readCloudFromFile(string fileName, int cloudId);
    int readCloudNormalFromFile(string fileName, int cloudId);
    int readCloudFromFile(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud);
    int writeCloudToFile(string fileName, int cloudId);
    int writeCloudNormalToFile(string fileName, int cloudId);
    int writeCloudToFile(string fileName, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud);
    int writeCloudToFile(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud);
    int writeCloudToFileTxt(string fileName, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloud);

    int shapeContext3DEstimation(int cloudInId, pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloudOut /* = NULL */, double minRadius /* = 0.5 */, double maxRadius /* = 0.5 */);
    
    int shapeContext3DEstimation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudIn /* = NULL */, 
                pcl::PointCloud<pcl::Normal>::Ptr cloudNormal /* = NULL */, 
                pcl::PointCloud<pcl::ShapeContext1980>::Ptr cloudOut /* = NULL */, double minRadius /* = 0.5 */, double maxRadius /* = 0.5 */);
    
    int calcCentroid(int idCloudIn, pcl::PointIndices indicesIn, Eigen::Vector4f *centroid);
    
    int conditionalRemoval(double xmin, double xmax, double ymin, double ymax, double zmin, double zmax, int inCloudId /* = 0 */, pcl::PointIndices indIn, int outCloudId /* = 0 */);
    int conditionalRemoval(double xmin, double xmax, double ymin, double ymax, double zmin, double zmax, int inCloudId /* = 0 */, pcl::PointIndices indIn, pcl::PointIndices *indOut);
    int shadowPoints(double threshold, bool calcNormals, double radius, int k, int inCloudId /* = 0 */);
    int statOutlierRemoval(double mulThres, int k, int inCloudId /* = 0 */);
    int fastTriangulation(int maxNearest, double mu, double radius, int minAngle, int maxAngle, int maxAngleSurface, bool consistency, int inCloudId /* = 0 */);
    int movingLeastSquares(double fitRadius, int polyOrder, double upRadius, int numPtsInRadius, double voxelSize, int inCloudId /* = 0 */);
    int voxelGrid(double xSize, double ySize, double zSize, int inCloudId /* = 0 */);
    int radiusOutlierRemoval(double radius, int minNeighbors, int inCloudId /* = 0 */);
    int principalCurvatures(string inPcdFile, string outTxtFile, string logFile, double radius /* = 0.25 */, double radNormal /* = 0.25 */);   
    int principalCurvatures(string inPcdFile, string outTxtFile, string logFile, int kNN);   
    int principalCurvatures(int inPcdId, string inPcdFile, string outTxtFile, string logFile, double radius /* = 0.25 */, double radNormal /* = 0.25 */);   
    int principalCurvatures(int inPcdId, double radNormal /* = 0.25 */);   
    int principalCurvatures(int inPcdId, string inPcdFile, string outTxtFile, string logFile, int kNN);   
    
    int floodFill_curvCloud(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr dataCloud, int outCloudId, int startNode, double targetCurv, double lowThres, double highThres, int replaceR, int replaceG, int replaceB, double radius);
    int floodFill_rgbCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr dataCloud, int outCloudId, int startNode, int targetR, int targetG, int targetB, int lowThres, int highThres, int replaceR, int replaceG, int replaceB, double radius);
    void showCurvatures(int cloudId, int opc, double limit);
    int readColorScale(string colorFile);
    int getCurvaturesCloud(pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud);
    
    //int floodFill()
    int addPlaneToView(pcl::ModelCoefficients coeff, string name);
    int removePlaneFromView(string name);
    int removeAllObjects();
    int showPointCloud(int idCloud);
    int computeNormals(int idCloud, int kSearch);
    int computeNormals(int idCloud, double radius);    
    int computeRegionNormals(int idCloud, double radius, float x, float y, float z);
    
    double distPtPlaneSigned(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff);
    double distPtPlane(pcl::PointXYZRGB pt, pcl::ModelCoefficients coeff);
    int modePolygonArea(int idCloud, int idCloudPolygon, pcl::ModelCoefficients coeff, float beanSize, float* value, int* n);
    int modePolygonAreaEfficient(int idCloud, vector<int> ptInlierIndex, pcl::ModelCoefficients coeff, float beanSize, float* value, int* n);
    void removeObstacles(int idCloud, int idCloudOut, double minX, double maxX, double minY, double maxY, pcl::PointIndices *indObst, bool color);
    pcl::PointXYZRGB getPoint(int idCloud, int idxPoint);
    void setPointColor(int idCloud, int idxPoint, int r, int g, int b);
    
    void getPlaneDegreeInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z);
    void getPlaneRadiansInclination(pcl::ModelCoefficients plane, double* x, double* y, double* z);
    
    int detectOnePlane(int cloudIdIn, pcl::PointIndices indices, pcl::PointIndices *inliers, double threshold, pcl::ModelCoefficients *coeff);
    
    int removeIndices(int idCloud, pcl::PointIndices indicesRem, pcl::PointIndices* indicesOut, int idCloudOut);
    
    int getPointCount(int idCloud){
        return m_cloud[idCloud]->points.size();
    }

    void setInputDir(string inputDir) {
        this->m_inputDir = inputDir;
    }

    string getInputDir() const {
        return m_inputDir;
    }

    
    
private:
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr m_curvaturesCloud;
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr m_xyzrgbnormalCloud;
    
    
    int readColorScale(string colorFile, vector<int>* r, vector<int>* g, vector<int>* b);
    vector<int> m_red;
    vector<int> m_green;
    vector<int> m_blue;
    
    //point cloud directory
    string m_inputDir;
    
    //point clouds
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr m_cloud[NUM_CLOUDS];
    pcl::PointCloud<pcl::Normal>::Ptr m_cloudNormal[NUM_CLOUDS];
    
    //surfaces
    pcl::PolygonMesh m_triangles;
    
    //3D visualizer variables
    boost::thread m_visualizerTh;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> m_viewer;
    boost::mutex m_mutexVisualizer;
    bool m_stopVisualizer;
    
    //3D visualizer functions
    void initPclVisualizer(string windowName);
    boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud, string windowName);
};

#endif	/* PCLPOINTCLOUD_H */