//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
#include "bbcreaVtkCreateMeshFromPoints.h"
#include "bbcreaVtkPackage.h"

#include "vtkTriangleStrip.h"
#include <vtkMath.h>

namespace bbcreaVtk
{

BBTK_ADD_BLACK_BOX_TO_PACKAGE(creaVtk,CreateMeshFromPoints)
BBTK_BLACK_BOX_IMPLEMENTATION(CreateMeshFromPoints,bbtk::AtomicBlackBox);
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void CreateMeshFromPoints::Process()
{

// THE MAIN PROCESSING METHOD BODY
//   Here we simply set the input 'In' value to the output 'Out'
//   And print out the output value
// INPUT/OUTPUT ACCESSORS ARE OF THE FORM :
//    void bbSet{Input|Output}NAME(const TYPE&)
//    const TYPE& bbGet{Input|Output}NAME() const 
//    Where :
//    * NAME is the name of the input/output
//      (the one provided in the attribute 'name' of the tag 'input')
//    * TYPE is the C++ type of the input/output
//      (the one provided in the attribute 'type' of the tag 'input')

//    bbSetOutputOut( bbGetInputIn() );
//    std::cout << "Output value = " <<bbGetOutputOut() << std::endl;

		std::vector<double> lstX		= bbGetInputLstX();
		std::vector<double> lstY		= bbGetInputLstY();
		std::vector<double> lstZ		= bbGetInputLstZ();
		std::vector<int> lstIndexs		= bbGetInputLstIndexs();
		if ( (lstIndexs.size()<=1) || (lstX.size()==0) || (lstX.size()!=lstY.size()) || (lstY.size()!=lstZ.size()) )
		{
			printf("PG CreateMeshFromPoints::Process: List of points X Y Z  and LstIndexes is not correct\n");
			bbSetOutputOut(NULL);
		} else  {
			int ii,sizeSegment1,sizeSegment2;
			int endSegment;
//			vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
			if (points!=NULL) points->Delete();
			points = vtkPoints::New();
			int i,sizeLstX 	= 	lstX.size();
			for (i=0;i<sizeLstX;i++)
			{
				points->InsertNextPoint(lstX[i],lstY[i],lstZ[i]);
			} // for i
//			vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
			if (cells!=NULL) cells->Delete();
			cells = vtkCellArray::New();
			int maxElements;
			int maxSegment1,maxSegment2;
			int iSeg1,iSeg2;
			int iGeneral	=	0;
			int	sizeLstIdexes=lstIndexs.size();
			for (i=0; i<sizeLstIdexes-1; i++ )
			{
				sizeSegment1 = lstIndexs[i];
				sizeSegment2 = lstIndexs[i+1];
				vtkSmartPointer<vtkTriangleStrip> triangleStrip = vtkSmartPointer<vtkTriangleStrip>::New();
				triangleStrip->GetPointIds()->SetNumberOfIds(sizeSegment1+sizeSegment2);
				maxElements=sizeSegment1;
				if (maxElements<sizeSegment2) maxElements=sizeSegment2;
				maxSegment1	= iGeneral+sizeSegment1;
				maxSegment2	= iGeneral+sizeSegment1+sizeSegment2;
				iSeg1		= iGeneral;
				iSeg2		= iGeneral+sizeSegment1;
				for (ii=0; ii<maxElements; ii++)
				{
					triangleStrip->GetPointIds()->SetId(ii*2  ,iSeg1);
					triangleStrip->GetPointIds()->SetId(ii*2+1,iSeg2);
					iSeg1++;
					iSeg2++;
                    if (iSeg1>=maxSegment1) { iSeg1=maxSegment1-1; }
                    if (iSeg2>=maxSegment2) { iSeg2=maxSegment2-1; }
				} // for ii 
				iGeneral=iGeneral+sizeSegment1;
				cells->InsertNextCell(triangleStrip);
			} //for  LstIndexs

			
			if(bbGetInputCloseSurface()){			
				//false = Open Contour
				//true = Closed Contour
				if(bbGetInputOpenClose()){			
					CloseContourSides(lstIndexs, true);
				}
				else{
					CloseOpenContourSurface(lstIndexs);
				}
			}
			
			
//			vtkPolyData *polydata = vtkPolyData::New();
			if (polydata!=NULL) polydata->Delete();
			polydata = vtkPolyData::New();
			polydata->SetPoints(points);
			polydata->SetStrips(cells);
//			vtkCleanPolyData *clean=vtkCleanPolyData::New();
			if (clean!=NULL) clean->Delete();
			clean = vtkCleanPolyData::New();
			clean->SetInputData(polydata);
			clean->Update();
//			vtkTriangleFilter *triangle = vtkTriangleFilter::New();
			if (triangle!=NULL) triangle->Delete();
			triangle = vtkTriangleFilter::New();
			triangle->SetInputData( clean->GetOutput() );
			triangle->Update();
			bbSetOutputOut( clean->GetOutput() );
		}// if listXYZ size
		//printf("PG CreateMeshFromPoints::Process: End\n");
}

/**
* Closes the sides of the contour
* iterates in one way or the other, depending on the order of the points and calculated vectors.
* uPointOrder: Points are order in a U shape
* lstIndexs: number of points on each spline
*/
void CreateMeshFromPoints::CloseContourSides(std::vector<int> lstIndexs, bool uPointOrder){
	int	sizeLstIdexes = lstIndexs.size();
	int sizePoints = bbGetInputLstX().size();

	int firstIndex, end, centroidId, numPointsFace, contraryId;
	int increment = uPointOrder?1:sizeLstIdexes;
	double centroid[3];
	
	for(int facesIdx = 0; facesIdx < 2; facesIdx++){
		std::fill(std::begin(centroid), std::end(centroid), 0);
		if(facesIdx == 0){
			firstIndex = 0;
			numPointsFace = uPointOrder?lstIndexs[0]: sizeLstIdexes;
			end = uPointOrder?firstIndex + numPointsFace:sizePoints - lstIndexs[sizeLstIdexes - 1] + 1;
			contraryId = sizePoints-1;
		}
		else{
			firstIndex = uPointOrder?sizePoints - lstIndexs[sizeLstIdexes-1]:lstIndexs[0]-1;
			numPointsFace = uPointOrder?lstIndexs[sizeLstIdexes-1]:sizeLstIdexes;
			end = uPointOrder?firstIndex + numPointsFace:sizePoints;
			contraryId = 0;
		}
		if(numPointsFace > 1){
			double lastPoint[3] = {};
			bool validCentroid = CalcValidCentroid(centroid, firstIndex, end, increment, numPointsFace);
			if(validCentroid){
				bool normalOrder = isPointingCorrectly(firstIndex, firstIndex+increment, centroid, contraryId);
				centroidId = points->InsertNextPoint(centroid[0], centroid[1], centroid[2]);
				vtkSmartPointer<vtkTriangleStrip> triangleStrip1 = vtkSmartPointer<vtkTriangleStrip>::New();
				triangleStrip1->GetPointIds()->SetNumberOfIds(numPointsFace*2+1);
				if(normalOrder){	//(facesIdx == 0 && uPointOrder) || (facesIdx == 1 && !uPointOrder)){
					int initial = firstIndex;
					int triangleIndex = 0;
					for(int index = initial; index < end; index+=increment){
						triangleStrip1->GetPointIds()->SetId(triangleIndex,index);
						if(index+increment >= end){
							triangleStrip1->GetPointIds()->SetId(triangleIndex+1,initial);
							triangleStrip1->GetPointIds()->SetId(triangleIndex+2,centroidId);
						}
						else{
							triangleStrip1->GetPointIds()->SetId(triangleIndex+1,centroidId);
						}
						triangleIndex+=2;
					}
					cells->InsertNextCell(triangleStrip1);
				}
				else{
					int initial = firstIndex-1;
					int triangleIndex = 0;
					int triangleStripStart = end-1;
					for(int index = triangleStripStart; index > initial; index-=increment){
						triangleStrip1->GetPointIds()->SetId(triangleIndex,index);
						if(index-increment <= initial){
							triangleStrip1->GetPointIds()->SetId(triangleIndex+1,triangleStripStart);
							triangleStrip1->GetPointIds()->SetId(triangleIndex+2,centroidId);
						}
						else{
							triangleStrip1->GetPointIds()->SetId(triangleIndex+1,centroidId);
						}
						triangleIndex+=2;
					}
					cells->InsertNextCell(triangleStrip1);
				}
			}//if validCentroid
		}//if numPointsFace
	}//for facesIdx

}

/**
* Checks if the normal from firstPointId, secPointId and centroid points away 
* from the vector centroid to contrPointId.
* Used to check that the order used to create the new polygons is correct.
*/
bool CreateMeshFromPoints::isPointingCorrectly( int firstPointId, int secPointId, double(&centroid)[3], int contrPointId) {

	double firstPoint[3], secPoint[3], contrPoint[3];
	points->GetPoint(firstPointId, firstPoint);
	points->GetPoint(secPointId, secPoint);
	
	double firstVect[3], secVect[3], normal[3], contrVect[3];
	
	vtkMath::Subtract(firstPoint, centroid, firstVect);
	vtkMath::Subtract(secPoint, centroid, secVect);
	
	points->GetPoint(contrPointId, contrPoint);
	vtkMath::Subtract(contrPoint, centroid, contrVect);
	
	vtkMath::Cross(firstVect, secVect, normal);
	double dotCalc;
	dotCalc = vtkMath::Dot(normal, contrVect);
	
	return dotCalc<0;
}

/**
* Checks if points on each side of the shapes represent a curve.
*/
bool CreateMeshFromPoints::CheckLinePointOrder(){
	int sizePoints = bbGetInputLstX().size();
	std::vector<int> lstIndexs = bbGetInputLstIndexs();
	double point1[3], point2[3], point3[3];
	double center[3];
	double firstRadiusSum = 0;
	double secondRadiusSum = 0;
	for(int i = 0; i < lstIndexs[0] && lstIndexs[0] > 3; i+=3){
		if(i+3 <= lstIndexs[0]){
			points->GetPoint(i, point1);
			points->GetPoint(i+1, point2);
			points->GetPoint(i+2, point3);
			firstRadiusSum += vtkMath::Solve3PointCircle(point1, point2, point3, center);
		}
	}
	
	for(int i = 0; i < sizePoints && lstIndexs.size() > 3; i+=(lstIndexs[0]*3)){
		if(i+(3*lstIndexs[0]) <= sizePoints){
			points->GetPoint(i, point1);
			points->GetPoint(i+lstIndexs[0], point2);
			points->GetPoint(i+(2*lstIndexs[0]), point3);
			secondRadiusSum += vtkMath::Solve3PointCircle(point1, point2, point3, center);
		}
	}
	
	return firstRadiusSum > secondRadiusSum;
}

/**
* Closes an open contour
* lstIndexs: number of points on each spline
*/
void CreateMeshFromPoints::CloseOpenContourSurface(std::vector<int> lstIndexs){
	int	sizeLstIdexes = lstIndexs.size();
	int sizeLstX = bbGetInputLstX().size();
	bool linePointOrder = CheckLinePointOrder();
	CloseContourSides(lstIndexs, !linePointOrder);
	CloseContourBottom(!linePointOrder);
}

/**
* Calculates centroid.
* centroid: array to store calculation
* start: start index of points to use
* end: end index of points to use
* increment: increment to be used in point iteration
* numPoints: number of points used to calculate the centroid.
* Returns a bool indicating the validity of the centroid calculated.
* False = invalid centroid = all points are the same.
*/
bool CreateMeshFromPoints::CalcValidCentroid(double(&centroid)[3], int start, int end, int increment, int numPoints){
	double currPoint[3] = {}, prevPoint[3] = {};
	bool samePoint = true;
	for(int i = start; i < end; i+=increment){
		points->GetPoint(i, currPoint);
		if(samePoint && (currPoint[0] != prevPoint[0] || currPoint[1] != prevPoint[1] || currPoint[2] != prevPoint[2])){
			samePoint = false;
		}
		centroid[0] += currPoint[0];
		centroid[1] += currPoint[1];
		centroid[2] += currPoint[2];
		std::copy(std::begin(currPoint), std::end(currPoint), prevPoint);
	}
	centroid[0] /= numPoints;
	centroid[1] /= numPoints;
	centroid[2] /= numPoints;
	
	return !samePoint;
}

/**
* Closes the bottom of the given countour.
* Should only be used when its an open contour.
* uPointOrder: points are ordered in U shape
*/
void CreateMeshFromPoints::CloseContourBottom(bool uPointOrder){
	std::vector<int> lstIndexs = bbGetInputLstIndexs();
	int sizeLstIdexes = lstIndexs.size();
	int sizeLstX = bbGetInputLstX().size();
	
	vtkSmartPointer<vtkTriangleStrip> triangleStripBottom = vtkSmartPointer<vtkTriangleStrip>::New();
	triangleStripBottom->GetPointIds()->SetNumberOfIds(sizeLstIdexes*2);
	int triangleIndex = 0, currentId = 0, nextId = 0;
	for(int splineIndex = 0; splineIndex < sizeLstIdexes;splineIndex++){
		triangleStripBottom->GetPointIds()->SetId(triangleIndex, currentId);
		nextId = uPointOrder?currentId + lstIndexs[splineIndex] - 1:sizeLstX - sizeLstIdexes + splineIndex;
		triangleStripBottom->GetPointIds()->SetId(triangleIndex+1, nextId);
		triangleIndex+=2;
		currentId = uPointOrder?nextId + 1: splineIndex+1;
	}
	cells->InsertNextCell(triangleStripBottom);
}

//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void CreateMeshFromPoints::bbUserSetDefaultValues()
{

//  SET HERE THE DEFAULT INPUT/OUTPUT VALUES OF YOUR BOX 
//    Here we initialize the input 'In' to 0
//   bbSetInputIn(0);
	bbSetInputCloseSurface(false);
	bbSetInputOpenClose(false);
  	points		= NULL;
	cells		= NULL;
	polydata	= NULL;
	clean		= NULL;
	triangle	= NULL;
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void CreateMeshFromPoints::bbUserInitializeProcessing()
{

//  THE INITIALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should allocate the internal/output pointers 
//    if any 

  
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void CreateMeshFromPoints::bbUserFinalizeProcessing()
{

//  THE FINALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should desallocate the internal/output pointers 
//    if any
  
}
}
// EO namespace bbcreaVtk