#include "CooneyFat.h"

using namespace Cooney;
using namespace Cooney::Math;

namespace Cooney
{	
	bool Fat::BakeFat(Cooney::Mesh &thinMesh, Cooney::Mesh &fatMesh, float epsilon,
					float stiffness, float damping, float cornerStiffness, float cornerDamping)
	{
		if( thinMesh.NumVertexes() != fatMesh.NumVertexes() )
			return false; // The thin and fat mesh do not map, fail.
		
		// Re-initialize the soft-body.
		fat = Cooney::SoftBody();
		// Clear all of the fat-info.
		fatinfo.clear();
		
		// Loop through all of the thin-mesh's quads
		// and look for fat-simulation candidates when
		// comparing to the fat-mesh.
		for(unsigned int i=0; i<thinMesh.NumQuads(); ++i)
		{
			bool bVAFat=false, bVBFat=false, bVCFat=false, bVDFat=false;
			
			// Get the quad information for both the thin and fat mesh.
			Cooney::MeshVertQuad thinQuad = thinMesh.GetVertQuad(i);
			Cooney::MeshVertQuad fatQuad = fatMesh.GetVertQuad(i);
			Cooney::MeshIndexQuad thinIndexQuad = thinMesh.GetIndexQuad(i);
			Cooney::MeshIndexQuad fatIndexQuad = fatMesh.GetIndexQuad(i);
			
			// Compare differences in distance for each of the quad's points.
			// If they exceed the local epsilon value, they will be added to
			// the fat simulation.
			// A
			if( Cooney::Math::Mag3(fatQuad.vertA.position - thinQuad.vertA.position) > epsilon )
				bVAFat = true;
			// B
			if( Cooney::Math::Mag3(fatQuad.vertB.position - thinQuad.vertB.position) > epsilon )
				bVBFat = true;
			// C
			if( Cooney::Math::Mag3(fatQuad.vertC.position - thinQuad.vertC.position) > epsilon )
				bVCFat = true;
			// D
			if( Cooney::Math::Mag3(fatQuad.vertD.position - thinQuad.vertD.position) > epsilon )
				bVDFat = true;
			
			// If there is no fat here at all, continue.
			if( !bVAFat && !bVBFat && !bVCFat && !bVDFat )
				continue;
			
			// Add the low, thin quad to the fat springymesh.
			int thinAIndex=-1, thinBIndex=-1, thinCIndex=-1, thinDIndex=-1;
			fat.AddQuad(thinQuad.vertA.position, thinQuad.vertB.position, thinQuad.vertC.position, thinQuad.vertD.position,
						&thinAIndex, &thinBIndex, &thinCIndex, &thinDIndex);
			
			// Add the high, not so thin (fat) quad to the fat springymesh.
			int fatAIndex=-1, fatBIndex=-1, fatCIndex=-1, fatDIndex=-1;
			fat.AddQuad(fatQuad.vertA.position, fatQuad.vertB.position, fatQuad.vertC.position, fatQuad.vertD.position,
						&fatAIndex, &fatBIndex, &fatCIndex, &fatDIndex);
			
			// Connect the thin and the fat layer.
			fat.AddEdge(thinAIndex, fatAIndex);
			fat.AddEdge(thinBIndex, fatBIndex);
			fat.AddEdge(thinCIndex, fatCIndex);
			fat.AddEdge(thinDIndex, fatDIndex);
			
			// Interconnect the thin and the fat layer.
			// Fat quad
			fat.AddEdge(fatAIndex, fatCIndex);
			fat.AddEdge(fatBIndex, fatDIndex);
			// Wall 1
			fat.AddEdge(thinAIndex, fatBIndex);
			fat.AddEdge(thinBIndex, fatAIndex);
			// Wall 2
			fat.AddEdge(thinBIndex, fatCIndex);
			fat.AddEdge(thinCIndex, fatBIndex);
			// Wall 3
			fat.AddEdge(thinCIndex, fatDIndex);
			fat.AddEdge(thinDIndex, fatCIndex);
			// Wall 4
			fat.AddEdge(thinDIndex, fatAIndex);
			fat.AddEdge(thinAIndex, fatDIndex);
			// Across edge braces (not corner braces).
			fat.AddEdge(thinAIndex, fatCIndex);
			fat.AddEdge(thinBIndex, fatDIndex);
			fat.AddEdge(thinCIndex, fatAIndex);
			fat.AddEdge(thinDIndex, fatBIndex);
			
			// Setup the fatpointinfos and get ready
			// to store thenm.
			FatPointInfo thinInfo[4], fatInfo[4];
			
			// Setup the thin fatpointinfos.
			// The thin layer is LOCKED.
			thinInfo[0].bLocked = true;
			thinInfo[0].meshLink = thinIndexQuad.indexA;
			thinInfo[0].fatLink = thinAIndex;
			thinInfo[1].bLocked = true;
			thinInfo[1].meshLink = thinIndexQuad.indexB;
			thinInfo[1].fatLink = thinBIndex;
			thinInfo[2].bLocked = true;
			thinInfo[2].meshLink = thinIndexQuad.indexC;
			thinInfo[2].fatLink = thinCIndex;
			thinInfo[3].bLocked = true;
			thinInfo[3].meshLink = thinIndexQuad.indexD;
			thinInfo[3].fatLink = thinDIndex;
			
			// Setup the fat fatpointinfos.
			// The fat layer is UNLOCKED.
			fatInfo[0].bLocked = false;
			fatInfo[0].meshLink = fatIndexQuad.indexA;
			fatInfo[0].fatLink = fatAIndex;
			fatInfo[1].bLocked = false;
			fatInfo[1].meshLink = fatIndexQuad.indexB;
			fatInfo[1].fatLink = fatBIndex;
			fatInfo[2].bLocked = false;
			fatInfo[2].meshLink = fatIndexQuad.indexC;
			fatInfo[2].fatLink = fatCIndex;
			fatInfo[3].bLocked = false;
			fatInfo[3].meshLink = fatIndexQuad.indexD;
			fatInfo[3].fatLink = fatDIndex;
			
			// Add the fatpointinfos.
			AddUniqueFatPointInfo(thinInfo[0]);
			AddUniqueFatPointInfo(thinInfo[1]);
			AddUniqueFatPointInfo(thinInfo[2]);
			AddUniqueFatPointInfo(thinInfo[3]);
			AddUniqueFatPointInfo(fatInfo[0]);
			AddUniqueFatPointInfo(fatInfo[1]);
			AddUniqueFatPointInfo(fatInfo[2]);
			AddUniqueFatPointInfo(fatInfo[3]);
			
			// Although we set the above fat-
			// point-infos to locked, we need
			// to tell the fat simulation the
			// same thing.
			fat.SetLocked(thinAIndex);
			fat.SetLocked(thinBIndex);
			fat.SetLocked(thinCIndex);
			fat.SetLocked(thinDIndex);
		}
		
		// Loop through all of the thin-mesh's quads
		// and look for fat-simulation candidates when
		// comparing to the fat-mesh.
		for(unsigned int i=0; i<thinMesh.NumFaces(); ++i)
		{
			bool bVAFat=false, bVBFat=false, bVCFat=false;
			
			// Get the triangle information for both the thin and fat mesh.
			Cooney::MeshVertFace thinFace = thinMesh.GetVertFace(i);
			Cooney::MeshVertFace fatFace = fatMesh.GetVertFace(i);
			Cooney::MeshIndexFace thinIndexFace = thinMesh.GetIndexFace(i);
			Cooney::MeshIndexFace fatIndexFace = fatMesh.GetIndexFace(i);
			
			// Compare differences in distance for each of the tri's points.
			// If they exceed the local epsilon value, they will be added to
			// the fat simulation.
			// A
			if( Cooney::Math::Mag3(fatFace.vertA.position - thinFace.vertA.position) > epsilon )
				bVAFat = true;
			// B
			if( Cooney::Math::Mag3(fatFace.vertB.position - thinFace.vertB.position) > epsilon )
				bVBFat = true;
			// C
			if( Cooney::Math::Mag3(fatFace.vertC.position - thinFace.vertC.position) > epsilon )
				bVCFat = true;
			
			// If there is no fat here at all, continue.
			if( !bVAFat && !bVBFat && !bVCFat )
				continue;
			
			// Add the low, thin quad to the fat springymesh
			int thinAIndex=-1, thinBIndex=-1, thinCIndex=-1;
			fat.AddTriangle(thinFace.vertA.position, thinFace.vertB.position, thinFace.vertC.position,
						&thinAIndex, &thinBIndex, &thinCIndex);
			
			// Add the high, not so thin (fat) quad to the fat springymesh
			int fatAIndex=-1, fatBIndex=-1, fatCIndex=-1;
			fat.AddTriangle(fatFace.vertA.position, fatFace.vertB.position, fatFace.vertC.position,
						&fatAIndex, &fatBIndex, &fatCIndex);
			
			// Connect the thin and the fat layer.
			fat.AddEdge(thinAIndex, fatAIndex);
			fat.AddEdge(thinBIndex, fatBIndex);
			fat.AddEdge(thinCIndex, fatCIndex);
			
			// Interconnect the thin and the fat layer.
			// Wall 1
			fat.AddEdge(thinAIndex, fatBIndex);
			fat.AddEdge(thinBIndex, fatAIndex);
			// Wall 2
			fat.AddEdge(thinBIndex, fatCIndex);
			fat.AddEdge(thinCIndex, fatBIndex);
			// Wall 3
			fat.AddEdge(thinCIndex, fatAIndex);
			fat.AddEdge(thinAIndex, fatCIndex);
			
			// Setup the fatpointinfos and get
			// ready to store them.
			FatPointInfo thinInfo[3], fatInfo[3];
			
			// Setup the thin fatpointinfos
			// The thin layer is LOCKED.
			thinInfo[0].bLocked = true;
			thinInfo[0].meshLink = thinIndexFace.indexA;
			thinInfo[0].fatLink = thinAIndex;
			thinInfo[1].bLocked = true;
			thinInfo[1].meshLink = thinIndexFace.indexB;
			thinInfo[1].fatLink = thinBIndex;
			thinInfo[2].bLocked = true;
			thinInfo[2].meshLink = thinIndexFace.indexC;
			thinInfo[2].fatLink = thinCIndex;
			
			// Setup the fat fatpointinfos.
			// The fat layer is UNLOCKED.
			fatInfo[0].bLocked = false;
			fatInfo[0].meshLink = fatIndexFace.indexA;
			fatInfo[0].fatLink = fatAIndex;
			fatInfo[1].bLocked = false;
			fatInfo[1].meshLink = fatIndexFace.indexB;
			fatInfo[1].fatLink = fatBIndex;
			fatInfo[2].bLocked = false;
			fatInfo[2].meshLink = fatIndexFace.indexC;
			fatInfo[2].fatLink = fatCIndex;
			
			// Add the fatpointinfos.
			// The fat layer is UNLOCKED.
			AddUniqueFatPointInfo(thinInfo[0]);
			AddUniqueFatPointInfo(thinInfo[1]);
			AddUniqueFatPointInfo(thinInfo[2]);
			AddUniqueFatPointInfo(fatInfo[0]);
			AddUniqueFatPointInfo(fatInfo[1]);
			AddUniqueFatPointInfo(fatInfo[2]);
			
			// Although we set the above fat-
			// point-infos to locked, we need
			// to tell the fat simulation the
			// same thing.
			fat.SetLocked(thinAIndex);
			fat.SetLocked(thinBIndex);
			fat.SetLocked(thinCIndex);
		}
		
		// Initiate and prepare the soft-body for realtime.
		fat.Initiate();
		// Prepare the fat simulation's soft-body parameters.
		fat.FloodSettings(stiffness, damping, cornerStiffness, cornerDamping);
		
		return false;
	}
	
	void Fat::OverlayFat(Cooney::Mesh &outMesh)
	{
		// Snag the output vertices array.
		Cooney::Math::Vec3 *meshVerts = outMesh.VertPositions();
		if( !meshVerts )
			return;
		
		// Loop through every fat-point and copy relevent
		// soft-body information to the output mesh.
		for(unsigned int i=0; i<fatinfo.size(); ++i)
		{
			// Locked fat represents the original thin mesh verts
			// which are animating. When overlaying the fat, we
			// want to overlay the thin mesh verts with the fat
			// unlocked, simulated verts. We skip overlaying locked
			// fat verts.
			if( fatinfo[i].bLocked )
				continue;

			// Invalid index check.
			if( fatinfo[i].fatLink >= fat.NumPoints() || fatinfo[i].meshLink >= outMesh.NumVertexes() )
				continue;
			
			// Get the fat point position from the soft body.
			Cooney::Math::Vec3 fatPoint = fat.GetPoint(fatinfo[i].fatLink);
			
			// Overlay the fat point from the soft body onto the outMesh.
			meshVerts[fatinfo[i].meshLink] = fatPoint;
		}
	}
	
	void Fat::UpdateFat(float timeStep, Cooney::Mesh &inMesh)
	{
		// Snag the output vertices array.
		Cooney::Math::Vec3 *meshVerts = inMesh.VertPositions();
		if( !meshVerts )
			return;
		
		// Loop through the fat info objects and force parts
		// of the soft-body that represent the thin mesh to
		// stick to the thin mesh.
		for(unsigned int i=0; i<(unsigned int)fatinfo.size(); ++i)
		{
			// Unlocked fat does not stick to the inMesh.
			// No need to process it.
			if( !fatinfo[i].bLocked )
				continue;

			// Invalid index check.
			if( fatinfo[i].fatLink >= fat.NumPoints() || fatinfo[i].meshLink >= inMesh.NumVertexes() )
				continue;
			
			// Get the point position from the source mesh.
			Cooney::Math::Vec3 meshPoint = meshVerts[fatinfo[i].meshLink];

			// Apply the point position to the relevant fat point.
			fat.SetPoint(fatinfo[i].fatLink, meshPoint);
		}

		// Take a simulation step.
		fat.Step(timeStep);
	}
	
	bool Fat::AddUniqueFatPointInfo(FatPointInfo &possibleFatPoint)
	{	
		// Loop through and look for duplicates.
		for(unsigned int i=0; i<(unsigned int)fatinfo.size(); ++i)
		{
			if( fatinfo[i].fatLink == possibleFatPoint.fatLink )
				return false;
		}
		
		// Add the unique fat point.
		fatinfo.push_back(possibleFatPoint);
		return true;
	}

	void Fat::PushGL()
	{
		// Delegate rendering to the soft-body.
		fat.PushGL();
	}
};