/*
** Ark - Libraries, Tools & Programs for MMORPG developpements.
** Copyright (C) 1999-2000 The Contributors of the Ark Project
** Copyright (C) 1999 Amit J. Patel
** 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.
*/


//------------------------------------------------------------------------------
// A really big part of this code has been taken from the Amit's Path-finding
// (A*) code, Copyright (C) 1999 Amit J. Patel
//
// Permission to use, copy, modify, distribute and sell this software
// and its documentation for any purpose is hereby granted without fee,
// provided that the above copyright notice appear in all copies and
// that both that copyright notice and this permission notice appear
// in supporting documentation.  Amit J. Patel makes no
// representations about the suitability of this software for any
// purpose.  It is provided "as is" without express or implied warranty.
//------------------------------------------------------------------------------

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

#include "HFAStar.h"

namespace Ark
{

#define MAXIMUM_PATH_LENGTH 100000

// g_PathDiv is used to modify the heuristic.  The lower the number,
// the higher the heuristic value.  This gives us worse paths, but
// it finds them faster.  This is a variable instead of a constant
// so that I can adjust this dynamically, depending on how much CPU
// time I have.  The more CPU time there is, the better paths I should
// search for.
int g_PathDiv = 6;

bool operator == (const Coord& a, const Coord &b)
{
   return (a.X == b.X) && (a.Y == b.Y);
}

// ===========================================================================
// NODES
// ===========================================================================

bool operator > (const Node& a, const Node& b)
{
  // To compare two nodes, we compare the `f' value, which is the
  // sum of the g and h values.
  return (a.gval+a.hval) > (b.gval+b.hval);
}

bool operator == (const Node& a, const Node& b)
{
  // Two nodes are equal if their components are equal
  return (a.coord == b.coord) && (a.gval == b.gval) && (a.hval == b.hval);
}

// ===========================================================================
// MARK ARRAY
// ===========================================================================

MarkArray::MarkArray (int sizex) : m_SizeX (sizex) {}
MarkArray::~MarkArray () {}

  // Erase the mark array, for all items in open or visited
void
MarkArray::Empty (Container &open, Container &visited)
{
  for( Container::iterator o = open.begin(); o != open.end(); ++o )
  {
    Marking &m = D(o->coord);
    m.iPrev.X = NONE;
    m.f = -1;
    m.g = -1;
  }

  for (Container::iterator v = visited.begin(); v != visited.end(); ++v)
  {
    Marking &m = D(v->coord);
    m.iPrev.X = NONE;
    m.g = -1;
  }
}

void
MarkArray::Empty (int n)
{
  m_Marks.reserve (n);

  for (int i = 0; i < n; i++)
    m_Marks[i] = Marking();
}

// ===========================================================================
// HEURISTIC
// ===========================================================================

Heuristic::Heuristic () : abort_path (5000)
{}

int
Heuristic::dist(AStar *astar, Coord a, Coord b)
{  
   int dist = 10 * (abs (a.X - b.X) + abs (a.Y - b.Y));

  // One step on the map is 10 in this function
  return dist;
}

int
Heuristic::cost(AStar *astar, Coord a, Coord b)
{
  // FIXME: Why this +10 ? teuf
   return astar->m_Costs[b.Y * astar->m_SizeX + b.X] + 10;
}

// ===========================================================================
/// ASTAR PATHFINDER
// ===========================================================================
AStar::AStar (uchar *data, int sizex, int sizey) :
   m_Mark (sizex),
   m_Costs (data),
   m_SizeX (sizex),
   m_SizeY (sizey)
{
  m_Mark.Empty (sizex*sizey);
}

AStar::~AStar ()
{
   delete[] m_Costs;
}


Container::iterator
AStar::find_in_open (const Coord &c)
{
  // Only search for this node if we know it's in the OPEN set
  if (is_valid(c) && in_open(c))
  {
    for(Container::iterator i = m_Open.begin(); i != m_Open.end(); ++i)
    {
      m_Stats.nodes_searched++;
      if( (*i).coord == c )
        return i;
    }
  }

  return m_Open.end();
}

#if 0
static int Neighbour[8][2] = 
{ {-1, -1}, { 0, -1}, { 1, -1},
  {-1,  0},           { 1,  0},
  {-1,  1}, { 0,  1}, { 1,  1} };
#endif

void
AStar::find_path (const Coord &a, const Coord &b, std::vector<Coord>& path)
{
  if (!reachable (a, b))
     return;

  // Clear stats
  m_Stats = PathStats();

  m_A = a;
  m_B = b;

  Node N;
  {
     // insert the original node
     N.coord = m_A;
     N.gval = 0;
     N.hval = m_Heuristic.dist (this, m_A, m_B);
     
     m_Open.push_back(N);
     m_Mark.D(m_A).f = (int)(N.gval+N.hval);
     m_Mark.D(m_A).g = (int)(N.gval);
     
     m_Stats.nodes_added++;
  }
  
  // * Things in OPEN are in the open container (which is a heap),
  //   and also their mark[...].f value is nonnegative.
  // * Things in CLOSED are in the visited container (which is unordered),
  //   and also their mark[...].direction value is not DirNone.
  
  // While there are still nodes to visit, visit them!
  while (!m_Open.empty())
  {
     get_first (m_Open, N);

     m_Mark.D (N.coord).f = -1;
     m_Visited.push_back (N);
     m_Stats.nodes_removed++;
    
     // If we're at the goal, then exit
     if (N.coord == m_B)
	break;
     
     // If we've looked too long, then exit
     if (m_Stats.nodes_removed >= m_Heuristic.abort_path)
     {
	// Select a good element of OPEN
	for (Container::iterator i = m_Open.begin(); i != m_Open.end(); ++i)
	{
	   if( (*i).hval*2 + (*i).gval < N.hval*2 + N.gval )
	      N = *i;
	}

	m_B = N.coord;
	break;
     }

     // Try to begin researches with nodes avoiding turning.
     int Neighbour[8][2] = 
     { {-1, -1}, { 0, -1}, { 1, -1},
       // 0         1         2
       {-1,  0},           { 1,  0},
       // 3                   4
       {-1,  1}, { 0,  1}, { 1,  1}
       // 5         6         7
     };

     static int BsNE[] = {0, 1, 3, 2, 5, 4, 6, 7};
     static int BsE[]  = {3, 0, 5, 1, 6, 2, 7, 4};
     static int BsSE[] = {5, 3, 6, 0, 7, 1, 4, 2};
     static int BsN[]  = {1, 0, 2, 3, 4, 5, 7, 6};
     static int BsS[]  = {6, 5, 7, 3, 4, 0, 2, 1};
     static int BsNW[] = {2, 1, 4, 0, 7, 3, 6, 5};
     static int BsW[]  = {4, 2, 7, 1, 6, 0, 5, 3};
     static int BsSW[] = {7, 6, 4, 2, 5, 1, 3, 0};
     static int TaddedCost[] = {-5, 0, 0, 20, 20, 40, 40};
     static int NaddedCost[] = {0, 0, 0, 0, 0, 0, 0, 0};

     int *Bs = BsNE;
     int *addedCost = NaddedCost;
     int nb = 8;
     
     Marking &mN = m_Mark.D (N.coord);
     if (mN.iPrev.X != NONE)
     {
	int dx = N.coord.X - mN.iPrev.X;
	int dy = N.coord.Y - mN.iPrev.Y;

	nb = 7;
	if (dx == -1 && dy == -1)      Bs = BsNE;
	else if (dx == -1 && dy == 0)  Bs = BsE;
	else if (dx == -1 && dy == 1)  Bs = BsSE;
	else if (dx == 0 && dy == -1)  Bs = BsN;
	else if (dx == 0 && dy == 1)   Bs = BsS;
	else if (dx == 1 && dy == -1)  Bs = BsNW;
	else if (dx == 1 && dy == 0)   Bs = BsW;
	else if (dx == 1 && dy == 1)   Bs = BsSW;

	addedCost = TaddedCost;
     }

     
     // Look at your neighbors.
     for (int i = 0; i < nb; i++)
     {
	Coord c (N.coord.X + Neighbour[Bs[i]][0],
		 N.coord.Y + Neighbour[Bs[i]][1]);

	int k = m_Heuristic.cost (this, N.coord, c) + addedCost[i];
	
	// Can't walk on this type of ground.
	// FIXME: I'd do this test only on m_Heuristic.cost
	// but it's not a big issue
	if (k > 200)
	   continue;

	Node N2;
	N2.coord = c;
	N2.gval = N.gval + k;
	N2.hval = m_Heuristic.dist (this, c, m_B);
	
	Marking &mark = m_Mark.D (c);
	
	// If this spot (c) hasn't been visited, its mark is NONE
	if (mark.iPrev.X == NONE)
	{
	   // The space is not marked
	   mark.iPrev = N.coord;
	   mark.f = (int)(N2.gval+N2.hval);
	   mark.g = (int)(N2.gval);
	   
	   m_Open.push_back (N2);
	   std::push_heap (m_Open.begin(), m_Open.end(), m_Comp);
	   m_Stats.nodes_added++;
	}
	else
	{
	   // We know it's in OPEN or CLOSED...
	   if (in_open(c))
	   {
	      // It's in OPEN, so figure out whether g is better
	      if (N2.gval < m_Mark.D (c).g)
	      {
		 // Search for c in open
		 Container::iterator find1 = find_in_open (c);
		 
		 if (find1 == m_Open.end())
		   // FIXME: this should not happen... just fixes a
		   // lock.
		   return;
		 
		 // Replace *find1's gval with N2.gval in the list&map

		 Marking &mark = m_Mark.D (c);
		 mark.iPrev = N.coord;
		 mark.g = (int)(N2.gval);
		 mark.f = (int)(N2.gval+N2.hval);
		 
		 (*find1).gval = N2.gval;
		 
		 // This is allowed but it's not obvious why:
		 // (It comes from the way you store heaps in an array - teuf)
		 std::push_heap (m_Open.begin(), find1+1, m_Comp );
	      }
	   }
	}
     }
  }
  
  if( N.coord == m_B)
  {
     m_Stats.path_cost = N.gval;
     // We have found a path, so let's copy it into `path'
     Coord c = m_B;
     
     while (!(c == m_A))
     {
	Coord nc = m_Mark.D(c).iPrev;
	path.push_back(c);
	
	c = nc;
	m_Stats.path_length++;
     }
     
     path.push_back(m_A);
     // path now contains the squares in which the unit must travel ..
     // backwards (like a stack)
  }
  
  m_Stats.nodes_left = m_Open.size();
  m_Stats.nodes_visited = m_Visited.size();

  m_Mark.Empty(m_Open, m_Visited);
  
  // make open & visited sets empty
  m_Open.clear ();
  m_Visited.clear ();
}

bool
AStar::reachable (const Coord &a, const Coord &b)
{
   if (m_Costs[b.Y * m_SizeX + b.X] < 200)
      return true;
   else
      return false;
}

}