/*
 * Add comments here
 *-----------------------------------------------------------------------------
 * Concave Hull SOP. This SOP creates a concave hull from a point cloud.
*/

#include "SOP_ConcaveHull.h"

#include <GU/GU_Detail.h>
#include <GEO/GEO_PrimPoly.h>
#include <OP/OP_Operator.h>
#include <OP/OP_AutoLockInputs.h>
#include <OP/OP_Operator.h>
#include <OP/OP_OperatorTable.h>
#include <CH/CH_LocalVariable.h>
#include <PRM/PRM_Include.h>
#include <UT/UT_DSOVersion.h>
#include <UT/UT_Interrupt.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/surface/concave_hull.h>

void newSopOperator(OP_OperatorTable *table)
{
    table->addOperator(new OP_Operator(
        "hdk_concavehull",
        "Concave Hull",
        SOP_ConcaveHull::myConstructor,
        SOP_ConcaveHull::myTemplateList,
        1,
        1,
        SOP_ConcaveHull::myVariables,
        OP_FLAG_GENERATOR));
}

static PRM_Name alphaName("alpha", "Alpha");

static PRM_Default	oneDefault(1);

PRM_Template SOP_ConcaveHull::myTemplateList[] = {
    PRM_Template(PRM_FLT, 1, &alphaName, &oneDefault),
    PRM_Template()
};

CH_LocalVariable SOP_ConcaveHull::myVariables[] = {
    { 0, 0, 0 },
};

OP_Node * SOP_ConcaveHull::myConstructor(OP_Network *net,
    const char *name, OP_Operator *op)
{
    return new SOP_ConcaveHull(net, name, op);
}

SOP_ConcaveHull::SOP_ConcaveHull(OP_Network *net,
    const char *name, OP_Operator *op) : SOP_Node(net, name, op)
{
}

SOP_ConcaveHull::~SOP_ConcaveHull() {}

OP_ERROR SOP_ConcaveHull::cookMySop(OP_Context &context)
{
    fpreal t = context.getTime();
    
    // Lock inputs before accessing geo
    OP_AutoLockInputs inputs(this);
    if (inputs.lock(context) >= UT_ERROR_ABORT)
        return error();
    
    //Parameters
    float a = ALPHA(t);
    
    //Check for critical error
    if (error() >= UT_ERROR_ABORT)
    {
        return error();
    }
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base ( 
        new pcl::PointCloud<pcl::PointXYZ>);
    
    //READ IN POINTS FROM GEO
    GA_Offset ptoff;
    GA_FOR_ALL_PTOFF(gdp, ptoff)
    {
        UT_Vector3 p = gdp->getPos3(ptoff);
        cloud_base->push_back( pcl::PointXYZ(p.x(), p.y(), p.z()) );
    }
    
    pcl::ConcaveHull<pcl::PointXYZ> chull = pcl::ConcaveHull<pcl::PointXYZ>();
    chull.setAlpha (a);
    chull.setDimension(3);
    chull.reconstruct (*cloud_base);
    
    gdp->clearAndDestroy();
    
    // Start the interrupt server
    UT_AutoInterrupt boss("Constructing Concave Hull");
    if (boss.wasInterrupted())
    {
        return error();
    }
    
    //BUILD CONCAVE HULL GEO
    int total_points = cloud_base->points.size();
    GEO_PrimPoly *poly = GEO_PrimPoly::build(gdp, total_points, 
        GU_POLY_CLOSED);
    for (int i = 0; i < total_points; i++)
    {
        if (boss.wasInterrupted())
            break;
        
        UT_Vector3 pos = UT_Vector3(cloud_base->points[i].x,
            cloud_base->points[i].y,
            cloud_base->points[i].z);
            
        GA_Offset ptoff = poly->getPointOffset(i);
        gdp->setPos3(ptoff, pos);
    }
    
    select(GA_GROUP_PRIMITIVE);
    
    return error();
}