/* $Id: HFPathfind.cpp,v 1.7 2003/03/13 23:35:55 zongo Exp $
**
** Ark - Libraries, Tools & Programs for MMORPG developpements.
** Copyright (C) 1999-2002 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.
*/


#include <Ark/ArkLoader.h>

#include "HFWorld.h"
#include "HFAStar.h"

#include <algorithm>

namespace Ark
{

#undef PATHDEBUG

#define DEF_SPEED 1.5 // FIXME: XXXX: speed depends on the entity..

   // Return the path to follow to go from pA to pB
   bool
   HeightField::FindPath (Path &path)
     //Path HeightField::FindPath(Vector3 pA, Vector3 pB, (speed))
     //we can also add a method to follow someone Find(Vector3 pA, Entity titi)
   {
      if (!m_Pathfinder)
	 return false;

      Vector3 pA = path.m_Entity->m_MState.m_Position;
      Vector3 pB;

      Timer timer;

#if 0
      if (path.m_MovingTarget)
      {
	 pB = path.m_EntTarget->m_MState.m_Position;
	 
	 // Don't recompute the path if the last point of the already
	 // computed path is near enough of the target point.
	 
	 scalar dist = (path.back().second - pB).GetMagnitude();
	 if (dist < path.m_MaxFollowDistance)
	    return true;
      }
      else
#endif
	 pB = path.m_PosTarget;

      Coord A (int (pA.X/m_Scale), int (pA.Z/m_Scale));
      Coord B (int (pB.X/m_Scale), int (pB.Z/m_Scale));

      if (A == B || (path.m_Entity->m_Flags & Entity::PATHFINDING) == 0)
      {
	 path.Reset();
	 path.AddPoint (pA, DEF_SPEED);
	 path.AddPoint (pB, DEF_SPEED);
      
	 return true;
      }

      Vector3 ab = pB - pA;

#if 0
      if (path.m_MovingTarget)
      {
	 // If we are near enough, stop, and go backward if we are too close
	 // I don't think going backward is very useful - teuf
	 scalar length = ab.GetMagnitude();

	 if (length < path.m_MinFollowDistance)
	    path.m_Backwards = true;

	 if (length != 0.0)
	    ab.Scale (1.0f/length);

	 pB = pB + (-path.m_MaxFollowDistance - 0.1f) * ab;
	 B = Coord (int (pB.X/m_Scale), int (pB.Z/m_Scale));
      }
      else
#endif
	 ab.Normalize();

      // FIXME: I don't think reachable returns the expected result...
      while (!m_Pathfinder->reachable (A,B))
      {
	 pB -= ab;
	 B = Coord(int (pB.X/m_Scale), int (pB.Z/m_Scale));
      }

      // Try to find a path...
      std::vector <Coord> vpath;
   
      m_Pathfinder->find_path (A, B, vpath);
   
      if (!vpath.size())
	 return false;
   
      // Fill the path
      std::vector <Coord>::reverse_iterator i;
      scalar lx = pA.X, ly = pA.Z;

      path.Reset();

      Vector3 copy = pA;
      copy.Y = GetHeight (pA.X, pA.Z);
      path.AddPoint (copy, DEF_SPEED);

      // Add a point at 30cm in the same direction the model is so that a rough
      // half-turn is avoided.
      Vector3 initialDir = (path.m_Entity->m_MState.m_Matrix.Transform
			    (Vector3(0.3f,0.0f,0.0f))); //FIXME: tune this
      initialDir.Y = GetHeight (initialDir.X, initialDir.Z);      
      path.AddPoint(initialDir,DEF_SPEED);

      for (i = vpath.rbegin(); i != vpath.rend(); i++)
      {
	 if (i == vpath.rbegin() || i == (vpath.rend()-1))
	    continue;

	 // Estimation of next point
	 scalar mx, my;
      
	 if (i < (vpath.rend()-1))
	 {
	    std::vector<Coord>::reverse_iterator n = i+1;
	    mx = (scalar(n->X) + lx) / 2;
	    my = (scalar(n->Y) + ly) / 2;
	 }
	 else
	 {
	    // Avoid turning at the last moment...
	    mx = pB.X;
	    my = pB.Z;
	 }
      
	 // Clamp..
	 lx = std::min (std::max (mx, static_cast<scalar>(i->X)), i->X+1.f);
	 ly = std::min (std::max (my, static_cast<scalar>(i->Y)), i->Y+1.f);
      
	 Vector3 r (lx * m_Scale, 0.0, ly * m_Scale);
	 r.Y = GetHeight (r.X, r.Z);
      
	 path.AddPoint (r, DEF_SPEED);
      }
   
      if (lx != pB.X || ly != pB.Z)
      {
	 Vector3 copy = pB;
	 copy.Y = GetHeight (pB.X, pB.Z);
	 path.AddPoint (copy, DEF_SPEED);   
      }

#if 0
      if (path.m_MovingTarget)
      {
	 const int n = std::min(static_cast<int>(path.size()) - 1, 4);

	 path.PlanifyUpdate (Timer::GetTime() + 2.0);
      }
#endif


#ifdef PATHDEBUG
      Ark::Sys()->Log
         ("Pathfinder : %f milliseconds\n"
          "  A = (%3.1f, %3.1f)\n"
          "  B = (%3.1f, %3.1f)\n"
          "  path_cost = %d\n"
          "  path_length = %d\n"
          "  nodes_searched = %d\n"
          "  nodes_added = %d\n"
          "  nodes_removed = %d\n"
          "  nodes_visited = %d\n"
          "  nodes_left = %d\n",
          timer.GetDelta() * 1000.0,
          pA.X, pA.Z,  pB.X, pB.Z,
          m_Pathfinder->m_Stats.path_cost,
          m_Pathfinder->m_Stats.path_length,
          m_Pathfinder->m_Stats.nodes_searched,
          m_Pathfinder->m_Stats.nodes_added,
          m_Pathfinder->m_Stats.nodes_removed,
          m_Pathfinder->m_Stats.nodes_visited,
          m_Pathfinder->m_Stats.nodes_left);
#endif

      return true;
   }

   /* Can a point be reached ?? */
   bool
   HeightField::IsReachable
   (const Vector3 &pA, const Vector3 &pB, bool gravity)
   {
      if (m_Pathfinder == NULL)
	 return false;

      Coord A (int (pA.X/m_Scale), int (pA.Z/m_Scale));
      Coord B (int (pB.X/m_Scale), int (pB.Z/m_Scale));

      return m_Pathfinder->reachable (A, B);
   }

   void
   HeightField::DestroyPathfinder ()
   {
      if (m_Pathfinder) delete m_Pathfinder;
   }

   // Init pathfinding stuff (only needed on server side)
   void
   HeightField::InitPathfinder ()
   {
     DestroyPathfinder();

     uchar lookup [256];
     String colldata = m_Config.GetStr ("heightfield::CollisionData", "");

     for (int i = 0; i < 256; i++)
       lookup[i] = (uchar) i;

     int totalsize = m_SizeX * m_SizeZ;
     uchar *data = new uchar [totalsize];

     if (colldata != "")
     {
       Image img;
       if (!Sys()->GetLoaders()->Load (&img, colldata))
         return;

       if (img.m_Format != Image::I_8 ||
           img.m_Width != m_SizeX ||
           img.m_Height != m_SizeZ)
       {
         Sys()->Warning ("%s: Bad image format for collision data...",
             colldata.c_str());
         return;
       }

       memcpy (data, img.m_Data, totalsize);
     }
     else
     {
       memset (data, 0, totalsize);
     }

     for (size_t z = 0; z < m_SizeZ; z++)
     {
       for (size_t x = 0; x < m_SizeX; x++)
       {
         Material *grd = GetGrd (x, z);
         if (grd && !(grd->m_Flags & MATERIAL_IS_WALKABLE))
         {
           data[z * m_SizeX + x] = 255;
         }
         else
         {
#if 0 // FIXME: do something of that kind.
           Vector3 bottom = GetCoord (x,z);
           bottom.Y += 0.1;

           Vector3 top = bottom;
           top.Y += 10.0;

           std::vector< Collision > cols;
           if (RayTrace (Ray (top, bottom),
                 Collision::ENTITY |
                 Collision::ONESHOT,
                 cols) && cols.size())
           {
             if (cols[0].m_Pos.GetMagnitude2() < 4.0)
               data[z * m_SizeX + x] = 255;
           }	     
#endif
         }
       }
     }

     std::vector< Entity* >::iterator it;
     for (it = m_Entities.begin(); it != m_Entities.end(); it++)
     {
       // set high costs (ie 255) for places where entities lay
       // so the pathfinder won't try those places.

       if ((*it)->m_MState.GetModel() == NULL ||
           ((*it)->m_Flags & Entity::STATIC) == 0)
         continue;

       // FIXME: better collision detection should be done here, in order
       // FIXME: to avoid having places marked as unwalkable whereas they are.
       BBox bbox = (*it)->m_MState.ExtractBbox();
       bbox.m_Min += (*it)->m_MState.m_Position;
       bbox.m_Max += (*it)->m_MState.m_Position;

       int mix = (int) floor(bbox.m_Min.X / m_Scale);
       int miz = (int) floor(bbox.m_Min.Z / m_Scale);
       int max = (int) ceil(bbox.m_Max.X / m_Scale);
       int maz = (int) ceil(bbox.m_Max.Z / m_Scale);

#ifdef PATHDEBUG
       Sys()->Log ("marking zone (%d,%d)->(%d,%d) as unwalkable\n",
           mix, miz, max, maz);
#endif

       for (int x = mix; x < max; x++)
       {
         for (int z = miz; z < maz; z++)
           data[z * m_SizeX + x] = 255;
       }
     }

     m_Pathfinder = new AStar (data, m_SizeX, m_SizeZ);
   }

}