#include "CutModelPolygon.H"


CutModelPolygon::CutModelPolygon() 
{
	_cutInsideOutside=0;
	_inImage=NULL;
	_transform = vtkTransform::New();
}

CutModelPolygon::~CutModelPolygon()
{
	if(_transform!=NULL)
	{
		_transform->Delete();
	}
	if(_points!=NULL)
	{
		_points->Delete();
	}
}

void CutModelPolygon::processOutImage(int cutInsideOutside)
{
	_cutInsideOutside=cutInsideOutside;

	int numPoints = _points->GetNumberOfPoints();

	double v1[3],v2[3];

	// Calculate Orthogonal Vectors using two vectors in the plane and its cross product
	calculateOrthogonalVectors(v1,v2);

	cout<<"PolyCutter::processOutImage"<<endl;
	cout<<"V1:"<<" X:"<<v1[0]<<" Y:"<<v1[1]<<" Z:"<<v1[2]<<endl;
	cout<<"V2:"<<" X:"<<v2[0]<<" Y:"<<v2[1]<<" Z:"<<v2[2]<<endl;
	cout<<"Direction:"<<" X:"<<_direction[0]<<" Y:"<<_direction[1]<<" Z:"<<_direction[2]<<endl;
	cout<<endl;

	// Update the inverse transform which it's applied to all the points of the contour
	// and the points of the input image.
	updateTransform(v1,v2,_direction);

	//Transform Points coordinates
	std::vector<double> vectorOutX;
	std::vector<double> vectorOutY;
	std::vector<double> vectorOutZ;

	transformContourPoints(&vectorOutX,&vectorOutY,&vectorOutZ);


	/// Printing Points
	int num = vectorOutX.size();
	for(int t=0;t<num;t++)
	{					
		cout<<"Final Point"<<t<<"-X:"<<vectorOutX[t]<<" Y:"<<vectorOutY[t]<<" Z:"<<vectorOutZ[t]<<endl;
	}

	cutInputImage(vectorOutX,vectorOutY,vectorOutZ);

}

//-------------------------------------------------------------------------
void CutModelPolygon::cutInputImage(std::vector<double> vectorOutX,std::vector<double> vectorOutY,std::vector<double> vectorOutZ)
{

	// Find the minimum value in Y axis
	int i;
	double minY=vectorOutY[0];
	for(i=1;i<vectorOutY.size();i++)
	{
		if(vectorOutY[i]<minY)
		{
			minY=vectorOutY[i];
		}
	}

	// All the contour points minus the minimum to translate to the positive octant
	/// FIX ME !!!
	for(i=0;i<vectorOutY.size();i++)
	{

		vectorOutY[i]=vectorOutY[i]-minY;
	}


	// Creates a poligonal contour model with the points
	manualBaseModel *model = InitializeContourModel(vectorOutX,vectorOutY,vectorOutZ);

	initializeOutputImage();

	int ext[6];
	_inImage->GetWholeExtent(ext);
	int dimX=ext[1]-ext[0]+1;
	int dimY=ext[3]-ext[2]+1;
	int dimZ=ext[5]-ext[4]+1;


	//CreaMaracasVisu Class which evaluates whether a point is inside the contour or not
	ContourExtractData *extract = new ContourExtractData(false);

	//Contour list just with the contour created with the points 
	std::vector<manualBaseModel*> list;
	list.push_back(model);

	//Y-value asigned arbitrary
	extract->SetSizeImageY(500);

	//Initialization
	extract->SetLstManualContourModel(list);
	extract->InitLstContoursLinesYPoints();

	cout<<"Processing..."<<endl;

	for (int i=0; i<dimX; i++)
	{
		if(i%10==0)
			cout<<"Print "<<i<<" of "<<dimX<<endl;

		for (int j=0; j<dimY; j++)
		{
			for (int k=0; k<dimZ; k++)
			{
				unsigned short *pImage=(unsigned short*)_inImage->GetScalarPointer(i,j,k);
				//unsigned short *pResult=(unsigned short*)_outImage->GetScalarPointer(i,j,k);
				double in[4],out[4];
				in[0]=i;
				in[1]=j;
				in[2]=k;
				in[3]=1;

				//Transform every point in the imageData to the space resultant of the mathematical transform
				_transform->MultiplyPoint(in,out);

				//All the imageData minus the minimum to translate to the positive octant
				out[1]=out[1]- minY;

				//Verify if the point is inside the contour or not
				if ((out[1]>=0 && out[1]<500) && (extract->isInside(out[0],out[1],0)==true))
				{

					if(_cutInsideOutside==0)//CutInside
					{
						*pImage=0;
					}

				} 
				else
				{
					if(_cutInsideOutside==1)//CutOutside
					{
						*pImage=0;
					}
				}

			} // for k
		} // for j
	}// for i

	_inImage->Modified();
	_inImage->Update();

}

//-------------------------------------------------------------------------
void CutModelPolygon::transformContourPoints(std::vector<double> *vectorOutX,std::vector<double> *vectorOutY,std::vector<double> *vectorOutZ)
{
	int numPoints = _points->GetNumberOfPoints();

	for(int t=0;t<numPoints;t++)
	{
		double point[3],in[4],out[4];
		_points->GetPoint(t,point);
		in[0]=point[0];
		in[1]=point[1];
		in[2]=point[2];
		in[3]=1;

		//CHG_LYON_JAN29 Multiply Contour Points by the inverse of the spacing. It's necessary to faire 
		//the hole in the correct position and not in other with a displacement

		double _spc[3];
		_inImage->GetSpacing(_spc);
		in[0]=in[0]*(1/_spc[0]);
		in[1]=in[1]*(1/_spc[1]);
		in[2]=in[2]*(1/_spc[2]);

		_transform->MultiplyPoint(in,out);
		vectorOutX->push_back(out[0]);
		vectorOutY->push_back(out[1]);
		vectorOutZ->push_back(out[2]);
	}

}
//-------------------------------------------------------------------------
void CutModelPolygon::updateTransform(double* v1, double* v2, double* v3)
{

	// ImageData Centroid
	int i, numPoints=_points->GetNumberOfPoints();
	double centerX=0,centerY=0,centerZ=0;
	for(i =0; i<numPoints;i++)
	{
		double point[3];
		_points->GetPoint(i,point);
		centerX+=point[0];
		centerY+=point[1];
		centerZ+=point[2];
	}

	centerX=centerX/numPoints;
	centerY=centerY/numPoints;
	centerZ=centerZ/numPoints;


	vtkMatrix4x4 *matrix = vtkMatrix4x4::New();

	//First Vector...
	matrix->SetElement(0,0,v1[0]);
	matrix->SetElement(1,0,v1[1]);
	matrix->SetElement(2,0,v1[2]);
	matrix->SetElement(3,0,0);


	//Second Vector...
	matrix->SetElement(0,1,v2[0]);
	matrix->SetElement(1,1,v2[1]);
	matrix->SetElement(2,1,v2[2]);
	matrix->SetElement(3,1,0);

	//Third Vector...
	matrix->SetElement(0,2,v3[0]);
	matrix->SetElement(1,2,v3[1]);
	matrix->SetElement(2,2,v3[2]);
	matrix->SetElement(3,2,0);

	//Fourth Vector...
	matrix->SetElement(0,3,centerX);
	matrix->SetElement(1,3,centerY);
	matrix->SetElement(2,3,centerZ);
	matrix->SetElement(3,3,1);

	_transform->SetMatrix(matrix);
	_transform->Update();

	_transform->Inverse();
	_transform->Update();
	matrix->Delete();
}
//-------------------------------------------------------------------------
void CutModelPolygon::calculateOrthogonalVectors(double* v1, double* v2)
{
	double p1[3],p2[3];
	_points->GetPoint(0,p1);
	_points->GetPoint(1,p2);


	v1[0]=p1[0]-p2[0];
	v1[1]=p1[1]-p2[1];
	v1[2]=p1[2]-p2[2];


	double magV1 = sqrt( v1[0]*v1[0] + v1[1]*v1[1] + v1[2]*v1[2]);
	v1[0]=v1[0]/magV1;
	v1[1]=v1[1]/magV1;
	v1[2]=v1[2]/magV1;

	v2[0] = v1[1]*_direction[2] - v1[2]*_direction[1];
	v2[1] = -(v1[0]*_direction[2] - v1[2]*_direction[0]);
	v2[2] = v1[0]*_direction[1] - v1[1]*_direction[0];

	double magV2 = sqrt( v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]);
	v2[0]=v2[0]/magV2;
	v2[1]=v2[1]/magV2;
	v2[2]=v2[2]/magV2;

	double magDir = sqrt( _direction[0]*_direction[0] + _direction[1]*_direction[1] + _direction[2]*_direction[2]);
	_direction[0]=_direction[0]/magDir;
	_direction[1]=_direction[1]/magDir;
	_direction[2]=_direction[2]/magDir;
}

//-------------------------------------------------------------------------
void CutModelPolygon::initializeOutputImage()
{
	//int ext[6];
	//_inImage->GetWholeExtent(ext);
	//int dimX=ext[1]-ext[0]+1;
	//int dimY=ext[3]-ext[2]+1;
	//int dimZ=ext[5]-ext[4]+1;

	//if (_outImage != NULL ) { _outImage->Delete(); }
	//_outImage=vtkImageData::New();
	//_outImage->SetDimensions(dimX,dimY,dimZ);
	//_outImage->SetScalarTypeToUnsignedShort();
	//_outImage->AllocateScalars();
}

//-------------------------------------------------------------------------

manualBaseModel* CutModelPolygon::InitializeContourModel(std::vector<double> pointsX, std::vector<double> pointsY,  std::vector<double> pointsZ)
{ 
	manualBaseModel *model = new manualContourModelPolygon();

	int numPoints = pointsX.size();
	for(int i = 0; i<numPoints;i++)
	{
		model->AddPoint(pointsX[i],pointsY[i],pointsZ[i]);
	}

	return model;
}



////////////////////////////////////////////
// Getters and setters
////////////////////////////////////////////

vtkImageData* CutModelPolygon::getInImage()
{
	return _inImage;
}

vtkImageData* CutModelPolygon::getOutImage()
{
	return _inImage;
}

vtkPoints* CutModelPolygon::getPoints()
{
	return _points;
}

double* CutModelPolygon::getDirection()
{
	return _direction;
}

void CutModelPolygon::setInImage(vtkImageData* pImage)
{
	_inImage=pImage;
}

void CutModelPolygon::setOutImage(vtkImageData* pImage)
{
	_inImage=pImage;
}

void CutModelPolygon::setPoints(vtkPoints *pPoints)
{
	_points=pPoints;
}

void CutModelPolygon::setDirection(double *pDirection)
{
	_direction=pDirection;
}