#include <algorithm>
#include <vector>

#include "defs.h"
#include "engine.h"
#include "ai_waypt.h"
#include "ai.h"
#include "console.h"
#include "math.h"
#include "maths.h"
#include "rules.h"

using namespace std;

waypoint waypts[1000];
int numWayPts;

vector<aNode*> aOpenList;
vector<aNode*> aClosedList;

int route;
int routeFlags;

int useWayPts;
int drawWayPts;

inline bool operator<(const aNode& x, const aNode& y)
{
   return x.costG + x.costH < y.costG + y.costH;
}

void addToPath(int waypt, int cost, aNode* previous, int depth, int linkNum, int endP)
{
   aNode* node = NULL;
   vector<aNode*>::iterator cur;

//   Console.WriteF("addToPath(%d,%d,%d,%d)", waypt, cost, depth, linkNum);

   // Check whether node at (x,y) is in open or closed list 
   // and the cost cannot be reduced.
   // If so immediately return to avoid double processing.
   for ( cur = aClosedList.begin(); cur != aClosedList.end(); )
   {
     if (waypt == (*cur)->waypoint)
     {
       // Remove item from list
       if ((*cur)->costG > cost)
       {
         node = *cur;
         cur = aClosedList.erase(cur);
         break;
       }
       // or return if no shorter path is found
       else return;
     }
     cur++;
   }

   // Do the same for the open list
   if (!node)
   for ( cur = aOpenList.begin(); cur != aOpenList.end(); )
   {
     if (waypt == (*cur)->waypoint)
     {
       // Remove item from list
       if ((*cur)->costG > cost)
       {
         node = *cur;
         cur = aOpenList.erase(cur);
         break;
       }
       // or return if no shorter path is found
       else return;
     }
     cur++;
   }

   // Create new node or use found one from list
   if (!node)
   {
      node = new aNode();
   }
   
   node->previous = previous;       // Parent node
   node->waypoint = waypt;          // Store position
   node->costG    = cost;           // cost from start
   node->costH    = INT(hypot(waypts[endP].x - waypts[waypt].x, waypts[endP].y - waypts[waypt].y)); // Guessed cost to target
   node->depth    = depth;
   node->linkNum  = linkNum;

   // Insert node into open list. We do this sorted with the overall
   // cost_F at the index idx (which has to be determined of course!)
   // as the node with the least cost has to be processed next and we
   // gain speed by doing this sorting/searching just once.
   aOpenList.push_back(node);
   sort(aOpenList.begin(), aOpenList.end());
}


int chain[100];
int chainFlags[100];

/*
int chainLen;
int pathPos = 0;
float bestLen;

int addToPath(int i, int flags)
{
   if (pathPos < 100)
   {
      chain[pathPos] = i;
      chainFlags[pathPos] = flags;
      pathPos++;
      return 1;
   }
   else
      return 0;
}

void dropFromPath()
{
   pathPos--;
}

void copyPath()
{
   route = chain[1];
   routeFlags = chainFlags[1];
}

int findChain(int startP, int endP, float len)
{
   int i;
   char found = 0;
   float curLen;

   for (i=0; i<4; i++) // check if we have a direct link
   if (waypts[startP].links[i].link != -1)
   if ((!waypts[startP].links[i].flags & AI_FLAG_ROPE) || (!tactics))
   if (!waypts[waypts[startP].links[i].link].used) // prevent dead loops
   {
      curLen = len + waypts[startP].links[i].dist;

      if (curLen >= bestLen) // already got a shorter path
      { 
         return 0;
      }

      if (waypts[startP].links[i].link == endP)
      {
         if (addToPath(endP, waypts[startP].links[i].flags))
         {
            copyPath(); // save the path
            bestLen = curLen;
            dropFromPath();
         }
         found = 1;
      }
      else
      {
         if (addToPath(waypts[startP].links[i].link, waypts[startP].links[i].flags))
         {
            waypts[waypts[startP].links[i].link].used = 1;
            if (findChain(waypts[startP].links[i].link, endP, curLen))
               found = 1;
            waypts[waypts[startP].links[i].link].used = 0;
            dropFromPath();
         }
      }
   }
   return found;
}


int findWay(int startP, int endP)
{
   int found;
   bestLen = 99999999;
   pathPos = 0;
   waypts[startP].used = 1;
   addToPath(startP, 0);
   found = findChain(startP, endP, 0);
   dropFromPath();
   waypts[startP].used = 0;
   return found; 
}
*/

void clearLists()
{
   vector<aNode*>::iterator cur;

   for ( cur = aClosedList.begin(); aClosedList.size(); )
   {
      delete (*cur);
      cur = aClosedList.erase(cur);
   }

   // Do the same for the open list
   for ( cur = aOpenList.begin(); aOpenList.size(); )
   {
      delete (*cur);
      cur = aOpenList.erase(cur);
   }

}

int findWay(int startP, int endP)
{
   vector<aNode*>::iterator cur;

   addToPath(startP, 0, NULL, 0, 0, endP);

//   Console.WriteF("findWay(%d,%d)", startP, endP);
   
   while (aOpenList.size() > 0)
   {
      cur = aOpenList.begin();
      aNode* here = *cur;
      
      if (here->waypoint == endP)
      {
//         Console.WriteF("found a way!");
         aNode* point = here;
         while (point)
         {
            chain[point->depth] = point->waypoint;
            chainFlags[point->depth] = 0;
            if (point->previous)
               chainFlags[point->depth] = waypts[point->previous->waypoint].links[point->linkNum].flags;
            point = point->previous;
         }
//         for (int i = 1; i <= here->depth; i++)
//         {
//            Console.WriteF("Route point %d is %d", i, chain[i]);
//         }
         route = chain[1];
         routeFlags = chainFlags[1];
         clearLists();
         return 1;
      }
      
      aClosedList.push_back(here);
      aOpenList.erase(cur);
      
      for (int i = 0; i < 4; i++)
      {
         if (waypts[here->waypoint].links[i].link != -1)
         {
            addToPath(waypts[here->waypoint].links[i].link, here->costG + INT(waypts[here->waypoint].links[i].dist), here, here->depth + 1, i, endP);
         }
      }
   }

//   Console.WriteF("no way found!");

   clearLists();
   return 0;
}

int findNearestPt(int x, int y)
{
   double dist, tmp;
   int pt;
   int i;
   dist = 900;
   pt = -1;
   
   if (!useWayPts) return -1;
   
   for (i=0; i<1000; i++)
   if (waypts[i].active)
   {
      tmp = hypot(x - waypts[i].x, y - waypts[i].y);
      if (tmp < dist)
      {
         traceLine(x, y + Engine.wormFireOffs, waypts[i].x, waypts[i].y, 0);

         if (trace)
         {
            dist = tmp;
            pt = i;
         }
      }
   }
   else
      break;

   return pt;
}

int findSafePt(worm *tmpWorm, int x, int y)
{
   double dist, tmp;
   int pt;
   int i, j;
   dist = 9999;
   pt = -1;
   for (i=0; i<1000; i++)
   if (waypts[i].active)
   if (waypts[i].flags & AI_WYPT_SAFE)
   {
      tmp = hypot(x - waypts[i].x, y - waypts[i].y);
      traceLine(x, y + Engine.wormFireOffs, waypts[i].x, waypts[i].y, 0);

      if (trace)
      {
         trace = 0;
         for (j=0; j<GP_NUM_PLAYERS; j++)
         if (!trace)
         if (worms[j].active)
         if (worms[j].health>0)
         if ((!teamplay) || (worms[j].team != tmpWorm->team))
            traceLine(waypts[i].x, waypts[i].y, worms[j].coords.x, worms[j].coords.y + 4, 0);

         if (!trace)   
         if (tmp < dist)
         {
            dist = tmp;
            pt = i;
         }
      }
   }
   return pt;
}

int findBotPath(int a, int b, int *point, int *flags)
{
   int sP, eP;
   int i;
   int found;
   sP = a;
   eP = b;
   if ((sP == -1) || (eP == -1))
      return 0;
   if (sP == eP)
   {
      *point = sP;
      *flags = AI_FLAG_JUMP | AI_FLAG_ROPE;
      return 1;
   }
   found = findWay(sP, eP);
   if (found)
   {
      *point = route;
      *flags = routeFlags;
   }
   return found;
}

int initWaypoints(char *name) {
   int i;
   
   sprintf(buf, "lev/%s/waypoint.cfg", name);
   override_config_file(buf);

   numWayPts = get_config_int("Map", "NumWayPoints", 0);
   drawWayPts = get_config_int("Map", "DrawPoints", 0);

   for (i=0; i<1000; i++)
      waypts[i].active = 0;

   for (i=0; i<numWayPts; i++)
   {
      sprintf(buf, "Point.%d", i);
      waypts[i].active = 1;
      waypts[i].used = 0;
      waypts[i].x = get_config_int(buf, "X", 0);
      waypts[i].y = get_config_int(buf, "Y", 0);
      waypts[i].flags = get_config_int(buf, "Flags", 0);
      waypts[i].links[0].link = get_config_int(buf, "Link0.Dest", -1);
      waypts[i].links[0].flags = get_config_int(buf, "Link0.Flags", 0);
      waypts[i].links[1].link = get_config_int(buf, "Link1.Dest", -1);
      waypts[i].links[1].flags = get_config_int(buf, "Link1.Flags", 0);
      waypts[i].links[2].link = get_config_int(buf, "Link2.Dest", -1);
      waypts[i].links[2].flags = get_config_int(buf, "Link2.Flags", 0);
      waypts[i].links[3].link = get_config_int(buf, "Link3.Dest", -1);
      waypts[i].links[3].flags = get_config_int(buf, "Link3.Flags", 0);
   }
   for (i=0; i<numWayPts; i++)
   {
      if (waypts[i].links[0].link != -1)
      {
         waypts[i].links[0].dist = hypot(waypts[i].x - waypts[waypts[i].links[0].link].x, waypts[i].y - waypts[waypts[i].links[0].link].y);
      }
      if (waypts[i].links[1].link != -1)
      {
         waypts[i].links[1].dist = hypot(waypts[i].x - waypts[waypts[i].links[1].link].x, waypts[i].y - waypts[waypts[i].links[1].link].y);
      }
      if (waypts[i].links[2].link != -1)
      {
         waypts[i].links[2].dist = hypot(waypts[i].x - waypts[waypts[i].links[2].link].x, waypts[i].y - waypts[waypts[i].links[2].link].y);
      }
      if (waypts[i].links[3].link != -1)
      {
         waypts[i].links[3].dist = hypot(waypts[i].x - waypts[waypts[i].links[3].link].x, waypts[i].y - waypts[waypts[i].links[3].link].y);
      }
   }
   Console.WriteF("Loaded %d waypoints...", numWayPts);
   useWayPts = (numWayPts > 0);
   return 0;
}

