// dHeightfield Collider
//  Martijn Buijs 2006 http://home.planet.nl/~buijs512/
// Based on Terrain & Cone contrib by:
//  Benoit CHAPEROT 2003-2004 http://www.jstarlab.com

#ifndef _DHEIGHTFIELD_H_
#define _DHEIGHTFIELD_H_
//------------------------------------------------------------------------------

#include <ode/common.h>
#include "collision_kernel.h"


#define HEIGHTFIELDMAXCONTACTPERCELL 10
// Bit flags for edge checking
#define HEIGHTFIELD_CHECKED_EDGE_01		0x01	// Edge vertex[0] to vertex[1]
#define HEIGHTFIELD_CHECKED_EDGE_12		0x02	// Edge vertex[1] to vertex[2]
#define HEIGHTFIELD_CHECKED_EDGE_20		0x04	// Edge vertex[2] to vertex[0]
#define HEIGHTFIELD_CHECKED_EDGE_ALL	(HEIGHTFIELD_CHECKED_EDGE_01 | HEIGHTFIELD_CHECKED_EDGE_12 | HEIGHTFIELD_CHECKED_EDGE_20)	// All three edges

class HeightFieldVertex;
class HeightFieldEdge;
class HeightFieldTriangle;

//
// dxHeightfieldData
//
// Heightfield Data structure
//
struct dxHeightfieldData
{
    dReal m_fWidth;				// World space heightfield dimension on X axis
    dReal m_fDepth;				// World space heightfield dimension on Z axis
    dReal m_fSampleWidth;		// Vertex spacing on X axis edge (== m_vWidth / (m_nWidthSamples-1))
    dReal m_fSampleDepth;		// Vertex spacing on Z axis edge (== m_vDepth / (m_nDepthSamples-1))
    dReal m_fSampleZXAspect;    // Relation of Z axis spacing to X axis spacing (== m_fSampleDepth / m_fSampleWidth)
    dReal m_fInvSampleWidth;		// Cache of inverse Vertex count on X axis edge (== m_vWidth / (m_nWidthSamples-1))
    dReal m_fInvSampleDepth;		// Cache of inverse Vertex count on Z axis edge (== m_vDepth / (m_nDepthSamples-1))

    dReal m_fHalfWidth;			// Cache of half of m_fWidth
    dReal m_fHalfDepth;			// Cache of half of m_fDepth

    dReal m_fMinHeight;        // Min sample height value (scaled and offset)
    dReal m_fMaxHeight;        // Max sample height value (scaled and offset)
    dReal m_fThickness;        // Surface thickness (added to bottom AABB)
    dReal m_fScale;            // Sample value multiplier
    dReal m_fOffset;           // Vertical sample offset

    int	m_nWidthSamples;       // Vertex count on X axis edge (number of samples)
    int	m_nDepthSamples;       // Vertex count on Z axis edge (number of samples)
    int m_bCopyHeightData;     // Do we own the sample data?
    int	m_bWrapMode;           // Heightfield wrapping mode (0=finite, 1=infinite)
    int m_nGetHeightMode;      // GetHeight mode ( 0=callback, 1=byte, 2=short, 3=float )

    const void* m_pHeightData; // Sample data array
    void* m_pUserData;         // Callback user data

    dContactGeom            m_contacts[HEIGHTFIELDMAXCONTACTPERCELL];

    dHeightfieldGetHeight* m_pGetHeightCallback;		// Callback pointer.

    dxHeightfieldData();
    ~dxHeightfieldData();

    void SetData( int nWidthSamples, int nDepthSamples,
        dReal fWidth, dReal fDepth,
        dReal fScale, dReal fOffset,
        dReal fThickness, int bWrapMode );

    void ComputeHeightBounds();

    bool IsOnHeightfield2  ( HeightFieldTriangle * tri,
        const dReal * const pos, char * checkedEdges) const;

    dReal GetHeight(int x, int z);
    dReal GetHeight(dReal x, dReal z);

};

typedef int HeightFieldVertexCoords[2];

class HeightFieldVertex
{
public:
    HeightFieldVertex():
    	edgecheck(0)
    {};

    dVector3 vertex;
    HeightFieldVertexCoords coords;
    bool state;

    // bit-code to indicate whether edges are already checked for collisions or not
    // edgecheck is a bitwise OR of up to 3 codes:
    // 1) EDGE_PX	(positive-X edge): (coords[0], coords[1]) -> (coords[0]+1, coords[1])
    // 2) EDGE_PXNZ	(posX-negZ  edge): (coords[0], coords[1]) -> (coords[0]+1, coords[1]-1)
    // 3) EDGE_NZ	(negative-Z edge): (coords[0], coords[1]) -> (coords[0], coords[1]-1)
    static const int EDGE_PX	= 0x01;		// Positive-X edge
    static const int EDGE_PXNZ	= 0x02;	// PosX-NegZ-edge
    static const int EDGE_NZ	= 0x04;		// Negative-Z edge
    char			edgecheck;
    inline void	setEdgeChecked(const char edge)		{ edgecheck |= edge;}
    inline void	resetEdgeChecked(const char edge)	{ edgecheck &= ~edge;}
    inline bool	isEdgeChecked(const char edge)		{ return edgecheck & edge;}
};

class HeightFieldEdge
{
public:
    HeightFieldEdge(){};

    HeightFieldVertex   *vertices[2];
};

class HeightFieldTriangle
{
public:
    HeightFieldTriangle(){};

    inline void setMinMax()
    {
        maxAAAB = vertices[0]->vertex[1] > vertices[1]->vertex[1] ? vertices[0]->vertex[1] : vertices[1]->vertex[1];
        maxAAAB = vertices[2]->vertex[1] > maxAAAB  ? vertices[2]->vertex[1] : maxAAAB;
    };

    HeightFieldVertex   *vertices[3];
    dReal               planeDef[4];
    dReal               maxAAAB;

    bool                isABC;	// If not ABC, then DBC
    bool                state;

    inline bool			isChecked01() const	// Checked edge between vertex0 and vertex1 for collisions?
    {
    	if (isABC)
    		// Edge AB
    		return vertices[0]->isEdgeChecked(HeightFieldVertex::EDGE_PX);
    	else // DBC
    		// Edge DB
    		return vertices[0]->isEdgeChecked(HeightFieldVertex::EDGE_NZ);
    }
    inline void			setChecked01()
    {
    	if (isABC)
    		vertices[0]->setEdgeChecked(HeightFieldVertex::EDGE_PX);
    	else
    		vertices[0]->setEdgeChecked(HeightFieldVertex::EDGE_NZ);
    }

    inline  bool		isChecked12() const	// Checked edge between vertex1 and vertex2 for collisions?
    {
    	if (isABC)
    		// Edge BC
    		return vertices[2]->isEdgeChecked(HeightFieldVertex::EDGE_PXNZ);
    	else // DBC
    		// Edge BC
    		return vertices[2]->isEdgeChecked(HeightFieldVertex::EDGE_PXNZ);
    }
    inline void			setChecked12()
    {
    	if (isABC)
    		vertices[2]->setEdgeChecked(HeightFieldVertex::EDGE_PXNZ);
    	else
    		vertices[2]->setEdgeChecked(HeightFieldVertex::EDGE_PXNZ);
    }

    inline bool			isChecked20() const	// Checked edge between vertex2 and vertex0 for collisions?
    {
    	if (isABC)
    		// Edge CA
    		return vertices[2]->isEdgeChecked(HeightFieldVertex::EDGE_NZ);
    	else // DBC
    		// Edge CD
    		return vertices[2]->isEdgeChecked(HeightFieldVertex::EDGE_PX);
    }
    inline void			setChecked20()
    {
    	if (isABC)
    		vertices[2]->setEdgeChecked(HeightFieldVertex::EDGE_NZ);
    	else
    		vertices[2]->setEdgeChecked(HeightFieldVertex::EDGE_PX);
    }

    // isChecked(int m) and setChecked(int m) checking for vertex[m] and vertex[m+1%3]
    inline bool			isChecked(const int m) const
    {
    	switch (m)
    	{
			case 0: return isChecked01();
			case 1: return isChecked12();
			case 2: return isChecked20();
			default: return false;
    	}
    }
    inline void			setChecked(const int m)
    {
    	switch (m)
    	{
			case 0: setChecked01(); break;
			case 1: setChecked12(); break;
			case 2: setChecked20(); break;
			default: break;
    	}
    }
    // Set all edge states to checked
    inline void			setEdgesChecked()
    {
		setChecked01();
		setChecked12();
		setChecked20();
    }
    inline void			resetEdgesChecked()
    {
    	// Don't reset all edges; vertices are often shared between different triangles.
    	if (isABC)
    	{
    		vertices[0]->resetEdgeChecked(HeightFieldVertex::EDGE_PX);	// 01
    		vertices[2]->resetEdgeChecked(HeightFieldVertex::EDGE_PXNZ);// 12
    		vertices[2]->resetEdgeChecked(HeightFieldVertex::EDGE_NZ);	// 20
    	}
    	else
    	{
    		vertices[0]->resetEdgeChecked(HeightFieldVertex::EDGE_NZ);	// 01
    		vertices[2]->resetEdgeChecked(HeightFieldVertex::EDGE_PXNZ);// 12
    		vertices[2]->resetEdgeChecked(HeightFieldVertex::EDGE_PX);	// 20
    	}
    }
};

class HeightFieldPlane
{
public:
    HeightFieldPlane():
      trianglelist(0),
      trianglelistReservedSize(0),
      trianglelistCurrentSize(0)
    {

    };
    ~HeightFieldPlane()
    {
        delete [] trianglelist;
    };

    inline void setMinMax()
    {
        const size_t asize = trianglelistCurrentSize;
        if (asize > 0)
        {  
            maxAAAB = trianglelist[0]->maxAAAB;
            for (size_t k = 1; asize > k; k++)
            {   
                if (trianglelist[k]->maxAAAB >  maxAAAB)
                    maxAAAB = trianglelist[k]->maxAAAB;
            }
        }
    };

    void resetTriangleListSize(const size_t newSize)
    {
        if (trianglelistReservedSize < newSize)
        {
            delete [] trianglelist;
            trianglelistReservedSize = newSize;
            trianglelist = new HeightFieldTriangle *[newSize];
        }
        trianglelistCurrentSize = 0;
    }

    void addTriangle(HeightFieldTriangle *tri)
    {
		dIASSERT(trianglelistCurrentSize < trianglelistReservedSize);

        trianglelist[trianglelistCurrentSize++] = tri;
    }

    HeightFieldTriangle **trianglelist;
    size_t              trianglelistReservedSize;
    size_t              trianglelistCurrentSize;

    dReal   maxAAAB;
    dReal   planeDef[4];
};

//
// dxHeightfield
//
// Heightfield geom structure
//
struct dxHeightfield : public dxGeom
{
    dxHeightfieldData* m_p_data;

    dxHeightfield( dSpaceID space, dHeightfieldDataID data, int bPlaceable );
    ~dxHeightfield();

    void computeAABB();

    int dCollideHeightfieldZone( const int minX, const int maxX, const int minZ, const int maxZ,  
        dxGeom *o2, const int numMaxContacts,
        int flags, dContactGeom *contact, int skip );

	enum
	{
		TEMP_PLANE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 4,
		TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_X = 4,
		TEMP_HEIGHT_BUFFER_ELEMENT_COUNT_ALIGNMENT_Z = 4,
		TEMP_TRIANGLE_BUFFER_ELEMENT_COUNT_ALIGNMENT = 1, // Triangles are easy to reallocate and hard to predict
	};

	static inline size_t AlignBufferSize(size_t value, size_t alignment) { dIASSERT((alignment & (alignment - 1)) == 0); return (value + (alignment - 1)) & ~(alignment - 1); }

	void  allocateTriangleBuffer(size_t numTri);
	void  resetTriangleBuffer();
	void  allocatePlaneBuffer(size_t numTri);
	void  resetPlaneBuffer();
	void  allocateHeightBuffer(size_t numX, size_t numZ);
    void  resetHeightBuffer();

    void  sortPlanes(const size_t numPlanes);

    HeightFieldPlane    **tempPlaneBuffer;
    HeightFieldPlane    *tempPlaneInstances;
    size_t              tempPlaneBufferSize;

    HeightFieldTriangle *tempTriangleBuffer;
    size_t              tempTriangleBufferSize;

    HeightFieldVertex   **tempHeightBuffer;
	HeightFieldVertex   *tempHeightInstances;
    size_t              tempHeightBufferSizeX;
    size_t              tempHeightBufferSizeZ;

};


//------------------------------------------------------------------------------
#endif //_DHEIGHTFIELD_H_