#include "vtkOrgan.h"


#include "vtkObjectFactory.h"
#include "vtkModeloDeformable.h"
#include "vtkLink.h"
#include "vtkContact.h"
#include "vtkesquiT2MeshWin32Header.h"
#include "vtkEsquiIdListCollection.h"


#include <float.h>



#ifndef max
#define max(a,b)            (((a) > (b)) ? (a) : (b))
#endif

vtkCxxRevisionMacro(vtkOrgan, "$Revision: 0.1 $");
vtkOrgan::vtkOrgan()
{
	// empty, undeformed mesh
	m_uNNodes = m_uNEl = 0;
	m_dScale		   = 0.0;
	m_bDeformed		   = false;
	m_bNewVertices	   = true;

	// default gravity in -z - direction
	m_yDirOfGravity    = 2;
	m_cOrientOfGravity = -1;

	// incision data
	m_fMaxCutDistance = 0.0;
	m_fTipsDistance   = 0.0f;

	m_bEnganchable = false;

	//parámetros de desgarro
	m_bDesgarrable = false;
	m_fUmbralDesgarro = 2.0f;
	
	_pDeformCModel = NULL;
	m_id = -1;

	m_fMaxX=FLT_MIN;
	m_fMaxY=FLT_MIN;
	m_fMaxZ=FLT_MIN;
	m_vCenter[0] = 0.0f;
	m_vCenter[1] = 0.0f;
	m_vCenter[2] = 0.0f;

	//información de elementos movidos
	m_nElementosMovidos = 0;
	m_piElementosMovidos = NULL;
	//información de nodos movidos
	m_nNodosMovidos = 0;
	m_piNodosMovidos = NULL;
	this->m_aluFixedNodes= vtkEsquiIdListCollection::New();
	this->m_aluFixedNodes->AddItem(vtkEsquiIdList::New());
	this->m_aluFixedNodes->AddItem(vtkEsquiIdList::New());
	this->m_aluFixedNodes->AddItem(vtkEsquiIdList::New());
	this->m_lpContacts = vtkEsquiContactCollection::New();
	this->m_lpLinks = vtkEsquiLinkCollection::New();
}
//
//
//
vtkOrgan::~vtkOrgan(){
	vtkLink* pLk=NULL;
	if( !m_lpLinks->IsEmpty() )
	{
		do
		{
			pLk = m_lpLinks->GetFirst(); 
			m_lpLinks->DeleteNode(0);
			pLk -> UnRef();
		}  while( !m_lpLinks->IsEmpty() );
	}
	delete[] m_afCurrentCoord;
	delete[] m_afTextureCoord;
	delete[] m_afUV;
	delete[] m_afColisionCoord;
	delete[] m_afColisionCoordAnt;
	delete[] m_auNodePtr;

	if(m_nElementosMovidos != 0)
		delete[] m_piElementosMovidos;
	if(m_nNodosMovidos != 0)
		delete[] m_piNodosMovidos;

	if(_pDeformCModel) 
		_pDeformCModel->Delete();
	this->m_aluFixedNodes->Delete();
	this->m_lpLinks->Delete();
}
//
//
//
void vtkOrgan::SetModelId(int id){
	m_id = id;
}

void vtkOrgan::Inicializar(){
	
	int numVertices=0;
	int numPolis=0;			
	int nIndice=0;
	int n;
	float uv[2];
	int i=0;
	float vertex[3];

	m_uNNodes = (int)this->vtkPolyData::GetNumberOfPoints();
	m_uNEl = (int) this->vtkPolyData::GetNumberOfPolys();
	

	/// Inicializacion de los punteros a vértices y caras
	/// De primeras se reserva un tamaño suficiente
	m_afCurrentCoord = new float[3*100*m_uNNodes];	// Reservamos espacio para 100 veces más 
	m_afColisionCoord = new float[3*1000*m_uNNodes];
	m_afColisionCoordAnt = new float[3*1000*m_uNNodes];
	m_afTextureCoord = new float[9*100*m_uNEl];	
	m_afUV = new float[6*100*m_uNEl];
	m_auNodePtr = new int[3*100*m_uNEl];
	
	/// Rellenamos la lista de Coordenadas de vértices
	for (i=0;i<m_uNNodes;i++){
		this->posicionVertice(i,vertex);
		m_afCurrentCoord[i*3] = vertex[0];
		m_afCurrentCoord[i*3+1] = vertex[1];
		m_afCurrentCoord[i*3+2] = vertex[2];
	
		m_vCenter[0] += vertex[0];
		m_vCenter[1] += vertex[1];
		m_vCenter[2] += vertex[2];
				
		if (vertex[0] <0 ) vertex[0]*=-1;
		if (vertex[1] <0 ) vertex[1]*=-1;
		if (vertex[2] <0 ) vertex[2]*=-1;

		if (vertex[0] > m_fMaxX ) m_fMaxX=vertex[0];	
		if (vertex[1] > m_fMaxY ) m_fMaxY=vertex[1];
		if (vertex[2] > m_fMaxZ ) m_fMaxZ=vertex[2];
	}
	/// Ahora creamos la lista de índices a cara y coordenadas de textura
	for (i=0;i<m_uNEl;i++){

		nIndice = this->idVerticePoligono(i,0);
		this->coordTexturaVertPoligono(i,0,uv);	
		n=i*3;
		m_auNodePtr[n] = nIndice;
		SetUV(n,uv[0],uv[1]);		
				
		nIndice = idVerticePoligono(i,1);		
		this->coordTexturaVertPoligono(i,1,uv);
		n=i*3+1;
		m_auNodePtr[n] = nIndice;		
		SetUV(n,uv[0],uv[1]);		
		
		nIndice = this->idVerticePoligono(i,2);
		this->coordTexturaVertPoligono(i,2,uv);
		n=i*3+2;
		m_auNodePtr[n] = nIndice;
		SetUV(n,uv[0],uv[1]);						
	}

	/// Creamos el objeto colision del modelo ... sólo se inicializa
	/// Se obtendrá el bueno después cuando se haya recalculado las extensiones de la caja
	/// Por parte del modelo deformable que corresponda llamando a UpdateCollisionModel()
	_pDeformCModel = vtkModeloDeformable::New();
	_pDeformCModel->numeroVertices = m_uNNodes;
	_pDeformCModel->numeroCaras = m_uNEl;
	_pDeformCModel->vertices = new float[3*100*m_uNNodes];	
	_pDeformCModel->caras = new int[3*100*m_uNEl];	
	_pDeformCModel->idVerticesMovidos = new int[100*m_uNNodes];
}
//
//
//
void vtkOrgan::InitCollisionModel(){	
	m_vCenter[0]/=m_uNNodes;
	m_vCenter[1]/=m_uNNodes;
	m_vCenter[2]/=m_uNNodes;

	_pDeformCModel->Caja.centro[0] = m_vCenter[0];
	_pDeformCModel->Caja.centro[1] = m_vCenter[1];
	_pDeformCModel->Caja.centro[2] = m_vCenter[2];
	_pDeformCModel->Caja.extension[0] = m_fMaxX;
	_pDeformCModel->Caja.extension[1] = m_fMaxY;
	_pDeformCModel->Caja.extension[2] = m_fMaxZ;

	_pDeformCModel->numVerticesMovidos = 0;	
	_pDeformCModel->numeroVertices = m_uNNodes;
	_pDeformCModel->numeroCaras = m_uNEl;	
	memcpy(_pDeformCModel->vertices,m_afCurrentCoord,3*m_uNNodes*sizeof(float));			
	memcpy(_pDeformCModel->caras,m_auNodePtr,3*m_uNEl*sizeof(int));	

}
//
//
//
void vtkOrgan::UpdateCollisionModel(){	
	m_vCenter[0]/=m_uNNodes;
	m_vCenter[1]/=m_uNNodes;
	m_vCenter[2]/=m_uNNodes;

	_pDeformCModel->Caja.centro[0] = m_vCenter[0];
	_pDeformCModel->Caja.centro[1] = m_vCenter[1];
	_pDeformCModel->Caja.centro[2] = m_vCenter[2];
	_pDeformCModel->Caja.extension[0] = m_fMaxX;
	_pDeformCModel->Caja.extension[1] = m_fMaxY;
	_pDeformCModel->Caja.extension[2] = m_fMaxZ;

	int numVertices;
	// Obtiene el numero de caras y vertices movidos
	int* idVertices = GetMovedNodes(numVertices);


	// Guarda los indices de los vertices que se han movido.
	// Se utiliza para actualizar correctamente la libreria de colisiones
	_pDeformCModel->numVerticesMovidos=numVertices;
	if(numVertices){ //Si se han movido vertices
		memcpy(_pDeformCModel->idVerticesMovidos,idVertices,numVertices*sizeof(int));
	}


	_pDeformCModel->numeroVertices = m_uNNodes;
	_pDeformCModel->numeroCaras = m_uNEl;	
	memcpy(_pDeformCModel->vertices,m_afCurrentCoord,3*m_uNNodes*sizeof(float));			
	memcpy(_pDeformCModel->caras,m_auNodePtr,3*m_uNEl*sizeof(int));	
}// fin updateCollisionModel()
//
//
//
bool vtkOrgan::MakeCoordLocal( float c_afCoord[], const UINT c_uNode0, const UINT c_uNode1, const UINT c_uNode2 )
{
	///Cambiar la clase CStdSubVector a vtkStdSubVector previa correccion de lo necesario.
	// short cut to nodal coordinates
	vtkStdSubVector svpCoord0( &m_afCurrentCoord[3*c_uNode0] );
	vtkStdSubVector svpCoord1( &m_afCurrentCoord[3*c_uNode1] );
	vtkStdSubVector svpCoord2( &m_afCurrentCoord[3*c_uNode2] );

	// point's position w.r.t. element's node 2 in global x- and y-direction
	const float fCX = c_afCoord[0] - svpCoord2[0], fCY = c_afCoord[1] - svpCoord2[1];

	// element's local coordinate system axes Xi0 and Xi1 in global coordinates
	//   (xi0 from node 2 to node 0 and xi1 from node 2 to node 1)
	const float fXi0X = svpCoord0[0] - svpCoord2[0];
	const float fXi0Y = svpCoord0[1] - svpCoord2[1];
	const float fXi1X = svpCoord1[0] - svpCoord2[0];
	const float fXi1Y = svpCoord1[1] - svpCoord2[1];

	// starting from XiC0 * (P0 - P2) + XiC1 * (P1 - P2) = P - P2, where 0, 1, and 2 indicate the
	//   local nodes and XiC the inquired point's local coordinates, determine the latter:
	// ensure non-zero denominator of solution by switching between x-, y-, and z-coordinates, if
	//   necessary, starting with the pair x/y
	double dDenominator, dEnumerator0, dEnumerator1;
	double dTerm1 = (double)fXi0Y * fXi1X;
	double dTerm2 = (double)fXi0X * fXi1Y;
	double dMaxAbsTerm = max( fabs( dTerm1 ),fabs( dTerm2 ) );
	if( dMaxAbsTerm > 0 && fabs( dDenominator = dTerm1 - dTerm2 ) / dMaxAbsTerm > 0.001 )
	{
		dEnumerator0 = (double)fCY * fXi1X - (double)fCX * fXi1Y;
		dEnumerator1 = (double)fCX * fXi0Y - (double)fCY * fXi0X;
	}
	else
	{
		// second try with x/z
		const float fCZ	  = c_afCoord[2] - svpCoord2[2];
		const float fXi0Z = svpCoord0[2] - svpCoord2[2];
		const float fXi1Z = svpCoord1[2] - svpCoord2[2];
		dTerm1 = (double)fXi0Z * fXi1X;
		dTerm2 = (double)fXi0X * fXi1Z;
		dMaxAbsTerm = max( fabs( dTerm1 ),fabs( dTerm2 ) );
		if( dMaxAbsTerm > 0 && fabs( dDenominator = dTerm1 - dTerm2 ) / dMaxAbsTerm > 0.001 )
		{
			dEnumerator0 = (double)fCZ * fXi1X - (double)fCX * fXi1Z;
			dEnumerator1 = (double)fCX * fXi0Z - (double)fCZ * fXi0X;
		}
		else
		{
			// last resource y/z
			dTerm1 = (double)fXi0Y * fXi1Z;
			dTerm2 = (double)fXi0Z * fXi1Y;
			dMaxAbsTerm = max( fabs( dTerm1 ),fabs( dTerm2 ) );
			if( dMaxAbsTerm > 0 && fabs( dDenominator = dTerm1 - dTerm2 ) / dMaxAbsTerm > 0.001 )
			{
				dEnumerator0 = (double)fCY * fXi1Z - (double)fCZ * fXi1Y;
				dEnumerator1 = (double)fCZ * fXi0Y - (double)fCY * fXi0Z;
			}
			else{
			
				Error( "Organ contains singular element." );

			}
		}
	}

	// finally compute local coordinates, where Xi2 follows from Xi0 + Xi1 + Xi2 = 0
	float fXiC0 = (float)( dEnumerator0 / dDenominator ),
		  fXiC1 = (float)( dEnumerator1 / dDenominator ),
		  fXiC2 = (float)( 1.0 - fXiC0 - fXiC1 );

	// assert legal coordinates (between 0 and 1 all of them)
	if( fXiC0 < -.01f || fXiC1 < -.01f || fXiC2 < -.01f )
	{
		c_afCoord[0] = 0.33f;
		c_afCoord[1] = 0.33f;
		c_afCoord[2] = 0.33f;
		return false;
	}
	if (fXiC0 < 0.0f )
		fXiC0 = 0.0f;
	if (fXiC1 < 0.0f )
		fXiC1 = 0.0f;
	if (fXiC2 < 0.0f )
		fXiC2 = 0.0f;

	// replace the input global coordinates by their local counterparts
	c_afCoord[0] = fXiC0;
	c_afCoord[1] = fXiC1;
	c_afCoord[2] = fXiC2;
	return true;

} 
//
//
//
void vtkOrgan::UpdateGeometry()
{
	if (!m_bNewVertices){
		/// No hay que regenerar la geometría, sólo mover los vértices
		this->modificaGeometria();		
	}
	else{
		
		m_bDeformed = false;
		m_bNewVertices=false;	
	}
} 
//
//
//
void vtkOrgan::GetUV(int i,float* u,float* v){

	*u=m_afTextureCoord[i*3];
	*v=m_afTextureCoord[i*3 + 1];
}
//
//
//
void vtkOrgan::SetUV(int i,float u,float v){

	m_afTextureCoord[i*3]=u;
	m_afTextureCoord[i*3 + 1]=v;
	m_afTextureCoord[i*3 + 2]=0.0f;
}
//
//
//
void vtkOrgan::AddBoundaryCondition(vtkIdType valorX, vtkIdType valorY, vtkIdType valorZ){
	

	if( this->m_aluFixedNodes->GetItem(0)->Find( valorX ) == NULL )
			this->m_aluFixedNodes->GetItem(0)->Append( valorX );
	if( this->m_aluFixedNodes->GetItem(1)->Find( valorY ) == NULL )
			this->m_aluFixedNodes->GetItem(1)->Append( valorY );
	if( this->m_aluFixedNodes->GetItem(2)->Find( valorZ ) == NULL )
			this->m_aluFixedNodes->GetItem(2)->Append( valorZ );

}

void vtkOrgan::LoadBoundaryConditions(){
	
 	m_acFixed.SetSize( m_uNNodes,BESS_RESERVE_NODES );
 	m_acFixed.Initialize( 0 );
	for (unsigned i=0;i<m_aluFixedNodes->GetItem(0)->GetCount();i++) {
 		m_acFixed[m_aluFixedNodes->GetItem(0)->GetId(i)]  = 1;
	}
	for (unsigned i=0;i<m_aluFixedNodes->GetItem(1)->GetCount();i++) {
 		m_acFixed[m_aluFixedNodes->GetItem(1)->GetId(i)]  += 2;
	}
	for (unsigned i=0;i<m_aluFixedNodes->GetItem(2)->GetCount();i++) {
 		m_acFixed[m_aluFixedNodes->GetItem(2)->GetId(i)]  += 4;
	}
 	
}