#include <stdio.h>
#include "btHfFluid.h"
#include "btHfFluidCollisionShape.h"
#include "btHfFluidBuoyantConvexShape.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "Demos/OpenGL/GLDebugDrawer.h"
								
btHfFluid::btHfFluid (btScalar gridCellWidth, int numNodesWidth, int numNodesLength)
{
	m_eta = NULL;
	m_temp = NULL;
	m_ground = NULL;
	m_height[0] = NULL;
	m_height[1] = NULL;
	m_u[0] = NULL;
	m_u[1] = NULL;
	m_v[0] = NULL;
	m_v[1] = NULL;
	m_r[0] = NULL;
	m_r[1] = NULL;
	m_flags = NULL;
	m_fillRatio = NULL;
	m_internalType = CO_HF_FLUID;
	m_heightIndex = 0;
	m_velocityIndex = 0;
	m_rIndex = 0;
	setGridDimensions (gridCellWidth, numNodesWidth, numNodesLength);

	btScalar maxHeight = 20.0;
	m_aabbMin = btVector3 (0.0, 0.0, 0.0);
	m_aabbMax = btVector3 (m_gridWidth, maxHeight, m_gridLength);

	setCollisionShape (new btHfFluidCollisionShape (this));

	m_globalVelocityU = btScalar(0.0f);
	m_globalVelocityV = btScalar(0.0f);
	m_gravity = btScalar(-10.0f);

	m_volumeDisplacementScale = btScalar(0.5f);
	m_horizontalVelocityScale = btScalar(0.5f);

	m_epsEta = btScalar(0.01f);
	m_epsHeight = btScalar(0.001f);
}

btHfFluid::~btHfFluid ()
{
	btCollisionShape* collisionShape = getCollisionShape ();
	delete collisionShape;
	btAlignedFree (m_temp);
	btAlignedFree (m_height[0]);
	btAlignedFree (m_height[1]);
	btAlignedFree (m_ground);
	btAlignedFree (m_eta);
	btAlignedFree (m_u[0]);
	btAlignedFree (m_u[1]);
	btAlignedFree (m_v[0]);
	btAlignedFree (m_v[1]);
	btAlignedFree (m_flags);
	btAlignedFree (m_fillRatio);
}

void btHfFluid::predictMotion(btScalar dt)
{
	transferDisplaced (dt);
	
	advectEta (dt);
	advectVelocityU (dt);
	advectVelocityV (dt);
	updateHeight (dt);
	computeFlagsAndFillRatio ();
	updateVelocity (dt);
	setReflectBoundaryLeft ();
	setReflectBoundaryRight ();
	setReflectBoundaryTop ();
	setReflectBoundaryBottom ();
	debugTests();
	//m_heightIndex = (m_heightIndex + 1) % 2;
	//m_velocityIndex = (m_velocityIndex + 1) % 2;
}

void btHfFluid::prep ()
{
	for (int i = 0; i < m_numNodesLength*m_numNodesWidth;i++)
	{
		m_height[0][i] = m_eta[i] + m_ground[i];
		m_height[1][i] = m_eta[i] + m_ground[i];
	}
	computeFlagsAndFillRatio ();
}

btScalar btHfFluid::widthPos (int i) const
{
	return m_gridCellWidth * i;
}

btScalar btHfFluid::lengthPos (int j) const
{
return m_gridCellWidth * j;
}

int btHfFluid::arrayIndex (int i, int j) const
{
	btAssert (i >= 0);
	btAssert (i < m_numNodesWidth);
	btAssert (j >= 0);
	btAssert (j < m_numNodesLength);
	int index = i + (j * m_numNodesWidth);
	return index;
}

int btHfFluid::arrayIndex (btScalar i, btScalar j) const
{
	int ii = (int)i; // truncate / floor
	int ij = (int)j; // truncate / floor

	return arrayIndex (ii, ij);
}

const btScalar* btHfFluid::getHeightArray () const
{
	return m_height[m_heightIndex];
}

const btScalar* btHfFluid::getGroundArray () const
{
	return m_ground;
}
const btScalar* btHfFluid::getEtaArray () const
{
	return m_eta;
}
const btScalar* btHfFluid::getVelocityUArray () const
{
	return m_u[m_velocityIndex];
}
const btScalar* btHfFluid::getVelocityVArray () const
{
	return m_v[m_velocityIndex];
}

const bool* btHfFluid::getFlagsArray () const
{
	return m_flags;
}

 btScalar* btHfFluid::getHeightArray ()
{
	return m_height[m_heightIndex];
}
 btScalar* btHfFluid::getGroundArray ()
{
	return m_ground;
}
 btScalar* btHfFluid::getEtaArray ()
{
	return m_eta;
}
 btScalar* btHfFluid::getVelocityUArray ()
{
	return m_u[m_velocityIndex];
}
 btScalar* btHfFluid::getVelocityVArray ()
{
	return m_v[m_velocityIndex];
}

bool* btHfFluid::getFlagsArray ()
{
	return m_flags;
}

void btHfFluid::setFluidHeight (int x, int y, btScalar height)
{
	int index = arrayIndex (x,y);
	setFluidHeight (index, height);
}

void btHfFluid::setFluidHeight (int index, btScalar height)
{
	m_eta[index] = height;
	m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
	m_flags[index] = true;
}

void btHfFluid::addFluidHeight (int x, int y, btScalar height)
{
	int index = arrayIndex (x,y);
	m_eta[index] += height;
	m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
	m_flags[index] = true;
}

void btHfFluid::getAabbForColumn (int i, int j, btVector3& aabbMin, btVector3& aabbMax)
{
	btVector3 com = getWorldTransform().getOrigin();
	int sw = arrayIndex (i, j);
	int se = arrayIndex (i+1, j);
	int nw = arrayIndex (i, j+1);
	int ne = arrayIndex (i+1, j+1);

	btScalar h = m_height[m_heightIndex][sw];
	btScalar g = m_ground[sw];

	aabbMin = btVector3(widthPos (i), g, lengthPos (j));
	aabbMax = btVector3(widthPos (i+1), h, lengthPos (j+1));
	aabbMin += com;
	aabbMax += com;
}

void btHfFluid::foreachGroundTriangle(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax)
{
	btVector3 verts[3];

	btScalar minX, minZ, maxX, maxZ;
	int startNodeX, startNodeZ, endNodeX, endNodeZ;

	minX = aabbMin.getX();
	minZ = aabbMin.getZ();
	maxX = aabbMax.getX();
	maxZ = aabbMax.getZ();

	startNodeX = (int)(minX * m_gridCellWidthInv);
	startNodeZ = (int)(minZ * m_gridCellWidthInv);

	endNodeX = (int)(maxX * m_gridCellWidthInv);
	endNodeZ = (int)(maxZ * m_gridCellWidthInv);

	endNodeX++;
	endNodeZ++;

	startNodeX = btMax (1, startNodeX);
	startNodeZ = btMax (1, startNodeZ);
	endNodeX = btMin (m_numNodesWidth-1, endNodeX);
	endNodeZ = btMin (m_numNodesLength-1, endNodeZ);

#ifdef __BRUTE__
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			// triangle 1
			verts[0] = btVector3(widthPos(i), m_ground[arrayIndex(i,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
			callback->processTriangle(verts,i,j);
			// triangle 2
			verts[0] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j+1)], lengthPos(j+1));
			callback->processTriangle(verts,i,j);
		}
	}
#else

	for (int i = startNodeX; i < endNodeX; i++)
	{
		for (int j = startNodeZ; j < endNodeZ; j++)
		{
			// triangle 1
			verts[0] = btVector3(widthPos(i), m_ground[arrayIndex(i,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
			callback->processTriangle(verts,i,j);
			// triangle 2
			verts[0] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_ground[arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_ground[arrayIndex(i+1,j+1)], lengthPos(j+1));
			callback->processTriangle(verts,i,j);
		}
	}
#endif
}

void btHfFluid::foreachFluidColumn (btHfFluidColumnCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax)
{
	btScalar minX, minZ, maxX, maxZ;
	int startNodeX, startNodeZ, endNodeX, endNodeZ;

	minX = aabbMin.getX();
	minZ = aabbMin.getZ();
	maxX = aabbMax.getX();
	maxZ = aabbMax.getZ();

	startNodeX = (int)(minX * m_gridCellWidthInv);
	startNodeZ = (int)(minZ * m_gridCellWidthInv);

	endNodeX = (int)(maxX * m_gridCellWidthInv);
	endNodeZ = (int)(maxZ * m_gridCellWidthInv);

	endNodeX++;
	endNodeZ++;

	startNodeX = btMax (1, startNodeX);
	startNodeZ = btMax (1, startNodeZ);
	endNodeX = btMin (m_numNodesWidth-2, endNodeX);
	endNodeZ = btMin (m_numNodesLength-2, endNodeZ);

	bool r;
	for (int i = startNodeX; i < endNodeX; i++)
	{
		for (int j = startNodeZ; j < endNodeZ; j++)
		{
			if (m_flags[arrayIndex (i, j)] == false)
				continue;

			r = callback->processColumn (this, i, j);
			if (!r)
				return;
		}
	}
}

void btHfFluid::foreachSurfaceTriangle (btTriangleCallback* callback, const btVector3& aabbMin, const btVector3& aabbMax)
{
	btVector3 verts[3];

	btScalar minX, minZ, maxX, maxZ;
	int startNodeX, startNodeZ, endNodeX, endNodeZ;

	minX = aabbMin.getX();
	minZ = aabbMin.getZ();
	maxX = aabbMax.getX();
	maxZ = aabbMax.getZ();

	startNodeX = (int)(minX * m_gridCellWidthInv);
	startNodeZ = (int)(minZ * m_gridCellWidthInv);

	endNodeX = (int)(maxX * m_gridCellWidthInv);
	endNodeZ = (int)(maxZ * m_gridCellWidthInv);

	endNodeX++;
	endNodeZ++;

	startNodeX = btMax (1, startNodeX);
	startNodeZ = btMax (1, startNodeZ);
	endNodeX = m_numNodesWidth-1;
	endNodeZ = m_numNodesLength-1;

	for (int i = startNodeX; i < endNodeX; i++)
	{
		for (int j = startNodeZ; j < endNodeZ; j++)
		{
			if (!m_flags[arrayIndex(i,j)])
				continue;
			// triangle 1
			verts[0] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_height[m_heightIndex][arrayIndex(i+1,j)], lengthPos(j));
			callback->processTriangle(verts,i,j);
			// triangle 2
			verts[0] = btVector3(widthPos(i+1), m_height[m_heightIndex][arrayIndex(i+1,j)], lengthPos(j));
			verts[1] = btVector3(widthPos(i), m_height[m_heightIndex][arrayIndex(i,j+1)], lengthPos(j+1));
			verts[2] = btVector3(widthPos(i+1), m_height[m_heightIndex][arrayIndex(i+1,j+1)], lengthPos(j+1));
			callback->processTriangle(verts,i,j);
		}
	}
}

void btHfFluid::setGridDimensions (btScalar gridCellWidth,
								   int numNodesWidth, int numNodesLength)
{
	m_gridWidth = gridCellWidth * numNodesWidth;
	m_gridLength = gridCellWidth * numNodesLength;
	m_gridCellWidth = gridCellWidth;
	m_numNodesWidth = numNodesWidth;
	m_numNodesLength = numNodesLength;
	m_gridCellWidthInv = btScalar(1.0) / gridCellWidth;

	allocateArrays ();
}

btScalar btHfFluid::bilinearInterpolate (const btScalar* array, btScalar iPos, btScalar jPos)
{
	int i = (int)iPos;
	int j = (int)jPos;

	btScalar iParam1 = iPos - btScalar(i);
	btScalar iParam0 = btScalar(1.0) - iParam1;

	btScalar jParam1 = jPos - btScalar(j);
	btScalar jParam0 = btScalar(1.0) - jParam1;

	btScalar SW = array[arrayIndex(i, j)];
	btScalar SE = array[arrayIndex(i+1, j)];
	btScalar NW = array[arrayIndex(i, j+1)];
	btScalar NE = array[arrayIndex(i+1, j+1)];

	btScalar a = jParam0 * SW + jParam1 * NW;
	btScalar b = jParam0 * SE + jParam1 * NE;
	return iParam0 * a + iParam1 * b;
}

btScalar btHfFluid::advect (const btScalar* array, btScalar i, btScalar j, btScalar di, btScalar dj,btScalar dt)
{
	// trace particle backwards in time
	btScalar srcI = i - di * dt * m_gridCellWidthInv;
	btScalar srcJ = j - dj * dt * m_gridCellWidthInv;

	// srcI and srcJ are indices into the array,
	// we need to clamp them to within the domain
	srcI = btMax (srcI, btScalar(0.0));
	srcI = btMin (srcI, btScalar(m_numNodesWidth-1));
	srcJ = btMax (srcJ, btScalar(0.0));
	srcJ = btMin (srcJ, btScalar(m_numNodesLength-1));

	return bilinearInterpolate (array, srcI, srcJ);
}

void btHfFluid::advectEta (btScalar dt)
{
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);

			if (!m_flags[index])
				continue;

			btScalar u = m_globalVelocityU;
			btScalar v = m_globalVelocityV;

			u += (m_u[m_velocityIndex][index]+m_u[m_velocityIndex][index+1]) * btScalar(0.5);
			v += (m_v[m_velocityIndex][index]+m_v[m_velocityIndex][index+m_numNodesWidth]) * btScalar(0.5);

			m_temp[index] = advect (m_eta, btScalar(i), btScalar(j), u, v, dt);
		}
	}
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);
			m_eta[index] = m_temp[index];
		}
	}
}

void btHfFluid::updateHeight (btScalar dt)
{
	for (int j = 1; j < m_numNodesLength-1; j++)
	{
		for (int i = 1; i < m_numNodesWidth-1; i++)
		{
			int index = arrayIndex (i, j);
			if (!m_flags[index])
			{
				m_height[m_heightIndex][index] = m_ground[index] + m_eta[index];
				continue;
			}
			btScalar deta = -btScalar(0.5f) * m_eta[index] * dt * m_gridCellWidthInv * ( (m_u[m_velocityIndex][index+1] - m_u[m_velocityIndex][index]) + (m_v[m_velocityIndex][index+m_numNodesWidth] - m_v[m_velocityIndex][index]));
			m_eta[index] += deta;
			m_height[m_heightIndex][index] = m_ground[index] + btMax(m_eta[index],btScalar(0.0f));
		}
	}
}

void btHfFluid::advectVelocityU (btScalar dt)
{
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);
			if (!m_flags[index])
			{
				continue;
			}
			btScalar u = m_globalVelocityU;
			btScalar v = m_globalVelocityV;

			u += m_u[m_velocityIndex][index];
			v += (m_v[m_velocityIndex][index]+m_v[m_velocityIndex][index+1]+m_v[m_velocityIndex][index+m_numNodesWidth]+m_v[m_velocityIndex][index+m_numNodesWidth+1]) * btScalar(0.25);

			m_temp[index] = advect (m_u[m_velocityIndex], btScalar(i), btScalar(j), u, v, dt);
		}
	}

	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);
			m_u[m_velocityIndex][index] = m_temp[index];
		}
	}
}

void btHfFluid::advectVelocityV (btScalar dt)
{
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);
			if (!m_flags[index])
			{
				continue;
			}
			btScalar u = m_globalVelocityU;
			btScalar v = m_globalVelocityV;

			u += (m_u[m_velocityIndex][index]+m_u[m_velocityIndex][index+1]+m_u[m_velocityIndex][index+m_numNodesWidth]+m_u[m_velocityIndex][index+m_numNodesWidth+1]) * btScalar(0.25);
			v += m_v[m_velocityIndex][index];
			

			m_temp[index] = advect (m_v[m_velocityIndex], btScalar(i), btScalar(j), u, v, dt);
		}
	}
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			int index = arrayIndex (i, j);
			m_v[m_velocityIndex][index] = m_temp[index];
		}
	}
}

void btHfFluid::addDisplaced (int i, int j, btScalar r)
{
	m_r[m_rIndex][arrayIndex(i,j)] += r;
}

void btHfFluid::transferDisplaced (btScalar dt)
{
	for (int i = 2; i < m_numNodesWidth - 2; i++)
	{
		for (int j = 2; j < m_numNodesLength - 2; j++)
		{
			btScalar deltaR = m_r[m_rIndex][arrayIndex(i,j)] - m_r[(m_rIndex+1)%2][arrayIndex(i,j)];
			// deltaR is in volume, but we want to change the height..
			deltaR = deltaR / (m_gridCellWidth * m_gridCellWidth);
			deltaR *= m_volumeDisplacementScale;
			btScalar qdeltaR = deltaR / btScalar(4.0f);
			m_eta[arrayIndex(i-1,j-1)] += qdeltaR;
			m_eta[arrayIndex(i-1,j+1)] += qdeltaR;
			m_eta[arrayIndex(i+1,j-1)] += qdeltaR;
			m_eta[arrayIndex(i+1,j+1)] += qdeltaR;
			m_eta[arrayIndex(i,j)] -= deltaR;
			// OPTIMIZATION: zero out next frames r value
			m_r[(m_rIndex+1)%2][arrayIndex(i,j)] = btScalar(0.0);
		}
	}
	m_rIndex = (m_rIndex + 1) % 2; // flip frame
}

void btHfFluid::updateVelocity (btScalar dt)
{
	for (int j = 1; j < m_numNodesLength-1; j++)
	{
		for (int i = 2; i < m_numNodesWidth-1; i++)
		{
			int index = arrayIndex (i, j);
			if (!m_flags[index])
			{
				continue;
			}
			m_u[m_velocityIndex][index] += m_gravity * dt * m_gridCellWidthInv * (m_height[m_heightIndex][index]-m_height[m_heightIndex][index-1]);
		}
	}

	for (int j = 2; j < m_numNodesLength-1; j++)
	{
		for (int i = 1; i < m_numNodesWidth-1; i++)
		{
			int index = arrayIndex (i, j);
			if (!m_flags[index])
			{
				continue;
			}
			m_v[m_velocityIndex][index] += m_gravity * dt * m_gridCellWidthInv * (m_height[m_heightIndex][index]-m_height[m_heightIndex][index-m_numNodesWidth]);
		}
	}
}

void btHfFluid::setReflectBoundaryLeft ()
{
	for (int j = 0; j < m_numNodesLength; j++)
	{
		int indexL = arrayIndex (0, j);

		m_height[m_heightIndex][indexL] = m_height[m_heightIndex][indexL+1];
		m_u[m_velocityIndex][indexL+1] = btScalar(0.0);
		m_v[m_velocityIndex][indexL] = btScalar(0.0);
	}
}

void btHfFluid::setReflectBoundaryRight ()
{
	for (int j = 0; j < m_numNodesLength; j++)
	{
		int indexR = arrayIndex (m_numNodesWidth-1, j);

		m_height[m_heightIndex][indexR] = m_height[m_heightIndex][indexR-1];
		m_u[m_velocityIndex][indexR-1] = btScalar(0.0);
		m_v[m_velocityIndex][indexR] = btScalar(0.0);
	}
}

void btHfFluid::setReflectBoundaryBottom ()
{
	for (int i = 0; i < m_numNodesWidth; i++)
	{
		int indexT = arrayIndex (i, 0);
		
		m_height[m_heightIndex][indexT] = m_height[m_heightIndex][indexT+m_numNodesWidth];
		m_v[m_velocityIndex][indexT+m_numNodesWidth] = btScalar(0.0);
		m_u[m_velocityIndex][indexT] = btScalar(0.0);
	}
}

void btHfFluid::setReflectBoundaryTop ()
{
	for (int i = 0; i < m_numNodesWidth; i++)
	{
		int indexB = arrayIndex (i, m_numNodesLength-1);

		m_height[m_heightIndex][indexB] = m_height[m_heightIndex][indexB-m_numNodesWidth];
		m_v[m_velocityIndex][indexB-m_numNodesWidth] = btScalar(0.0);
		m_u[m_velocityIndex][indexB] = btScalar(0.0);
	}
}

void btHfFluid::setAbsorbBoundaryLeft (btScalar dt)
{
	for (int j = 0; j < m_numNodesLength; j++)
	{
		int indexL = arrayIndex (0, j);

		btScalar c = btSqrt(m_eta[indexL+1]*m_gravity);
		m_height[m_heightIndex][indexL] = ((m_gridCellWidthInv * m_height[(m_heightIndex+1)%2][indexL+1])+(dt*c*m_height[m_heightIndex][indexL+1]))/(m_gridCellWidthInv + dt * c);
		m_u[m_velocityIndex][indexL+1] = btScalar(0.0f);
		m_v[m_velocityIndex][indexL+1] = btScalar(0.0);
	}
}

void btHfFluid::setAbsorbBoundaryRight (btScalar dt)
{
}

void btHfFluid::setAbsorbBoundaryTop (btScalar dt)
{
}

void btHfFluid::setAbsorbBoundaryBottom (btScalar dt)
{
}

void btHfFluid::computeFlagsAndFillRatio ()
{
	for (int i = 1; i < m_numNodesWidth-1; i++)
	{
		for (int j = 1; j < m_numNodesLength-1; j++)
		{
			btScalar h = m_height[m_heightIndex][arrayIndex(i,j)];
			btScalar hMin = computeHmin(i,j);
			btScalar hMax = computeHmax(i,j);
			btScalar etaMax = computeEtaMax(i,j);
			if (h <= hMin && etaMax < m_epsEta)
			{
				m_flags[arrayIndex(i,j)] = false;
				m_fillRatio[arrayIndex(i,j)] = btScalar(0.0f);
			} else if (h > hMax) {
				m_flags[arrayIndex(i,j)] = true;
				m_fillRatio[arrayIndex(i,j)] = btScalar(1.0f);
			} else {
				m_flags[arrayIndex(i,j)] = true;
				m_fillRatio[arrayIndex(i,j)] = (h - hMin)/(hMax - hMin);
			}
			
		}
	}
}

btScalar btHfFluid::computeHmin (int i, int j)
{
	btAssert (i > 0);
	btAssert (i < m_numNodesWidth-1);
	btAssert (j > 0);
	btAssert (j < m_numNodesLength-1);

	btScalar h1 = m_height[m_heightIndex][arrayIndex(i-1,j-1)];
	btScalar h2 = m_height[m_heightIndex][arrayIndex(i-1,j+1)];
	btScalar h3 = m_height[m_heightIndex][arrayIndex(i+1,j-1)];
	btScalar h4 = m_height[m_heightIndex][arrayIndex(i+1,j+1)];
	btScalar h = m_height[m_heightIndex][arrayIndex(i,j)];
	btScalar minh = btMin(h1, btMin(h2, btMin(h3,h4)));

	return (minh + h) * btScalar(0.5f);
}

btScalar btHfFluid::computeHmax (int i, int j)
{
	btAssert (i > 0);
	btAssert (i < m_numNodesWidth-1);
	btAssert (j > 0);
	btAssert (j < m_numNodesLength-1);

	btScalar h1 = m_height[m_heightIndex][arrayIndex(i-1,j-1)];
	btScalar h2 = m_height[m_heightIndex][arrayIndex(i-1,j+1)];
	btScalar h3 = m_height[m_heightIndex][arrayIndex(i+1,j-1)];
	btScalar h4 = m_height[m_heightIndex][arrayIndex(i+1,j+1)];
	btScalar h = m_height[m_heightIndex][arrayIndex(i,j)];
	btScalar maxh = btMax(h1, btMax(h2, btMax(h3,h4)));

	return (maxh + h) * btScalar(0.5f) + m_epsHeight;
}

btScalar btHfFluid::computeEtaMax (int i, int j)
{
	btAssert (i > 0);
	btAssert (i < m_numNodesWidth-1);
	btAssert (j > 0);
	btAssert (j < m_numNodesLength-1);

	btScalar eta1 = m_eta[arrayIndex(i-1,j-1)];
	btScalar eta2 = m_eta[arrayIndex(i-1,j+1)];
	btScalar eta3 = m_eta[arrayIndex(i+1,j-1)];
	btScalar eta4 = m_eta[arrayIndex(i+1,j+1)];
	btScalar eta = m_eta[arrayIndex(i,j)];
	btScalar maxeta = btMax(eta1, btMax(eta2, btMax(eta3,eta4)));

	return (maxeta + eta) * btScalar(0.5f);
}

void btHfFluid::allocateArrays ()
{
	if (m_temp)
		btAlignedFree (m_temp);
	if (m_height[0])
	{
		btAlignedFree (m_height[0]);
		btAlignedFree (m_height[1]);
	}
	if (m_ground)
		btAlignedFree (m_ground);
	if (m_eta)
		btAlignedFree (m_eta);
	if (m_u[0])
	{
		btAlignedFree (m_u[0]);
		btAlignedFree (m_u[1]);
	}
	if (m_v)
	{
		btAlignedFree (m_v[0]);
		btAlignedFree (m_v[1]);
	}
	if (m_r)
	{
		btAlignedFree (m_r[0]);
		btAlignedFree (m_r[1]);
	}
	if (m_flags)
		btAlignedFree (m_flags);
	if (m_fillRatio)
		btAlignedFree (m_fillRatio);

	m_heightIndex = 0;
	m_velocityIndex = 0;
	m_temp = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_height[0] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_height[1] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_ground = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_eta = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_u[0] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_u[1] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_v[0] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_v[1] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_r[0] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_r[1] = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_fillRatio = (btScalar*)btAlignedAlloc (sizeof(btScalar) * m_numNodesWidth * m_numNodesLength, 16);
	m_flags = (bool*)btAlignedAlloc (sizeof(bool) * m_numNodesWidth * m_numNodesLength, 16);

	{
		int bufferSize = sizeof(btScalar) * m_numNodesWidth * m_numNodesLength;
		printf("[HfFluid] Fluid buffer size %d bytes\n", bufferSize);
		printf("[HfFluid] Temp %d buffers\n", 1);
		printf("[HfFluid] Height %d buffers\n", 2);
		printf("[HfFluid] Velocity %d buffers\n", 4);
		printf("[HfFluid] Eta %d buffers\n", 1);
		printf("[HfFluid] Fill Ratio %d buffers\n", 1);
		printf("[HfFluid] ===============================\n");
		printf("[HfFluid] Total %d buffers\n", 9);
		printf("[HfFluid] Total %d KB\n", (9 * bufferSize)/1024);
	}
	for (int i = 0; i < m_numNodesWidth*m_numNodesLength; i++)
	{
		m_eta[i] = btScalar(0.0);
		m_u[0][i] = btScalar(0.0);
		m_u[1][i] = btScalar(0.0);
		m_v[0][i] = btScalar(0.0);
		m_v[1][i] = btScalar(0.0);
		m_r[0][i] = btScalar(0.0);
		m_r[1][i] = btScalar(0.0);
		m_height[0][i] = btScalar(0.0);
		m_height[1][i] = btScalar(0.0);
		m_ground[i] = btScalar(0.0);
		m_flags[i] = false;
		m_fillRatio[i] = btScalar(0.0);
		m_temp[i] = btScalar(0.0);
	}
}


void btHfFluid::debugTests ()
{
	static btScalar total_volume = btScalar(0.0f);
	btScalar new_total_volume = btScalar(0.0f);
	for (int i = 0; i < m_numNodesWidth*m_numNodesLength; i++)
	{
		new_total_volume += m_eta[i] * m_gridCellWidth * m_gridCellWidth;
	}
	printf("volume = %f volume delta = %f\n", new_total_volume, new_total_volume - total_volume);
	total_volume = new_total_volume;
}

// You can enforce a global velocity at the surface of the fluid
// default: 0.0 and 0.0
void btHfFluid::setGlobaVelocity (btScalar globalVelocityU, btScalar globalVelocityV)
{
	m_globalVelocityU = globalVelocityU;
	m_globalVelocityV = globalVelocityV;
}

void btHfFluid::getGlobalVelocity (btScalar& globalVelocityU, btScalar& globalVelocityV) const
{
	globalVelocityU = m_globalVelocityU;
	globalVelocityV = m_globalVelocityV;
}

// Control force of gravity, should match physics world
// default: -10.0
void btHfFluid::setGravity (btScalar gravity)
{
	m_gravity = gravity;
}
btScalar btHfFluid::getGravity () const
{
	return m_gravity;
}

// When a body is submerged into the fluid, the displaced fluid
// is spread to adjacent cells. You can control the percentage of this
// by setting this value between 0.0 and 1.0
// default: 0.5
void btHfFluid::setVolumeDisplacementScale (btScalar volumeDisplacementScale)
{
	m_volumeDisplacementScale = volumeDisplacementScale;
}

btScalar btHfFluid::getVolumeDisplacementScale () const
{
	return m_volumeDisplacementScale;
}

// The horizontal velocity of the fluid can influence bodies submerged
// in the fluid. You can control how much influence by setting this
// between 0.0 and 1.0
// default: 0.5
void btHfFluid::setHorizontalVelocityScale (btScalar horizontalVelocityScale)
{
	m_horizontalVelocityScale = horizontalVelocityScale;
}

btScalar btHfFluid::getHorizontalVelocityScale () const
{
	return m_horizontalVelocityScale;
}

static btScalar rangeOverlap (btScalar lo1, btScalar hi1, btScalar lo2, btScalar hi2, btScalar& loOut, btScalar& hiOut)
{
	if (!(lo1 <= hi2 && lo2 <= hi1))
		return btScalar(0.0f);

	if (lo1 >= lo2 && lo1 <= hi2 &&
		hi1 >= lo2 && hi1 <= hi2)
	{
		hiOut = hi1;
		loOut = lo1;
		return hi1 - lo1;
	} else if (lo2 >= lo1 && lo2 <= hi1 &&
			   hi2 >= lo1 && hi2 <= hi1)
	{
		hiOut = hi2;
		loOut = lo2;
		return hi2 - lo2;
	} else if (hi1 >= lo2 && lo1 <= lo2) {
		hiOut = hi1;
		loOut = lo2;
		return hi1 - lo2;
	} else {
		hiOut = hi2;
		loOut = lo1;
		return hi2 - lo1;
	}
}

btHfFluidColumnRigidBodyCallback::btHfFluidColumnRigidBodyCallback (btRigidBody* rigidBody, btIDebugDraw* debugDraw, btScalar density, btScalar floatyness)
{
	m_rigidBody = rigidBody;
	m_buoyantShape = (btHfFluidBuoyantConvexShape*)rigidBody->getCollisionShape();
	m_debugDraw = debugDraw;
	m_rigidBody->getAabb (m_aabbMin, m_aabbMax);
	m_volume = btScalar(0.0f);
	m_density = density;
	m_floatyness = floatyness;
	m_numVoxels = m_buoyantShape->getNumVoxels ();
	for (int i = 0; i < m_numVoxels; i++)
	{
		btVector3 p = m_buoyantShape->getVoxelPositionsArray()[i];
		p = m_rigidBody->getWorldTransform().getBasis() * p;
		p += m_rigidBody->getWorldTransform().getOrigin();
		m_voxelPositionsXformed[i] = p;
		m_voxelSubmerged[i] = false;
	}
}

static bool sphereVsAABB (const btVector3& aabbMin, const btVector3& aabbMax, const btVector3& sphereCenter, const btScalar sphereRadius)
{
  btScalar totalDistance = 0;

  // Accumulate the distance of the sphere's center on each axis
  for(int i = 0; i < 3; ++i) {

    // If the sphere's center is outside the aabb, we've got distance on this axis
    if(sphereCenter[i] < aabbMin[i]) {
      btScalar borderDistance = aabbMin[i] - sphereCenter[i];
      totalDistance += borderDistance * borderDistance;

    } else if(sphereCenter[i] > aabbMax[i]) {
      btScalar borderDistance = sphereCenter[i] - aabbMax[i];
      totalDistance += borderDistance * borderDistance;

    }
    // Otherwise the sphere's center is within the box on this axis, so the
    // distance will be 0 and we do not need to accumulate anything at all

  }

  // If the distance to the box is lower than the sphere's radius, both are overlapping
  return (totalDistance <= (sphereRadius * sphereRadius));
}

bool btHfFluidColumnRigidBodyCallback::processColumn (btHfFluid* fluid, int w, int l)
{
	btVector3 columnAabbMin,columnAabbMax;

	fluid->getAabbForColumn (w, l, columnAabbMin, columnAabbMax);

	bool applyBuoyancyImpulse = true;
	bool applyFluidVelocityImpulse = true;
	bool applyFluidDisplace = true;

	btScalar dt = btScalar(1.0f/60.0f);

	btScalar columnVolume = btScalar(0.0f);

	for (int i = 0; i < m_buoyantShape->getNumVoxels(); i++)
	{
		if (m_voxelSubmerged[i])
			continue;

		if (sphereVsAABB(columnAabbMin, columnAabbMax, m_voxelPositionsXformed[i], m_buoyantShape->getVoxelRadius()))
		{
			m_voxelSubmerged[i] = true;
			btScalar voxelVolume = m_buoyantShape->getVolumePerVoxel();
			columnVolume += voxelVolume;

			btVector3 application_point = m_voxelPositionsXformed[i];
			btVector3 relative_position = application_point - m_rigidBody->getCenterOfMassPosition();

			if (applyBuoyancyImpulse)
			{
				btScalar massDisplacedWater = voxelVolume * m_density * m_floatyness;
				btScalar force = massDisplacedWater * -fluid->getGravity();
				btScalar impulseMag = force * dt;
				btVector3 impulseVec = btVector3(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f)) * impulseMag;
//#define CENTER_IMPULSE_ONLY 1
#ifdef CENTER_IMPULSE_ONLY
				m_rigidBody->applyCentralImpulse (impulseVec);
#else
				m_rigidBody->applyImpulse (impulseVec, relative_position);
#endif
			}
		}
	}

	if (columnVolume > btScalar(0.0))
	{
		m_volume += columnVolume;

		if (applyFluidDisplace)
		{
			fluid->addDisplaced (w, l, columnVolume);
		}

		if (applyFluidVelocityImpulse)
		{
			int index = fluid->arrayIndex (w,l);
			btScalar u = fluid->getVelocityUArray()[index];
			btScalar v = fluid->getVelocityVArray()[index];
			btVector3 vd = btVector3(u, btScalar(0.0f), v);
			btVector3 impulse = vd * dt * fluid->getHorizontalVelocityScale();
			m_rigidBody->applyCentralImpulse (impulse);
		}
	}

	return true;
}