/* $Id: HFQuadtree.cpp,v 1.24 2003/03/26 17:07:17 zongo Exp $
**
** Ark - Libraries, Tools & Programs for MMORPG developpements.
** Copyright (C) 1999-2003 The Contributors of the Ark Project
** Please see the file "AUTHORS" for a list of contributors
**
** 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.
**
** 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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <algorithm>
#include <math.h>

#ifdef WIN32
 #include <windows.h>
#endif

#include <GL/gl.h>

#include <Ark/ArkSystem.h>
#include <Ark/ArkRenderer.h>
#include <Modules/HeightField/HFWorld.h>
#include <Modules/HeightField/HFQuadtree.h>

static GLfloat g_fogBlackColor[4] = {0., 0., 0., 1.};


namespace Ark
{
    // List of patch to render
    typedef std::vector<Patch*> PatchList;
    typedef std::vector<Patch*>::iterator PatchListIterator;
    
   // =======================================================================
   // Quadtree patch.
   // =======================================================================

   /* The terrain is meshed as follows:
            ...
         +-+-+-+-+            x<---+
         |\|/|\|/|                 |
      ...+-+-+-+-+...              V
         |/|\|/|\|                 z
         +-+-+-+-+
            ...
      
      So there are two types of vertices: those surrounded by
      four triangles (x+z is odd), and those surrounded by
      eight (x+z is even).
   */
   class Patch : public QuadtreeNode
   {	    
       friend class QuadtreeRenderManager;
       
       // I want to use a vector instead of a list for QuadtreeBlockList..
       typedef std::vector<PrimitiveBlock> QuadtreeBlockList;

       // we do not own these members
       HeightField* m_Heightfield;
       QuadtreeRenderManager* m_Manager;

       // we manage these data
       VertexBuffer m_Triangles;
       VertexBuffer m_BlendedTriangles;
       
       QuadtreeBlockList m_TriangleIndices;
       QuadtreeBlockList m_BlendedTriangleIndices;
       PrimitiveBlock m_BlackTriangleIndices;

       // Material used in the m_Materials of QuadtreeRenderManager
       std::vector<int> m_MaterialsUsed;

       // This this the number of vertices in m_BlendedTriangles
       int m_BlendedTriangleCount;

      protected:
	 EntityList m_Entities;
	 
      public:
	 // Size of the patch
	 enum { SIZE=16 };

	 // I do not want them to be recomputed every frame
	 int m_MinX, m_MinZ;

	 /// offset from (0,0,0) to the patch lower corner
	 Vector3 m_Offset;

	 /// Center point of the patch
	 Vector3 m_CenterPoint;

	 bool m_Dirty;
	 bool m_CollisionDirty;

      public:
	 Patch() : 
	     QuadtreeNode(),
	     m_Heightfield(0),
	     m_Manager(0),
	     m_Dirty (true),
	     m_CollisionDirty (true)
	   {}

	 virtual ~Patch()
	 {
	     // Cleans the HF and manager
	     m_Heightfield = 0;
	     m_Manager = 0;
	 }

	 virtual void Build(HeightField* hf, 
		 int minX, int minZ, int size,
		 QuadtreeRenderManager* manager)
	 {
	     assert(size == SIZE);
	     assert(hf);
	     assert(manager);

	     // Sets basic data
	     m_Heightfield = hf;
	     m_Manager = manager;
	     m_MinX = minX;
	     m_MinZ = minZ;

	     // Sets patch offset
	     m_Offset = hf->GetCoord(minX, minZ);

	     m_Triangles.SetFormat(VertexBuffer::VB_HAS_COORD|VertexBuffer::VB_HAS_NORMAL|VertexBuffer::VB_HAS_COLOR);
	     m_Triangles.Resize((SIZE+1) * (SIZE+1));

	     Validate();
	 }

	 
	 // This function is declared after QuadtreeRenderManager
	 void RenderPatch();
	 
	 void Validate()
	 {
	     const int maxX = m_MinX + SIZE;
	     const int maxZ = m_MinZ + SIZE;
	     const int sx = (int) m_Heightfield->m_SizeX - 1;
	     const int sz = (int) m_Heightfield->m_SizeZ - 1;

	     const int midX = (m_MinX + ((sx < maxX) ? sx : maxX)) / 2;
	     const int midZ = (m_MinZ + ((sx < maxZ) ? sx : maxZ)) / 2;
	     m_CenterPoint = m_Heightfield->GetCoord(midX, midZ);

	     int idx = 0;
	     for (int z = m_MinZ ; z <= maxZ ; z++)
	     {
		 for (int x = m_MinX ; x <= maxX ; x++)
		 {
		     const int rx = (sx < x) ? sx : x;
		     const int rz = (sz < z) ? sz : z;

		     Vector3 point = m_Heightfield->GetCoord(rx, rz);

		     // Adds point to bounding box
		     m_BBox.AddPoint(point);

		     m_Triangles.Coord(idx) = point;
		     m_Triangles.Normal(idx) = ComputeNormal (rx, rz, m_Triangles.Coord(idx));
		     Material *grd = m_Heightfield->GetGrd (rx, rz);
		     RGBA &rgba = m_Triangles.Color4(idx);

		     if (grd)
		     {
			 rgba.R = uchar (grd->m_Diffuse.R * 255.f);
			 rgba.G = uchar (grd->m_Diffuse.G * 255.f);
			 rgba.B = uchar (grd->m_Diffuse.B * 255.f);
			 rgba.A = 255;
		     }
		     else
		     {
			 rgba.R = rgba.G = rgba.B = rgba.A = 255;
		     }

		     ++idx;
		 }
	     }

	     m_Dirty = false;
	     // std::cerr << "Patch BB is " << m_BBox.m_Min << " " << m_BBox.m_Max << std::endl;

	     BuildTriangleIndices();
	 }
	 
	 /**
	  * Returns the ground material in the heightfield using patch coordinates.
	  *
	  * (0, 0) is the corner of the patch, not of the heightfield
	  */
	 uchar Ground(int x, int z) const
	 {
	     const int hx = x + m_MinX;
	     const int hz = z + m_MinZ;

	     const int nx = m_Heightfield->m_SizeX - 1;
	     const int nz = m_Heightfield->m_SizeZ - 1;

	     const int mx = (nx<hx) ? nx : hx;
	     const int mz = (nz<hz) ? nz : hz;

	     return m_Heightfield->GetGround(mx, mz);
	 }


	 /// Add an entity to this patch..
	 void AddEntity (Entity *ent)
	 {
	     m_CollisionDirty = true;

	     EntityLI iterator = std::find(m_Entities.begin(), m_Entities.end(), ent);

	     if (iterator == m_Entities.end())
	     {
		 m_Entities.push_back(ent);
	     }

	 }
  

	 /// Remove an entity from this patch..
	 void RemoveEntity (Entity *ent)
	 {
	     m_CollisionDirty = true;

	     // Remove entity from list
	     m_Entities.erase(std::remove(m_Entities.begin(), m_Entities.end(), ent), m_Entities.end());
	 }

	 

	 virtual void InvalidatePatches(const BBox& box)
	 {
	     if (m_BBox.Overlap(box))
	     {
		 m_Dirty = true;
	     }
	 }


	 /**
	  * Add all the patches entities to the provided list, checking that no
	  * entity is added twice in the list.
	  */
	 void AddPatchEntsToList(EntityList &list)
	 {

	     for (EntityLI i = m_Entities.begin(); i != m_Entities.end(); ++i)
	     {
		 EntityLI found = std::find(list.begin(), list.end(), *i);

		 // Add *i to list if not already found
		 if (found == list.end())
		 {
		     list.push_back (*i);
		 }
	     }      
	 }



	 void RayTrace (const Ray &ray, std::vector<int>& potentialquads)
	 {
	     // Needed for boundary checks in patch space
	     const Vector3& bbmin = m_BBox.m_Min;
	     const scalar size = static_cast<scalar>(SIZE);

	     Vector3 v0 = m_Heightfield->GetVectorInGridSpace( ray.m_From ); 
	     Vector3 v1 = m_Heightfield->GetVectorInGridSpace( ray.m_To );

	     scalar p0[2] = { v0.X - bbmin.X, v0.Z - bbmin.Z };
	     scalar p1[2] = { v1.X - bbmin.X, v1.Z - bbmin.Z };

	     // Corner for the quad
	     // -1 if not found
	     int cornerIndex = -1; 

	     // Flipping of axis
	     bool transform[2] = { false, false }; 
	     int which = 0;

	     // Patch index moves
	     int index = 0;
	     int offset[2] = { 1, SIZE + 1 };

	     // Horizontal shift
	     if (p1[0] < p0[0])
	     {
		 transform[0] = true;
		 p0[0] = size - p0[0];
		 p1[0] = size - p1[0];
		 index += SIZE;
		 offset[0] = -offset[0];
	     }

	     // Vertical shift
	     if (p1[1] < p0[1])
	     {
		 transform[1] = true;
		 p0[1] = size - p0[1];
		 p1[1] = size - p1[1];
		 index += (SIZE + 1) * SIZE;
		 offset[1] = -offset[1];
	     }

	     // First bissectrice shift
	     if ((p1[0] - p0[0]) < (p1[1] - p0[1]))
	     {
		 which = 1;
	     }

	     const int other = 1-which;
	     const int right = offset[which];
	     const int up = offset[other];

	     // Select interval
	     const scalar minX = p0[which];
	     const scalar maxX = p1[which];

	     // Length along the abscisses
	     const scalar lengthX = maxX - minX;

	     // Height of the ray on this scale
	     const scalar startHeight = ray.m_From.Y;
	     const scalar endHeight = ray.m_To.Y;
	     const scalar diffHeight = endHeight - startHeight;
	     const scalar diffHeightPerX = diffHeight / lengthX;

	     // Length along the ordonnees
	     const scalar minY = p0[other];
	     const scalar lengthY = p1[other] - minY;
	     const scalar slope = lengthY / lengthX;

	     // Start position must be minX rounded down or 0
	     scalar startX = std::max(floorf(minX), 0.0f);

	     // We stay in 1 grid cell (endX-startX) is always bigger than (endY-startY)
	     if ((maxX - startX) < 1.0f)
	     {
		 // Push the corner and let the end function resolve the intersection
		 const int theX = static_cast<int>( v0.X );
		 const int theZ = static_cast<int>( v0.Z );
		 // std::cerr << "Vertical ray at " << theX << " " << theZ << std::endl;
		 potentialquads.push_back(theX);
		 potentialquads.push_back(theZ);
		 return;
	     }

	     // Look at Y position there
	     scalar lastY = (startX - minX) * slope + minY;
	     scalar floorY = floorf(lastY);

	     // Simple counter, must be > 1  to have intersection
	     // If we hit under the patch on the first test, we did not intersect
	     int testCount = 0;

	     // We are not yet in the patch
	     if (lastY < 0.0f)
	     {
		 // Recompute nearest integer X for Y at 0
		 const scalar shiftedX = (0.0f - minY) / slope + minX;
		 assert((startX < shiftedX) && "The new startX should be greater");
		 startX = std::max(floorf(shiftedX), 0.0f);

		 // Compute exact Y for this startX
		 lastY = (startX - minX) * slope + minY;
		 floorY = -1.f;
	     }

	     // shift by firstIndex difference
	     const int startIndex = static_cast<int>(startX);
	     index += startIndex * right;

	     // rayHeight, get increased by diffHeightPerX at each iteration
	     scalar currentRayHeight = startHeight + (startX - minX) * diffHeightPerX;

	     if (0.0f <= lastY)
	     {
		 // We have to manually check the first segment
		 // on  the left side of the ray or patch
		 index += static_cast<int>(floorY) * up;

		 const Vector3& vertex0 = m_Triangles.Coord(index);
		 const Vector3& vertex1 = m_Triangles.Coord(index + up);
		 const scalar interp = (lastY - floorY) / 1.0f;
		 const scalar height = interp * vertex1.Y + (1.f - interp) * vertex0.Y;

		 if (currentRayHeight < height)
		 {
		     // We are already under the patch, return doing nothing
		     return;
		 }

		 ++testCount;
	     }

	     const scalar endX = std::min(ceilf(maxX), size);
	     const int endIndex = static_cast<int>(endX);

	     // The computation is cranked one rank to the right
	     for (int i=startIndex ; i<endIndex ; ++i)
	     {
		 // Advancement ratio
		 const scalar x = static_cast<scalar>(i);
		 const scalar y = (x - minX) * slope + minY;
		 const scalar currentY = floorf(y);

		 // Goes one right
		 index += right;
		 const Vector3& vertex0 = m_Triangles.Coord(index);

		 // We went one line up
		 if (floorY < currentY)
		 {
		     // Goes up one level
		     index += up;

		     // Check against cell on the left
		     const Vector3& vertexL = m_Triangles.Coord(index - right);
		     const scalar interp = (currentY - lastY) / (y - lastY);
		     const scalar height = interp * vertex0.Y + (1.f - interp) * vertexL.Y;

		     const scalar rayHeight = startHeight + interp * diffHeightPerX;
		     if (rayHeight < height)
		     {
			 // We got under the patch, break from loop
			 cornerIndex = index - right;
			 break;
		     }

		     ++testCount;
		     if (size <= currentY)
		     {
			 // We went out of the patch
			 break;
		     }

		     floorY = currentY;
		 }

		 // Check against cell on the top
		 currentRayHeight += diffHeightPerX;

		 const Vector3& vertexU = m_Triangles.Coord(index + up);
		 const scalar interp = (y - currentY) / 1.0f;
		 const scalar height = interp * vertexU.Y + (1.f - interp) * vertex0.Y;

		 if (currentRayHeight < height)
		 {
		     // We got under the patch, break from loop
		     cornerIndex = index - right;
		     break;
		 }

		 ++testCount;
		 lastY = y;
	     }

	     // We got something
	     if ((0 <= cornerIndex) && (1 < testCount))
	     {
		 // Now we got the index, we must move it to the NorthWest side of the patch
		 if (transform[1])
		     cornerIndex -= SIZE+1;

		 if (transform[0])
		     cornerIndex -= 1;

		 const int row = cornerIndex % (SIZE+1);
		 const int line = cornerIndex / (SIZE+1);

		 potentialquads.push_back(m_MinX + row);
		 potentialquads.push_back(m_MinZ + line);
		 // std::cerr << "Intersection at " << (m_MinX + row) << " and " << (m_MinZ + line) << std::endl;
	     }

	 }




	 /**
	  * Make a list of potential colliders. If nothing has moved
	  * in the patch, we assume there can't be any collision.
	  * Note: we use a map to ensure there no collision is computed
	  * twice.
	  * Once this function has returned, the patch collision dirty
	  * flag is cleared.
	  */
	 virtual void CollectColliders (ColliderList &list)
	 {
	     if (m_CollisionDirty == false)
		 return;

	     for (EntityLI iter1=m_Entities.begin() ; iter1!=m_Entities.end(); ++iter1)
	     {
		 Entity* entity1 = *iter1;

		 if (!(entity1->m_Flags & Entity::COLLISION))
		     continue;

		 for (EntityLI iter2=iter1 + 1 ; iter2!=m_Entities.end(); ++iter2)
		 {
		     Entity *entity2 = *iter2;

		     if (!(entity2->m_Flags & Entity::COLLISION) ||
			     ((entity1->m_Flags & Entity::STATIC) && 
			      (entity2->m_Flags & Entity::STATIC)))
			 continue;

		     if (entity1->GetBBox().Overlap (entity2->GetBBox()))
		     {
			 list[ Collider(entity1,entity2) ] = true;
		     }
		 }
	     }
	 }
	 
      private:
	 Vector3 ComputeNormal (int x, int z, const Vector3& coord) const
	 {
	     const int nx = m_Heightfield->m_SizeX;
	     const int nz = m_Heightfield->m_SizeZ;

	     // Border gets Vertical Normal
	     if (x < 1 || z < 1 || (nx-1) < x || (nz-1) < z)
	     {
		 return Vector3(0.0f, 1.0f, 0.0f);
	     }

	     Vector3 p0 = m_Heightfield->GetCoord (x, z);
	     Vector3 pW = m_Heightfield->GetCoord (x-1, z);
	     Vector3 pN = m_Heightfield->GetCoord (x, z-1);
	     Vector3 pE = m_Heightfield->GetCoord (x+1, z);
	     Vector3 pS = m_Heightfield->GetCoord (x, z+1);

	     Vector3 normal;
	     normal += Vector3::ComputeNormal (p0, pW, pN);
	     normal += Vector3::ComputeNormal (p0, pN, pE);
	     normal += Vector3::ComputeNormal (p0, pE, pS);
	     normal += Vector3::ComputeNormal (p0, pS, pW);
	     normal.Normalize();

	     return normal;
	 }


       void BuildTriangleIndices()
       {
	   // empty existing list
	   m_TriangleIndices.resize(0);
	   m_BlendedTriangleIndices.resize(0);
	   m_BlackTriangleIndices.SetType(PRIM_TRIANGLES);
	   m_BlackTriangleIndices.SetEnabledSize(0);

	   // empty the material list too
	   m_MaterialsUsed.resize(0);

	   m_BlendedTriangles.SetFormat(
		   VertexBuffer::VB_HAS_COORD|
		   VertexBuffer::VB_HAS_NORMAL|
		   VertexBuffer::VB_HAS_COLOR);

	   // starting size
	   const int initialSize = SIZE * 3;
	   m_BlendedTriangles.Resize(initialSize);
	   m_BlendedTriangleCount = 0;

	   for (int mz0=0 ; mz0<SIZE ; ++mz0)
	   {
	       const int mz1 = mz0 + 1;

	       for (int mx0=0 ; mx0<SIZE ; ++mx0)
	       {
		   const int mx1 = mx0 + 1;

		   AddMaterial(mx0, mz0);
		   AddMaterial(mx0, mz1);
		   AddMaterial(mx1, mz1);
		   AddMaterial(mx1, mz0);

		   BuildTriangle (mx0, mz0, mx1, mz0, mx1, mz1);
		   BuildTriangle (mx1, mz1, mx0, mz1, mx0, mz0);
	       }
	   }

       }

       /**
	* Adds a new material if necessary
	*
	* The material are indices in m_Materials in QuadtreeRenderManager
	*
	* Creates the associated PrimitiveBlock for blended and non-blended
	* triangles using this material (any block can end up empty)
	*/
       void AddMaterial(int x, int z)
       {
	   const int material = static_cast<int>(Ground(x,z));
	   
	   std::vector<int>::iterator i;
	   i = std::find(m_MaterialsUsed.begin(), m_MaterialsUsed.end(), material);

	   if (i == m_MaterialsUsed.end())
	   {
	       m_MaterialsUsed.push_back(material);

	       // Non blended list of triangles
	       {
		   PrimitiveBlock pb;
		   pb.SetType (PRIM_TRIANGLES);
		   pb.SetEnabledSize(0);
		   m_TriangleIndices.push_back(pb);
	       }
	       // Blended list of triangles
	       {
		   PrimitiveBlock pb;
		   pb.SetType (PRIM_TRIANGLES);
		   pb.SetEnabledSize(0);
		   m_BlendedTriangleIndices.push_back(pb);
	       }
	   }
       }

       /**
	* Returns the index in m_MaterialsUsed of a material.
	*
	* This is used to get access of primitive blocks for the given material
	*/
       int GetMaterialIndex (uchar material)
       {
	   std::vector<int>::iterator i;
	   i = std::find(m_MaterialsUsed.begin(), m_MaterialsUsed.end(), material);

	   assert(i!=m_MaterialsUsed.end() && "material not found in list");
	   return std::distance(m_MaterialsUsed.begin(), i);
       }

       /**
	* Build the blended triangles vertex buffer.
	*
	* Copy from m_Triangles at indexFrom to m_BlendedTriangles at indexTo
	*/
       void CopyVertex(int indexFrom, int indexTo, bool alpha)
       {
	   const Vector3& point  = m_Triangles.Coord(indexFrom);
	   const Vector3& normal = m_Triangles.Normal(indexFrom);
	   RGBA rgba             = m_Triangles.Color4(indexFrom);
	   if (alpha)
	   {
	       rgba.A = 0;
	   }

	   m_BlendedTriangles.Coord (indexTo) = point;
	   m_BlendedTriangles.Normal(indexTo) = normal;
	   m_BlendedTriangles.Color4(indexTo) = rgba;
       }

       /**
	* Add a blended triangle to a primitive block.
	*
	* The vertex buffer is built as needed.
	*/
       void AddBlendedTriangle(PrimitiveBlock& pb, 
	       int index1, bool alpha1,
	       int index2, bool alpha2, 
	       int index3, bool alpha3)
       {
	   const int baseIndex = 3*m_BlendedTriangleCount;
	   ++m_BlendedTriangleCount;

	   // Ensures vertex buffer is large enough
	   const int vbSize = m_BlendedTriangles.Size();
	   if (vbSize < (baseIndex+3))
	   {
	       m_BlendedTriangles.Resize(vbSize + (SIZE * 3));
	   }

	   // copy vertices
	   CopyVertex(index1, baseIndex+0, alpha1);
	   CopyVertex(index2, baseIndex+1, alpha2);
	   CopyVertex(index3, baseIndex+2, alpha3);

	   AddTriangle(pb, baseIndex+0, baseIndex+1, baseIndex+2);

	   // this is a special hack to get black triangles added
	   // alpha1 is 'false' only for the first call of AddBlendedTriangle
	   if (!alpha1)
	   {
	       AddTriangle(m_BlackTriangleIndices, baseIndex+0, baseIndex+1, baseIndex+2);
	   }
       }

       /**
	* Add a non-blended triangle to a primitive block.
	*/
       void AddTriangle(PrimitiveBlock& pb, int index1, int index2, int index3)
       {
	   const size_t triangleindex = pb.EnabledSize();
	   //std::cerr << "triangleindex=" << triangleindex << std::endl;
	   pb.Add(index1);
	   pb.Add(index2);
	   pb.Add(index3);
	   //std::cerr << "pb.Size()=" << pb.Size() << std::endl;
	   pb.SetEnabledSize(triangleindex + 3);
       }

       /**
	* Build primitive blocks for 1 triangle.
	*/
       void BuildTriangle (int x1, int y1,
	       int x2, int y2,
	       int x3, int y3)
       {
	   const int padding = SIZE + 1;
	   const int index1 = x1 + y1 * padding;
	   const int index2 = x2 + y2 * padding;
	   const int index3 = x3 + y3 * padding;

	   const uchar material1 = Ground (x1, y1);
	   const uchar material2 = Ground (x2, y2);
	   const uchar material3 = Ground (x3, y3);

	   const bool material2isMaterial1 = material1 == material2;
	   const bool material3isMaterial1 = material1 == material3;

	   // the three vertex have the same material. 
	   if (material2isMaterial1 && material3isMaterial1)
	   {
	       // look for the material in the list
	       const int materialIndex = GetMaterialIndex(material1);
	       PrimitiveBlock &pb = m_TriangleIndices[materialIndex];
	       AddTriangle(pb, index1, index2, index3);

	       // done for this one
	       return;
	   }

	   // last equality relation :)
	   const bool material3isMaterial2 = material2 == material3;

	   // handles material 1 blended triangles
	   {
	       // look for the material in the list
	       const int materialIndex = GetMaterialIndex(material1);
	       PrimitiveBlock &pb = m_BlendedTriangleIndices[materialIndex];

	       // do we have to make the other corners transparent ?
	       const bool alpha2 = !material2isMaterial1;
	       const bool alpha3 = !material3isMaterial1;
	       AddBlendedTriangle(pb, index1, false, index2, alpha2, index3, alpha3);

	       // we must go to test material2 and 3, but at least one is different
	   }

	   // handles material 2 blended triangles
	   if (!material2isMaterial1)
	   {
	       // look for the material in the list
	       const int materialIndex = GetMaterialIndex(material2);
	       PrimitiveBlock &pb = m_BlendedTriangleIndices[materialIndex];

	       // do we have to make the other corners transparent ?
	       const bool alpha3 = !material3isMaterial2;
  	       AddBlendedTriangle(pb, index1, true, index2, false, index3, alpha3);
	   }

	   if (!material3isMaterial1 && !material3isMaterial2)
	   {
	       // look for the material in the list
	       const int materialIndex = GetMaterialIndex(material3);
	       PrimitiveBlock &pb = m_BlendedTriangleIndices[materialIndex];

	       // we now know the 2 other corner must be transparent
	       AddBlendedTriangle(pb, index1, true, index2, true, index3, false);
	   }
       }

   };


   // =======================================================================
   struct RenderStats
   {
	 int m_NumTris;
	 int m_NumFlushes;
   } g_RenderStats;

   bool g_UpdateVis = true;
   
   //Sorter based on camera distance
   class CameraSorter
   {
       Vector3 m_CameraPosition;
   public:
       bool operator()(Patch* p1, Patch* p2)
       {
	   Vector3 cp1(
		   p1->m_CenterPoint.X - m_CameraPosition.X, 
		   p1->m_CenterPoint.Y - m_CameraPosition.Y, 
		   p1->m_CenterPoint.Z - m_CameraPosition.Z);
	   const scalar d1 = cp1.X*cp1.X + cp1.Y*cp1.Y + cp1.Z*cp1.Z;
	   Vector3 cp2(
		   p2->m_CenterPoint.X - m_CameraPosition.X, 
		   p2->m_CenterPoint.Y - m_CameraPosition.Y, 
		   p2->m_CenterPoint.Z - m_CameraPosition.Z);
	   const scalar d2 = cp2.X*cp2.X + cp2.Y*cp2.Y + cp2.Z*cp2.Z;

	   return d1<d2;
       }
       void SetCamera(const Camera& camera)
       { m_CameraPosition = camera.m_PointOfView; }
   };

   /** 
    *
    */
   class QuadtreeRenderManager
   {

       // List of materials for rendering
       Material m_Black;
       MaterialList m_Materials;

       // Renderer (set once, theorically)
       Renderer* m_Renderer;

       // Camera (set each frame)
       const Camera* m_Camera;
       CameraSorter m_Sorter;

	   // Fog color (for multipass back state)
	   const Color* m_FogColor;

       // List of patches, approximatively sorted front to back
       PatchList m_PatchList;
       

       void FlushNormalTriangles(Material& material, const PrimitiveBlock &pb)
       {
	   g_RenderStats.m_NumTris += pb.EnabledSize() / 3;

	   const int pflags = material.m_Passes[0].m_Flags;
	   material.m_Passes[0].m_Flags &= ~PASS_HAS_BLENDING;

	   m_Renderer->RenderBlock(material, pb.Type(), &pb[0], pb.EnabledSize());

	   material.m_Passes[0].m_Flags = pflags;
       }

       void FlushBlendTriangles(Material& material, const PrimitiveBlock &pb)
       {
	   g_RenderStats.m_NumTris += pb.EnabledSize() / 3;

	   const int flags = material.m_Flags;
	   material.m_Flags &= ~(MATERIAL_HAS_PASS2 | MATERIAL_HAS_PASS3 | MATERIAL_HAS_PASS4);

	   m_Renderer->RenderBlock(material, pb.Type(), &pb[0], pb.EnabledSize());

	   material.m_Flags = flags;
       }


       public:
       QuadtreeRenderManager (const MaterialList& materials) :
	    m_Black ("black"),
	    m_Renderer(0),
	    m_Camera(0)
       {
	   SetMaterials (materials);
	   m_Black.m_Flags = MATERIAL_HAS_PASS1;
	   m_Black.m_Passes[0].m_Flags = 0;
	   m_Black.m_Passes[0].m_BlendColor = Color(0.f,0.f,0.f,1.f);
       }

       ~QuadtreeRenderManager ()
       {
	   ClearMaterials();
       }

       void ClearMaterials()
       {
	   m_Materials.resize(0);
       }
       
       void SetMaterials(const MaterialList& materialList)
       {
	   // Unref first previous materials set
	   ClearMaterials();

	   // Copy by ref the material list
	   const int materialCount = materialList.size();
	   m_Materials.resize(materialCount);
	   for (int material=0 ; material<materialCount ; ++material)
	   {
		   // these are MaterialPtr, get refs by copy
	       m_Materials[material] = materialList[material];
	   }
       }

       void SetRenderData(Renderer& render, const Camera& camera, const Color& fogColor)
       {
	   m_Renderer = &render;
	   m_FogColor = &fogColor;
	   m_Camera = &camera;
	   m_Sorter.SetCamera(camera);
       }

       void AddPatch(Patch* patch)
       {
	   assert(m_Camera && "Camera was not set, or NULL");

	   PatchListIterator i = std::lower_bound(m_PatchList.begin(), m_PatchList.end(), patch, m_Sorter);
	   m_PatchList.insert(i, patch);
       }

       void DrawPatches()
       {
	   assert(m_Renderer && "Renderer was not set, or NULL");

	   ++g_RenderStats.m_NumFlushes;

	   PatchListIterator i;
	   for (i=m_PatchList.begin() ; i!=m_PatchList.end() ; ++i)
	   {
	       Patch* current = *i;

	       /// Initialize the vertex buffer.
	       m_Renderer->SetActiveVB (current->m_Triangles);
	       m_Renderer->LockVB (0, current->m_Triangles.Size());

	       const int pbNormalCount = current->m_TriangleIndices.size();
	       for (int normal=0 ; normal<pbNormalCount ; ++normal)
	       {
		   const int materialUsed = current->m_MaterialsUsed[normal];
		   Material& material = *m_Materials[materialUsed];
		   
		   PrimitiveBlock& pb = current->m_TriangleIndices[normal];
		   // Draw the "normal" non-blended triangles ('one material' tris)
		   FlushNormalTriangles(material, pb);
	       }

	       m_Renderer->UnlockVB();
	   }

	   const GLfloat fogColor[4] = { m_FogColor->R, m_FogColor->G, m_FogColor->B, m_FogColor->A }; 
	   
	   for (i=m_PatchList.begin() ; i!=m_PatchList.end() ; ++i)
	   {
	       Patch* current = *i;

	       // Render 'em first black so additive blending will work.
	       if (0 < current->m_BlendedTriangleCount)
	       {
		   /// Initialize the blending vertex buffer.
		   m_Renderer->SetActiveVB (current->m_BlendedTriangles);
		   m_Renderer->LockVB (0, current->m_BlendedTriangles.Size());

		   glDisable (GL_COLOR_ARRAY);
		   //glDisable (GL_CULL_FACE);

		   glColor4f (0.0f, 0.0f, 0.0f, 1.0f);
		   glFogfv(GL_FOG_COLOR, g_fogBlackColor);
		   FlushNormalTriangles(m_Black, current->m_BlackTriangleIndices);
		   glFogfv(GL_FOG_COLOR, fogColor);
		   glColor4f (1.0f, 1.0f, 1.0f, 1.0f);

		   //glEnable (GL_CULL_FACE);
		   glEnable (GL_COLOR_ARRAY);

		   glColorMaterial(GL_FRONT, GL_DIFFUSE);
		   glEnable (GL_COLOR_MATERIAL);

		   const int blendCount = current->m_BlendedTriangleIndices.size();
		   for (int blend=0 ; blend<blendCount ; ++blend)
		   {
		       PrimitiveBlock& pb = current->m_BlendedTriangleIndices[blend];

		       // job done
		       if (pb.EnabledSize() <= 0)
			   continue;

		       // Render 'multiple terrain' triangles with blending.
		       //m_Renderer->OverrideVB ( VertexBuffer::VB_HAS_COLOR, m_CurPatch->m_Triangles);

		       const int materialUsed = current->m_MaterialsUsed[blend];
		       Material& material = *m_Materials[materialUsed];
		       FlushBlendTriangles(material,  pb);
		   }

		   glColor4f (1.0, 1.0, 1.0, 1.0);
		   glDisable (GL_COLOR_MATERIAL);

		   // unlock the blending VB
		   m_Renderer->UnlockVB();
	       }

	   }
	   
	   // TODO: Recycle list for faster access ?
	   m_PatchList.resize(0);
       }
       

   };

   void Patch::RenderPatch()
   {
       m_Manager->AddPatch(this);
   }


   // =======================================================================
   


   QuadtreeNode::QuadtreeNode() 
   {
      for (int i = 0; i < 4; i++)
	 m_Children[i] = 0;
   }

   QuadtreeNode::~QuadtreeNode ()
   {
      for (int i = 0; i < 4; i++)
      {
	 delete m_Children[i];
	 m_Children[i] = 0;
      }
   }

   void
   QuadtreeNode::Build(HeightField* hf, 
	   int minX, int minZ, int size,
	   QuadtreeRenderManager* manager)
   {
       const int childSize = size/2;
       const bool usePatch = (childSize == Patch::SIZE);

       for (int i=0 ; i<4 ; ++i)
       {
	   if (usePatch)
	   {
	       m_Children[i] = new Patch();
	   }
	   else
	   {
	       m_Children[i] = new QuadtreeNode();
	   }

	   const int childEast = (i % 2) ? childSize : 0;
	   const int childSouth = (1 < i) ? childSize : 0;
	   const int childX = minX + childEast;
	   const int childZ = minZ + childSouth;

	   // Virtual call to Build
	   m_Children[i]->Build(hf, childX, childZ, childSize, manager);

	   // Make bounding box
	   m_BBox.AddBBox (m_Children[i]->m_BBox);
       }
       // std::cerr << "Node BB is " << m_BBox.m_Min << " " << m_BBox.m_Max << std::endl;

   }

   Patch*
   QuadtreeNode::FindPatch(scalar x, scalar z)
   {
       const Vector3& bbmin = m_BBox.m_Min;
       const Vector3& bbmax = m_BBox.m_Max;

       if ((x < bbmin.X || x > bbmax.X) ||
	       (z < bbmin.Z || z > bbmax.Z))
	   return 0;

       // Leaf is a patch
       if (!m_Children[0])
       {
	   return static_cast<Patch*>(this);
       }

       const scalar midX = (bbmin.X + bbmax.X) / 2.0f;
       const scalar midZ = (bbmin.Z + bbmax.Z) / 2.0f;

       int child = 0;
       if (midX < x)
	   ++child;

       if (midZ < z)
	   child += 2;
       
       return m_Children[child]->FindPatch(x, z); 
   }

   void
   QuadtreeNode::CollectColliders(ColliderList& list)
   {
      for (int i=0 ; i<4 ; ++i)
      {
	 m_Children[i]->CollectColliders(list);
      }
   }

   void
   QuadtreeNode::InvalidatePatches(const BBox& box)
   {
       if (!m_BBox.Overlap(box))
	   return;
       
       for (int i=0 ; i<4 ; ++i)
	  m_Children[i]->InvalidatePatches(box);
   }

   bool QuadtreeNode::RayTrace (
			    const Ray& ray,
                            std::vector< int >& potentialquads,
                            EntityList& potentialents)
   {
      Vector3 c;
      if (ray.HitBBox (m_BBox, &c) == false)
	 return false;
	 
      if (m_Children[0])
      {
	 for (int i = 0; i < 4; i++)
	 {
	    if (m_Children[i]->RayTrace (ray, potentialquads, potentialents))
	       return true;
	 }
	 
	 return false;
      }
      else
      {
	  Patch* patch = static_cast<Patch*>(this);
	  patch->AddPatchEntsToList (potentialents);
	  patch->RayTrace(ray, potentialquads);
      }

      return false;
   }

   void
   QuadtreeNode::Render(const Frustum& view, Visibility vis)
   {
      if (g_UpdateVis)
	  m_Vis = vis;

      if (m_Vis != INSIDE)
      {
	  if (g_UpdateVis)
	      m_Vis = view.GetVisibility(m_BBox);

	  if (m_Vis == OUTSIDE )
	      return;
      }
      
      // We assume a patch do not have children
      if (!m_Children[0])
      {
	  Patch *patch  = static_cast< Patch* >( this );

	  if (patch->m_Dirty)
	      patch->Validate();

	  // Will  add itself to its manager
	  patch->RenderPatch();
      }
      else
      {
	  for (int i = 0; i < 4; i++)
	  {
	      m_Children[i]->Render(view, (Visibility)m_Vis);
	  }
      }
   }

   /////////////////////////////////////////////////////////////////////

   Quadtree::Quadtree (HeightField *heightfield) : 
      m_Heightfield (heightfield),
      m_Rootnode (NULL)
   {
      m_Rootnode = new QuadtreeNode();
      m_RenderManager = new QuadtreeRenderManager(heightfield->m_Grounds);
      m_Size = (int) heightfield->m_SizeX;
      if (Patch::SIZE <= m_Size)
      {
	  m_Rootnode->Build(heightfield, 0, 0, m_Size, m_RenderManager);
      }
      else
      {
	  Ark::Sys()->Fatal ("Cannot create height field quadtree.");
      }
   }

   Quadtree::~Quadtree ()
   {
       // Deletes quadtree and sets to invalid ptr
       delete m_Rootnode;
       m_Rootnode = 0;

       // Render manager too
       delete m_RenderManager;
       m_RenderManager = 0;
   }

   /**
    * Mark the patches in the specified area as "invalidate", which
    * means their topology has changed since the quadtree has been
    * created.
    */
   void
   Quadtree::Invalidate (scalar minx, scalar minz, scalar maxx, scalar maxz)
   {
       const BBox& quadtreeBox = m_Rootnode->GetBoundingBox();
       BBox invalidateBox;
       invalidateBox.m_Min = Vector3(minx, quadtreeBox.m_Min.Y, minz);
       invalidateBox.m_Max = Vector3(maxx, quadtreeBox.m_Max.Y, maxz);

       m_Rootnode->InvalidatePatches(invalidateBox);
   }

   bool Quadtree::RayTrace (const Ray &ray,
			    Collision &collision,
			    bool test_ents)
   {
       // std::cerr << "RayTrace called" << std::endl;
     std::vector< int > potentialquads;
     EntityList potentialents;

      m_Rootnode->RayTrace (ray, potentialquads, potentialents);

      HeightField *hf = m_Heightfield;

      ColSystem *cs = hf->GetCache()->GetColSystem();

      collision.m_Entity = 0;
      collision.m_Flags = 0;

      scalar mindist = 1000000.0;
      bool foundcol = false;

      if (cs != NULL && test_ents)
      {
	 Vector3 p;

	 for (EntityLI e = potentialents.begin();
	      e != potentialents.end(); ++e)
	 {
	    if (!ray.HitBBox ((*e)->GetBBox(), &p))
	       continue;

	    if (cs->RayTrace ((*e)->m_MState, ray, collision))
	    {
	       scalar dist = (collision.m_Pos - ray.m_From).GetMagnitude();

	       if (dist > mindist) continue;

	       mindist = dist;
	       foundcol = true;
	       collision.m_Flags |= Collision::ENTITY;
	       collision.m_Entity = *e;
	    }
	 }
      }

      if (foundcol) return true;

      Vector3 coord;
      // One quad is a couples (x,z) in hf indices
      const int quadCount = potentialquads.size() / 2;
      for (int i=0; i<quadCount; ++i)
      {
	  const int base = 2*i;
	  const int x = potentialquads[base];
	  const int z = potentialquads[base+1];
	  
	  // Gets the corners
	  Vector3 v[4] = {
	      hf->GetCoord(x, z),
	      hf->GetCoord(x+1, z),
	      hf->GetCoord(x, z+1),
	      hf->GetCoord(x+1, z+1),
	  };

	  int indices[4] = { 0, 1, 2, 3 };
	  
	  // if x+z is odd, the triangles do not break on the base of the quad
	  if ((x+z) % 2)
	  {
	      indices[0] = 1;
	      indices[1] = 0;
	      indices[2] = 3;
	      indices[3] = 2;
	  }

	  if (ray.HitTriangle(v[indices[0]], v[indices[1]], v[indices[2]], &coord))
	  {
	      collision.m_Flags = Collision::WORLD
		  |Collision::PLANE
		  |Collision::POSITION
		  |Collision::WORLD;

	      collision.m_Pos = coord;
	      collision.m_Plane = Plane::GetTriPlane (v[indices[0]], v[indices[1]], v[indices[2]]);
	      collision.m_Material = hf->GetGrd (x, z);
	      // std::cerr << "Intersection at << " << (x) << " and " << (z) << " confirmed" << std::endl;
	      return true;
	  }
	  else if (ray.HitTriangle(v[indices[1]], v[indices[2]], v[indices[3]], &coord))
	  {
	      collision.m_Flags = Collision::WORLD
		  |Collision::PLANE
		  |Collision::POSITION
		  |Collision::WORLD;

	      collision.m_Pos = coord;
	      collision.m_Plane = Plane::GetTriPlane (v[indices[1]], v[indices[2]], v[indices[3]]);
	      collision.m_Material = hf->GetGrd (x, z);
	      // std::cerr << "Intersection at << " << (x) << " and " << (z) << " confirmed" << std::endl;
	      return true;
	  }
      }

      return false;
   }

   // Remove an entity from the quadtree's patches...
   void
   Quadtree::RemoveEntity (Entity *ent, HFEntityData *data)
   {
     std::vector< Patch* >::iterator i;

      for (i = data->m_Patches.begin(); i != data->m_Patches.end(); i++)
	 (*i)->RemoveEntity (ent);
   }
   
   // Add/update an entity in the quadtree's patches.
   void
   Quadtree::UpdateEntity (Entity *ent, HFEntityData *data)
   {
      RemoveEntity (ent, data);

      const BBox &bbox = ent->GetBBox();

      /// Add the entity to all the patches it might belong to...
      scalar cs[4][2] = 
	 {
	    {
	       bbox.m_Min.X,
	       bbox.m_Min.Z,
	    },
	    {
	       bbox.m_Min.X,
	       bbox.m_Max.Z,
	    },
	    {
	       bbox.m_Max.X,
	       bbox.m_Max.Z,
	    },
	    {
	       bbox.m_Max.X,
	       bbox.m_Min.Z,
	    },
	 };

      for (int i = 0; i < 4; i++)
      {
	 Patch *p = m_Rootnode->FindPatch(cs[i][0], cs[i][1]);

	 /// Add the entity to this patch.
	 if (p)
	 {
	    p->AddEntity (ent);
	    data->m_Patches.push_back (p);
	 }
      }
   }
   
   /*
    * Make a list of potential colliders.
    * Note: we use a map to ensure there no collision is computed
    * twice.
    */
   void
   Quadtree::GetColliders (ColliderList &list)
   {
       m_Rootnode->CollectColliders(list);
   }    


   /////////////////////////////////////////////////////////////////////

   void
   Quadtree::Render(Renderer &renderer, const Camera &camera, const Color& fogColor)
   {
      g_RenderStats.m_NumTris = 0;
      g_RenderStats.m_NumFlushes = 0;

      m_RenderManager->SetRenderData(renderer, camera, fogColor);
      m_Rootnode->Render(renderer.GetFrustum(), SOME_CLIP);
      m_RenderManager->DrawPatches();

#if 0
      Sys()->Log ("Quadtree rendered : %d triangles, %d flushes\n", 
		  g_RenderStats.m_NumTris,
		  g_RenderStats.m_NumFlushes);
#endif
   }

   void 
    Quadtree::SetMaterials (const MaterialList& materials) 
    {
	 m_RenderManager->SetMaterials( materials );
    }
		    
} // ns Ark