/**
 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version. The Blender
 * Foundation also sells licenses for use in proprietary software under
 * the Blender License.  See http://www.blender.org/BL/ for information
 * about this.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 * The Original Code is Copyright (C) 2006 by NaN Holding BV.
 * All rights reserved.
 *
 * The Original Code is: all of this file.
 *
 * Contributor(s): Daniel Genrich (Genscher)
 *
 * ***** END GPL/BL DUAL LICENSE BLOCK *****
 */

#include <windows.h>

#include <stdio.h>
#include <float.h>

#include <ctime>    // For time()
#include <cstdlib>  // For srand() and rand()

#include "cloth_extern.h"
#include "vec3n.h"
#include "vecmath.h"

#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"	
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"

#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"

#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "LinearMath/btStackAlloc.h"

/*
#include <SOLID.h>
#include "MT_Point3.h"
#include "MT_Vector3.h"
*/
// #include "vclip.h"



#include <string.h>
#include <stdio.h>


#define SPRING_STRUCT (0)
#define SPRING_SHEAR  (1)
#define SPRING_BEND   (2)

static const float3x3 I(1,0,0,0,1,0,0,0,1);


#define VECADD(v1,v2,v3) 	{*(v1)= *(v2) + *(v3); *(v1+1)= *(v2+1) + *(v3+1); *(v1+2)= *(v2+2) + *(v3+2);}
#define VECSUB(v1,v2,v3) 	{*(v1)= *(v2) - *(v3); *(v1+1)= *(v2+1) - *(v3+1); *(v1+2)= *(v2+2) - *(v3+2);}
#define VECADDFAC(v1,v2,v3,fac) {*(v1)= *(v2) + *(v3)*(fac); *(v1+1)= *(v2+1) + *(v3+1)*(fac); *(v1+2)= *(v2+2) + *(v3+2)*(fac);}
#define VECCOPY(v1,v2)          {*(v1)= *(v2); *(v1+1)= *(v2+1); *(v1+2)= *(v2+2);}
#define INPR(v1, v2)		( (v1)[0]*(v2)[0] + (v1)[1]*(v2)[1] + (v1)[2]*(v2)[2] )

float Normalise(float *n)
{
	float d;
	
	d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
	/* A larger value causes normalise errors in a scaled down models with camera xtreme close */
	if(d>1.0e-35F) {
		d= (float)sqrt(d);

		n[0]/=d; 
		n[1]/=d; 
		n[2]/=d;
	} else {
		n[0]=n[1]=n[2]= 0.0;
		d= 0.0;
	}
	return d;
}

void VecMulf(float *v1, float f)
{

	v1[0]*= f;
	v1[1]*= f;
	v1[2]*= f;
}



using namespace std;


inline float3x3 dfdx_spring(const float3 &dir,float length,float rest,float k)
{
	// dir is unit length direction, rest is spring's restlength, k is spring constant.
	return  ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k;
}
inline float3x3 dfdx_damp(const float3 &dir,float length,const float3& vel,float rest,float damping)
{
	// inner spring damping   vel is the relative velocity  of the endpoints.  
	return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest)));
}
inline float3x3 dfdv_damp(const float3 &dir,float damping)
{
	// derivative of force wrt velocity.  
	return outerprod(dir,dir) * damping;
}



SpringNetwork::SpringNetwork(int _n):X(_n),V(_n),Xold(_n),Vold(_n),Xold2(_n),Vold2(_n),F(_n),dX(_n),z(_n),olddV(_n),A(_n),dFdX(_n),dFdV(_n),M(_n)
{
	assert(SPRING_STRUCT==0);
	assert(&spring_shear == &spring_struct +SPRING_SHEAR);
	assert(&spring_bend  == &spring_struct +SPRING_BEND);
	assert(&spring_struct== &spring_k[SPRING_STRUCT]);
	assert(&spring_shear == &spring_k[SPRING_SHEAR ]);
	assert(&spring_bend  == &spring_k[SPRING_BEND  ]);
	spring_struct=1000.0f; 
	spring_shear=100.0f;
	spring_bend=25.0f;
	spring_damp=5.0f;
	spring_air=1.0f;
	cb = 200 * spring_bend;
	cf = 0.01;
	csf = 0.01;
	cloth_gravity=float3(0,0,-9.81); // gravity is meters per sec^2
	step = 1;
	dtold = 0;
}


SpringNetwork::Spring &SpringNetwork::AddBlocks(Spring &s)
{
	/*
	if(s.type==SPRING_BEND)
		return s;
	*/
	// Called during initial creation of springs in our spring network.  
	// Sets up the sparse matrices corresponding to connections.
	// Note the indices (s.iab,s.iba) are also stored with spring to avoid looking them up each time a spring is applied
	// All 3 matrices A,dFdX, and dFdV are contstructed identically so the block array layout will be the same for each.
	s.iab = A.blocks.count;   // added 'ab' blocks will have this index.
	A.blocks.Add(Block(s.a,s.b)); 
	dFdX.blocks.Add(Block(s.a,s.b));
	dFdV.blocks.Add(Block(s.a,s.b));
	s.iba = A.blocks.count;   // added 'ba' blocks will have this index.
	A.blocks.Add(Block(s.b,s.a)); 
	dFdX.blocks.Add(Block(s.b,s.a));
	dFdV.blocks.Add(Block(s.b,s.a));
	return s;
}

void Zero(float3x3 &m)
{
	m.x.x = m.y.y = m.z.z = m.x.y = m.x.z = m.y.x =  m.y.z = m.z.x = m.z.y = 0;
}

void SpringNetwork::PreSolveSpring(const SpringNetwork::Spring &s, float3N &cX, float3N &cV)
{
	// DO IT HERE 
	
}


void SpringNetwork::CalcForces(ClothVertex *extforce, float3N &cX, float3N &cV)
{
	// Collect forces and derivatives:  F,dFdX,dFdV
	dFdX.Zero();
	dFdV.InitDiagonal(-spring_air);
	F.Init(cloth_gravity);
	
	if(extforce)
	{
		for(int i=0;i<F.count;i++) 
		{
			F[i] += float3(extforce[i].extForce[0], extforce[i].extForce[1], extforce[i].extForce[2]);		
		}
	}
	
	
	F -= cV * spring_air;
	
	for(int i=0;i<springs.count;i++) 
	{
		PreSolveSpring(springs[i], cX, cV); // will add to F,dFdX,dFdV
	}
}


void SpringNetwork::BuildM(int n)
{	
	// float random;
	// float signum;
	
	// srand(time(0));
	
	float3x3 temp, temp2;
	temp[0][1] = temp[0][2] = temp[1][0] = temp[1][2] = temp[2][0] = temp[2][1] = 0;

	for (int h=0; h<n; h++)
	{
		// for (int w=0; w<n; w++)
		{
			
			/*
			random = (rand() % 800) + 100; // 100 - 900
			random = 1.0f / random; 
			
			signum = (rand() %2);
			
			if(!signum)
				signum = -1.0f;
			
			
			random = weight + signum * random * weight; 		
			*/
			
			temp[0][0] = temp[1][1] = temp[2][2] = weight; //random;	
			
			/*
			if(temp[0][0]==0)
				temp2[0][0] = 0;
			else
				temp2[0][0] = 1 / temp[0][0];
				
			if(temp[1][1]==0)
				temp2[1][1] = 0;
			else
				temp2[1][1] = 1 / temp[1][1];
				
			if(temp[2][2]==0)
				temp2[2][2] = 0;
			else
				temp2[2][2] = 1 / temp[2][2];
			*/			
				
			// only fill the diagonals
			M.blocks[h].m = float3x3(temp[0][0],0,0,0,temp[1][1],0,0,0,temp[2][2]); 			
		}
	}
}


void SpringNetwork::Simulate(float3N &cX, float3N &cV, float3N &cXold, float3N &cVold, float3N &Xnew, float3N &Vnew, float dt, float ldtold, ClothVertex *extforce, int update)
{	
	
	// Get ready for conjugate gradient iterative solver step.
	// Initialize operands.
		
	int n=X.count;  // all our big vectors are of this size
	float3N temp(n);  // temp to store result of matrix multiply
	float3N B(n);  
	
	float3N dFdXmV(n);

	dX.Zero();
	A.Identity();  // build up the big matrix we feed to solver
	A -= dFdV * dt +  dFdX * (dt*dt) ;  
	dFdXmV = dFdX * V;
	B = F * dt + dFdXmV * (dt*dt);
	PreConjGradientFiltered(epsilon, dX,A,B,S, dX,z, dt);
	
	Vnew = V + dX;
	Xnew = X + Vnew*dt; 
}


void SpringNetwork::SimulateCentral(float3N &cX, float3N &cV, float3N &Vnew, float dt, ClothVertex *extforce)
{	
	int n=X.count;  // all our big vectors are of this size
	float3N temp(n);  // temp to store result of matrix multiply
	float3N B(n);  
	
	dX.Zero();	
	A.Identity();  // build up the big matrix we feed to solver

	A = M * A;
	A -= dFdV*(dt/2.0f);

	B.Zero();
	B = M * cV;
	B += (dt/2.0f)*F;

	// solve Ax = b for x (where x equals dX)
	PreConjGradientFiltered(epsilon, dX,A,B,S,z, olddV, dt);	
	// ConjGradientFiltered(dV, A, B, S, z);

	Vnew = 1.0f * dX;
}


void SpringNetwork::copyAllFromClothNetwork(ClothVertex *source, int numberV)
{
	// 1. copy cloth positions
	for(int i = 0; i < numberV; i++)
	{		
		source[i].prevV[0] = source[i].v[0];
		source[i].prevV[1] = source[i].v[1];
		source[i].prevV[2] = source[i].v[2];
	}
}

int SpringNetwork::advanceStep(float dt, ClothVertex *extforce, ClothModifierData *clmd, CM_COLLISION_SELF self_collision, CM_COLLISION_OBJ obj_collision, ClothVertex *source)
{	
	int n=X.count;  // all our big vectors are of this size
		
	float3N Xnew(n);
	float3N Vnew(n);

	float3N Vcorr(n);
	
	float t = 0.0f;
	float tf = 1.0f;

	
	LARGE_INTEGER before, before1, before2, before3;
	LARGE_INTEGER after, after1, after2, after3;
	LARGE_INTEGER freq;
	double milliseconds;
	bool timecoll_occured = 0, coll_occured = 0;
	int c = 0;

	if(dtold == 0)
		dtold = dt;

	while(t < tf)
	{ 
			
		// clean matrix S to only hold pinned verts
		for(int i =  S.blocks.count-1; i >=0 ; i--)
		{
			while((i<S.blocks.count) && (!S.blocks[i].pinned))
			{
				z[S.blocks[i].c] = float3(0,0,0);
				S.blocks.DelIndexWithLast(i);
			}
		}
		
		CalcForces(extforce, X, V);

		Simulate(X, V, Xold, Vold, Xnew, Vnew, dt, dt, extforce, 1);
		
		// update collision positions with suggested future positions
		for(int i = 0; i < n; i++)
		{		
			source[i].v[0] = (Xnew[i])[0];
			source[i].v[1] = (Xnew[i])[1];
			source[i].v[2] = (Xnew[i])[2];
			
			VECSUB(source[i].velocity, source[i].v, source[i].prevV);
			VecMulf(source[i].velocity, 1.0f/dt);
		}	
		
		// get & save all coliding faces
		if (obj_collision) 
		{
			// get & save all coliding faces
			obj_collision (clmd, step, implicit_collision_response);		
			
			// check for near faces and resolve colissions
			coll_occured = solve_coherence(dt, source); 	
		}	
		
		/*
		if (obj_collision) 
		{			
			// get & save all coliding faces
			obj_collision (clmd, step, implicit_timecollision_response);
			
			// check for near faces and resolve potential collisions
			timecoll_occured = solve_timecoherence(dt, source);
		}			
		*/
		
		if(coll_occured)
		{

			// update the corrected velocity
			for(int i = 0; i < n; i++)
			{		
				(Vcorr[i])[0] = source[i].velocity[0];
				(Vcorr[i])[1] = source[i].velocity[1];
				(Vcorr[i])[2] = source[i].velocity[2];
			}

			Xnew = X + Vcorr*dt;

			CalcForces(extforce, Xnew, Vcorr);

			SimulateCentral(X, Vcorr, Vnew, dt, extforce);	

			olddV = Vnew - Vcorr;
			
			// update the cloth network
			Vold = Vcorr;
		}
		else
			Vold = V;

		Xold = X;
		
		for(int i = 0; i < n; i++)
		{		
			source[i].prevV[0] = (Xnew[i])[0];
			source[i].prevV[1] = (Xnew[i])[1];
			source[i].prevV[2] = (Xnew[i])[2];		
		}
				
		V = Vnew;
		X = Xnew;
		
		t = t + dt;
		
		/*
		if(!step)
			step = true;
		*/

		c++;	

		
		// if(c==4)
		// break;
		
	}
			
	return true;
}



extern "C"
SpringNetwork *initClothNetwork(ClothVertex *source, int numberV, ClothSpring *springs, int numberS, SimulationSettings *simsettings, CollisionSettings *collsettings)
{
	SpringNetwork *cloth = new SpringNetwork(numberV);
	
	// 1. copy settings	
	cloth->spring_damp = simsettings->Cdis * 0.01; // mechanical spring damping
	cloth->spring_air = simsettings->Cvi * 0.01; // viscous spring damping
	cloth->cloth_gravity[0] = simsettings->gravity[0]; cloth->cloth_gravity[1] = simsettings->gravity[1]; cloth->cloth_gravity[2] = simsettings->gravity[2]; // gravity
	cloth->spring_struct = simsettings->structural; // spring_struct=1000.0f;
	cloth->spring_shear = simsettings->shear; // spring_shear=100.0f;
	cloth->spring_bend = simsettings->flexion; // spring_bend=25.0f;
	cloth->cb = 20000 * cloth->spring_bend; // bei 2000 * bending => 500 - 1000 struct/shear = viele Falten, 3000 = wenig/keine Falten
	cloth->weight = simsettings->mass;
	cloth->epsilon = collsettings->epsilon;
	cloth->cf = collsettings->friction * 0.01; // friction
	cloth->csf = collsettings->self_friction * 0.01; // selffriction

	// 2. copy cloth positions
	for(int i = 0; i < numberV; i++)
	{
		cloth->X[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
		cloth->Xold[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);

		if(source [i].flags & CVERT_FLAG_PINNED)
		{
			cloth->S.blocks.Add(Block(i,i)).m=float3x3(0,0,0,0,0,0,0,0,0);
			cloth->S.blocks[cloth->S.blocks.count-1].ndof = 0;
			cloth->S.blocks[cloth->S.blocks.count-1].pinned = 1;
		}
	}
	
	
	// 3. copy springs
	for (int i = 0; i < numberS; i++)
	{
		switch (springs [i].type)
		{	
			case STRUCTURAL:
			cloth->CreateSpring(SPRING_STRUCT,springs [i].ij,springs [i].kl); 			
			break;
			case SHEAR:
			cloth->CreateSpring(SPRING_SHEAR,springs [i].ij,springs [i].kl); 
			break;
			case FLEXION:	
			cloth->CreateSpring(SPRING_BEND,springs [i].ij,springs [i].kl); 
			break;
		}
		
	}
	
	cloth->	BuildM(numberV);
	
	return cloth;
}

extern "C"
void setupzOfClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV, float dt)
{	
	// apply velocity of the pinned verteces
	for(int i = 0; i < numberV; i++)
	{
		if(source [i].flags & CVERT_FLAG_PINNED)
		{
			float3 tempa(source [i].v[0], source [i].v[1], source [i].v[2]);  
			float3 tempb(source [i].prevV[0], source [i].prevV[1], source [i].prevV[2]);  
			cloth->z[i]= (tempa - tempb) * dt;	
		}
	}

}

extern "C"
void copyToClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV, ClothSpring *springs, int numberS)
{
	if(!cloth)
		return;
	
	// TODO
	// check if width*height == numberV
	
	// 1. copy cloth positions
	for(int i = 0; i < numberV; i++)
	{
		cloth->X[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
		cloth->Xold[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
	}
	
	// 2. copy springs
	for (int i = 0; i < numberS; i++)
	{
		switch (springs [i].type)
		{	
			case STRUCTURAL:
			cloth->CreateSpring(SPRING_STRUCT,springs [i].ij,springs [i].kl); 			
			break;
			case SHEAR:
			cloth->CreateSpring(SPRING_SHEAR,springs [i].ij,springs [i].kl); 
			break;
			case FLEXION:	
			cloth->CreateSpring(SPRING_BEND,springs [i].ij,springs [i].kl); 
			break;
		}
		
	}
	
	// 3. copy settings
	// TODO

}

extern "C"
void copyFromClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV)
{
	if(!cloth)
		return;
		
	// 1. copy cloth positions
	for(int i = 0; i < numberV; i++)
	{		
		source[i].v[0] = (cloth->X[i])[0];
		source[i].v[1] = (cloth->X[i])[1];
		source[i].v[2] = (cloth->X[i])[2];
	}
}



extern "C"
void deleteClothNetwork(SpringNetwork *cloth)
{	
	if(cloth)
	{					
		delete cloth;
		cloth = NULL;
	}
}

extern "C"
void simulateClothNetwork(SpringNetwork *clothNetwork, float timestep, ClothVertex *extforce, ClothModifierData *clmd, CM_COLLISION_SELF self_collision, CM_COLLISION_OBJ obj_collision, ClothVertex *source)
{
	if(!clothNetwork)
		return;
		
	clothNetwork->advanceStep(timestep, extforce, clmd,  self_collision, obj_collision, source);
	
}

extern "C"
int getStepFromClothNetwork(SpringNetwork *cloth)
{
	if(!cloth)
		return -1;
		
	return cloth->step;
}

extern "C"
void setStepFromClothNetwork(SpringNetwork *cloth, int step)
{
	if(!cloth)
		return;
		
	cloth->step = step;
}