/**
  @file /ai/PathFind/pathfindskelet.cpp
  @brief Zdrojovy kod s implementaci obecne kostry A* algoritmu na vyhledavani cesty.
  
  Zdrojovy kod s implementaci obecne kostry A* algoritmu na vyhledavani cesty.
  
  @author PZ
  @version 0.1
*/

#include "ai/PathFind/pathfindskelet.h"

namespace ai_ns
{
namespace pathfind_ns
{

CPathFindSkelet::CPathFindSkelet()
{
  Open=0;
  Closed=0;
  OpenHeap.root=0;
  OpenHeap.last_added_node=0;
  ClosedHeap.root=0;
  ClosedHeap.last_added_node=0;
  HeapNodesMap=0;
}

CPathFindSkelet::~CPathFindSkelet()
{
  if (Open) freeList(Open);
  Open=0;
  if (Closed) freeList(Closed);
  Closed=0;
  if (!heapIsEmpty(OpenHeap)) freeHeap(OpenHeap);
  if (!heapIsEmpty(ClosedHeap)) freeHeap(ClosedHeap);
  if (HeapNodesMap) KMemFree(HeapNodesMap);
  HeapNodesMap=0;
}

void CPathFindSkelet::initMarkMap()
{
  int i;
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 44 pathfindskelet");
  for(i=0;i<(getMapWidth()*getMapHeight());i++) mm[i]=false;
}

void CPathFindSkelet::initMarkMapForFlood(PF_MARKMAP &gmmff)
{
  int i;
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 51 pathfindskelet");
  for(i=0;i<(getMapWidth()*getMapHeight());i++) mmff[i]=gmmff[i];
}

TNode* CPathFindSkelet::createNode(COORDS & su,int g,int h,TNode* _parent)
{
  TNode* nu=(TNode*) KMemAlloc(sizeof(TNode));
  nu->souuz=su;
  nu->g=g;
  nu->h=h;
  nu->f=g+h;
  nu->parent=_parent;
  nu->next=0;
  return nu;
}

void CPathFindSkelet::addToOpen(TNode* au)
{
  TNode* pruk=Open;

  while ((pruk) && (!(COORDSSAME(au->souuz,pruk->souuz)))) pruk=pruk->next;   
  if (!pruk)
  {
    au->next=Open;
    Open=au;
  }
  else
  {
    if (pruk->f>au->f)
    {
      pruk->g=au->g;
      pruk->h=au->h;
      pruk->f=au->f;
      pruk->parent=au->parent;
    }
    KMemFree(au);
  }
}

void CPathFindSkelet::moveToClosed(TNode* mu)
{
  TNode* pruk=Open;
  if (mu==Open)
  {
    Open=mu->next;
  }
  else
  {
    while (pruk->next!=mu) pruk=pruk->next;
    pruk->next=mu->next;
  }
  mu->next=Closed;
  Closed=mu;
}

TNode* CPathFindSkelet::bestNode()
{
  TNode* nej=Open;
  TNode* pruk=Open;

  if (!Open) return 0;
  while (pruk)
  {
    if (pruk->f<nej->f) nej=pruk;
    pruk=pruk->next;
  }
  return nej;
}

void CPathFindSkelet::freeList(TLIST & lh)
{
  TNode* lpr;
  while (lh)
  {
    lpr=lh;
    lh=lh->next;
    KMemFree(lpr);
  }
  lh=0;
}

THeapNodeValues* CPathFindSkelet::createHeapNode(COORDS & su,int g,int h,THeapNodeValues* _parent)
{
  THeapNodeValues* nhnv=(THeapNodeValues*) KMemAlloc(sizeof(THeapNodeValues));
  nhnv->souuz=su;
  nhnv->g=g;
  nhnv->h=h;
  nhnv->f=g+h;
  nhnv->parent=_parent;
  nhnv->heap_node_pointer=0;
  return nhnv;
}

void CPathFindSkelet::addToHeap(THEAP& heap,THeapNodeValues* added_heap_node_values,bool CheckHeapNodesMap)
{
  THeapNode* added_heap_node=(THeapNode*) KMemAlloc(sizeof(THeapNode));
  added_heap_node_values->heap_node_pointer=(void*) added_heap_node;
  added_heap_node->values=added_heap_node_values;
  added_heap_node->next=0;
  added_heap_node->prev=0;
  added_heap_node->left=0;
  added_heap_node->right=0;
  added_heap_node->heap_parent=0;
  
  THeapNodeValues* pExHN=HeapNodesMap[COORDS_TRANSFORM_FUNCTION(added_heap_node->values->souuz)];
  if ((CheckHeapNodesMap) && (pExHN)) // existuje uzel s hodnotami se stejnymi souradnicemi
  {
    if (added_heap_node->values->f<pExHN->f)
    {
      pExHN->f=added_heap_node->values->f;
      pExHN->g=added_heap_node->values->g;
      pExHN->h=added_heap_node->values->h;
      pExHN->parent=added_heap_node->values->parent;

      // konsolidace haldy
      THeapNode* prHNP=(THeapNode*) (pExHN->heap_node_pointer);
      while ((prHNP!=heap.root) && (prHNP->values->f<prHNP->heap_parent->values->f))
      {
        THeapNodeValues* tmpHNValues=prHNP->values;
        prHNP->values=prHNP->heap_parent->values;
        prHNP->heap_parent->values=tmpHNValues;
        prHNP->values->heap_node_pointer=(void*) prHNP;
        prHNP->heap_parent->values->heap_node_pointer=(void*) (prHNP->heap_parent);
        prHNP=prHNP->heap_parent;
      }
    }
    KMemFree(added_heap_node->values);
    KMemFree(added_heap_node);
  }
  else
  {
    if (CheckHeapNodesMap) HeapNodesMap[COORDS_TRANSFORM_FUNCTION(added_heap_node->values->souuz)]=added_heap_node->values;
    added_heap_node->left=0;
    added_heap_node->right=0;
    added_heap_node->next=0;
    added_heap_node->prev=heap.last_added_node;
    if (heapIsEmpty(heap)) // halda je prazdna
    {
      added_heap_node->heap_parent=0;
      added_heap_node->prev=0; // pro jistotu
      heap.root=added_heap_node;
      heap.last_added_node=added_heap_node;
    }
    else
    {
      if (heap.last_added_node==heap.root) // v halde je jediny uzel
      {
        added_heap_node->heap_parent=heap.root;
        (heap.root)->left=added_heap_node;
      }
      else
      {
        if ((heap.last_added_node)->heap_parent->right)
        {
          added_heap_node->heap_parent=(heap.last_added_node)->heap_parent->next;
          added_heap_node->heap_parent->left=added_heap_node;
        }
        else
        {
          added_heap_node->heap_parent=(heap.last_added_node)->heap_parent;
          (heap.last_added_node)->heap_parent->right=added_heap_node;
        }
      }
      (heap.last_added_node)->next=added_heap_node;
      heap.last_added_node=added_heap_node;
    }

    // konsolidace haldy
    THeapNode* prHNP=heap.last_added_node;
    while ((prHNP!=heap.root) && (prHNP->values->f<prHNP->heap_parent->values->f))
    {
      THeapNodeValues* tmpHNValues=prHNP->values;
      prHNP->values=prHNP->heap_parent->values;
      prHNP->heap_parent->values=tmpHNValues;
      prHNP->values->heap_node_pointer=(void*) prHNP;
      prHNP->heap_parent->values->heap_node_pointer=(void*) (prHNP->heap_parent);
      prHNP=prHNP->heap_parent;
    }

  }
}

THeapNodeValues* CPathFindSkelet::popHeapRoot(THEAP& heap)
{
  if (heapIsEmpty(heap)) return 0;
  if (heap.last_added_node==heap.root) // v halde je jediny uzel
  {
    THeapNodeValues* rv=(heap.root)->values;
    KMemFree(heap.root);
    heap.root=0;
    heap.last_added_node=0;
    return rv;
  }
  
  THeapNode* rv=heap.last_added_node;
  if (rv->heap_parent->left==rv) // posledni uzel byl levym potomkem
  {
    rv->heap_parent->left=0;
  }
  else // posledni uzel byl pravym potomkem
  {
    rv->heap_parent->right=0;
  }
  heap.last_added_node=(heap.last_added_node)->prev;
  (heap.last_added_node)->next=0;
  rv->heap_parent=0;
  rv->prev=0;

  THeapNodeValues* tmpHNValues=rv->values;
  rv->values=(heap.root)->values;
  (heap.root)->values=tmpHNValues;
  rv->values->heap_node_pointer=(void*) rv;
  (heap.root)->values->heap_node_pointer=(void*) (heap.root);
  
  THeapNode* prHNP=heap.root;
  while (((prHNP->left) && (prHNP->right)) && ((prHNP->values->f>prHNP->left->values->f) || (prHNP->values->f>prHNP->right->values->f)))
  {
    if (prHNP->left->values->f<prHNP->right->values->f)
    {
      tmpHNValues=prHNP->values;
      prHNP->values=prHNP->left->values;
      prHNP->left->values=tmpHNValues;
      prHNP->values->heap_node_pointer=(void*) prHNP;
      prHNP->left->values->heap_node_pointer=(void*) (prHNP->left);
      prHNP=prHNP->left;
    }
    else
    {
      tmpHNValues=prHNP->values;
      prHNP->values=prHNP->right->values;
      prHNP->right->values=tmpHNValues;
      prHNP->values->heap_node_pointer=(void*) prHNP;
      prHNP->right->values->heap_node_pointer=(void*) (prHNP->right);
      prHNP=prHNP->right;
    }
  }
  if ((prHNP->left) && (prHNP->values->f>prHNP->left->values->f)) // pro pripad,ze posledni odebirany uzel byl (na zacatkuteto metody) pravym potomkem
  {
    tmpHNValues=prHNP->values;
    prHNP->values=prHNP->left->values;
    prHNP->left->values=tmpHNValues;
    prHNP->values->heap_node_pointer=(void*) prHNP;
    prHNP->left->values->heap_node_pointer=(void*) (prHNP->left);
  }
  
  THeapNodeValues* realrv=rv->values;
  KMemFree(rv);
  return realrv;
}

bool CPathFindSkelet::heapIsEmpty(THEAP& heap)
{
  if (!(heap.root)) return true;
  return false;
}

void CPathFindSkelet::freeHeapTree(THeapNode* & heap_root)
{
  if (!heap_root) return;
  freeHeapTree(heap_root->left);
  freeHeapTree(heap_root->right);
  KMemFree(heap_root->values);
  KMemFree(heap_root);
}

void CPathFindSkelet::freeHeap(THEAP& heap)
{
  freeHeapTree(heap.root);
  heap.root=0;
  heap.last_added_node=0;
}
TPATH CPathFindSkelet::astarSimple(int unit_id,COORDS & start,COORDS & target,const TPathFindTransparency & pathfind_transparency)
{
  TNode* bu;
  int costtemp;
  TPATH rp;
  TNEIGHBOURS sez_sou;
  TNEIGHBOURS_ITERATOR sez_sou_it;

  pft=pathfind_transparency;
  target_hex_coords=target;
  rp.clear();
  if (!((validSou(start)) && (validSou(target)))) return rp;
  freeList(Open);
  freeList(Closed);
  initMarkMap();
  addToOpen(createNode(start,0,heuristicFunction(start,target),0));
  do
  {
    bu=bestNode();
    sez_sou=chooseNeighbours(bu->souuz);
    for(sez_sou_it=sez_sou.begin();sez_sou_it!=sez_sou.end();sez_sou_it++)
    {
      if (MARKMAP_CELL(mm,(*sez_sou_it))==false)
      {
        costtemp=getCost(unit_id,bu->g,bu->souuz,(*sez_sou_it));
        if (costtemp>0) addToOpen(createNode((*sez_sou_it),(bu->g)+costtemp,heuristicFunction((*sez_sou_it),target),bu));
      }
    }
    sez_sou.clear();
    moveToClosed(bu);
    MARKMAP_CELL(mm,bu->souuz)=true;
  }
  while ((Open) && (!(COORDSSAME(bu->souuz,target))));
  if ((!Open) && (!(COORDSSAME(bu->souuz,target)))) 
  {
    freeList(Closed);
    return rp;
  }

  while (bu)
  {
    PATH_COORDS pathpoint;
    pathpoint.x=(bu->souuz).x;
    pathpoint.y=(bu->souuz).y;
    pathpoint.whole_path_cost=bu->g;
    if (bu->parent)
      pathpoint.step_cost=bu->g-bu->parent->g;
    else
      pathpoint.step_cost=0;
    rp.push_front(pathpoint);
    bu=bu->parent;
  }

  freeList(Open);
  freeList(Closed);
  // zvazit vypusteni posledniho vrcholu ... da se jednoduse zaridit v predchozi while podmince testem na NULL rodice bu
  return rp;
}

TPATH CPathFindSkelet::astarHeaps(int unit_id,COORDS & start,COORDS & target,const TPathFindTransparency & pathfind_transparency)
{
  TPATH rp;
  int hnmIt;
  if (!HeapNodesMap) HeapNodesMap=(THeapNodeValues**) KMemAlloc(getMapWidth()*getMapHeight()*sizeof(THeapNodeValues*));
  for(hnmIt=0;hnmIt<getMapWidth()*getMapHeight();hnmIt++) HeapNodesMap[hnmIt]=0;
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 387 pathfindskelet");


  THeapNodeValues* bu;
  int costtemp;
  TNEIGHBOURS sez_sou;
  TNEIGHBOURS_ITERATOR sez_sou_it;

  pft=pathfind_transparency;
  target_hex_coords=target;
  rp.clear();
  if (!((validSou(start)) && (validSou(target)))) return rp;
  freeHeap(OpenHeap);
  freeHeap(ClosedHeap);
  initMarkMap();
  addToHeap(OpenHeap,createHeapNode(start,0,heuristicFunction(start,target),0),true);
  do
  {
    bu=popHeapRoot(OpenHeap);
    sez_sou=chooseNeighbours(bu->souuz);
    for(sez_sou_it=sez_sou.begin();sez_sou_it!=sez_sou.end();sez_sou_it++)
    {
      if (MARKMAP_CELL(mm,(*sez_sou_it))==false)
      {
        costtemp=getCost(unit_id,bu->g,bu->souuz,(*sez_sou_it));
        if (costtemp>0) addToHeap(OpenHeap,createHeapNode((*sez_sou_it),(bu->g)+costtemp,heuristicFunction((*sez_sou_it),target),bu),true);
      }
    }
    sez_sou.clear();
    addToHeap(ClosedHeap,bu,false);
    MARKMAP_CELL(mm,bu->souuz)=true;
  } while ((!heapIsEmpty(OpenHeap)) && (!(COORDSSAME(bu->souuz,target))));
  if ((heapIsEmpty(OpenHeap)) && (!(COORDSSAME(bu->souuz,target)))) 
  {
    freeHeap(ClosedHeap);
    return rp;
  }

  while (bu)
  {
    PATH_COORDS pathpoint;
    pathpoint.x=(bu->souuz).x;
    pathpoint.y=(bu->souuz).y;
    pathpoint.whole_path_cost=bu->g;
    if (bu->parent)
      pathpoint.step_cost=bu->g-bu->parent->g;
    else
      pathpoint.step_cost=0;
    rp.push_front(pathpoint);
    bu=bu->parent;
  }

  freeHeap(OpenHeap);
  freeHeap(ClosedHeap);
  // zvazit vypusteni posledniho vrcholu ... da se jednoduse zaridit v predchozi while podmince testem na NULL rodice bu

// ZRUSIT TOTO ???
/* UNCOMMENT THIS IF THAT MAKES ANY PROBLEM
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 444 pathfindskelet");
  for(hnmIt=0;hnmIt<getMapWidth()*getMapHeight();hnmIt++) 
  {
    HeapNodesMap[hnmIt]=0;
  }
*/
  return rp;
}

void CPathFindSkelet::astarFloodSimple(int unit_id,COORDS & start,PF_MARKMAP &gmmff,FLOOD_MASK &rc,const TPathFindTransparency & pathfind_transparency) throw(E_8K_AI_Pathfind_BadFloodFillStartingPoint)
{
  TNode* bu;
  int i,costtemp;
  TNEIGHBOURS sez_sou;
  TNEIGHBOURS_ITERATOR sez_sou_it;

  if (!(validSou(start))) THROW(E_8K_AI_Pathfind_BadFloodFillStartingPoint,"Invalid starting point for FloodFill.");
  pft=pathfind_transparency;
  target_hex_coords.x=-1;
  target_hex_coords.y=-1;
  freeList(Open);
  freeList(Closed);
  initMarkMapForFlood(gmmff);
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 467 pathfindskelet");
  for(i=0;i<(getMapWidth()*getMapHeight());i++) 
  {
    if (mmff[i])
    {
      rc[i]=FLOOD_MASK_VALUE_CANT_SEE;
    }
    else
    {
      rc[i]=FLOOD_MASK_VALUE_NOT_ACCESSIBLE;
    }
  }

  addToOpen(createNode(start,0,0,0));
  do
  {
    bu=bestNode();
    sez_sou=chooseNeighbours(bu->souuz);
    for(sez_sou_it=sez_sou.begin();sez_sou_it!=sez_sou.end();sez_sou_it++)
    {
      if (MARKMAP_CELL(mmff,(*sez_sou_it))==false)
      {
        costtemp=getCost(unit_id,bu->g,bu->souuz,(*sez_sou_it));
        if (costtemp>0) addToOpen(createNode((*sez_sou_it),(bu->g)+costtemp,0,bu));
      }
    }
    sez_sou.clear();
    moveToClosed(bu);
    MARKMAP_CELL(mmff,bu->souuz)=true;
  }
  while (Open);

  freeList(Open);
  bu=Closed;
  while (bu)
  {
    rc[((bu->souuz).y*(getMapWidth()))+(bu->souuz).x]=bu->g;
    bu=bu->next;
  }
  freeList(Closed);
}

void CPathFindSkelet::astarFloodHeaps(int unit_id,COORDS & start,PF_MARKMAP &gmmff,FLOOD_MASK &rc,const TPathFindTransparency & pathfind_transparency) throw(E_8K_AI_Pathfind_BadFloodFillStartingPoint)
{
  THeapNodeValues* bu;
  int i,costtemp,hnmIt;
  TNEIGHBOURS sez_sou;
  TNEIGHBOURS_ITERATOR sez_sou_it;

  if (!HeapNodesMap) HeapNodesMap=(THeapNodeValues**) KMemAlloc(getMapWidth()*getMapHeight()*sizeof(THeapNodeValues*));
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 517 pathfindskelet");
  for(hnmIt=0;hnmIt<getMapWidth()*getMapHeight();hnmIt++) HeapNodesMap[hnmIt]=0;

  if (!(validSou(start))) THROW(E_8K_AI_Pathfind_BadFloodFillStartingPoint,"Invalidni startovaci bod pro FloodFill.");
  pft=pathfind_transparency;
  target_hex_coords.x=-1;
  target_hex_coords.y=-1;
  freeHeap(OpenHeap);
  freeHeap(ClosedHeap);
//  initMarkMapForFlood(gmmff);
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 527 pathfindskelet");
  for(i=0;i<(getMapWidth()*getMapHeight());i++) 
  {
    if (gmmff[i]) // ZDE
    {
      rc[i]=FLOOD_MASK_VALUE_CANT_SEE;
    }
    else
    {
      rc[i]=FLOOD_MASK_VALUE_NOT_ACCESSIBLE;
    }
  }

  addToHeap(OpenHeap,createHeapNode(start,0,0,0),true);
  do
  {
    bu=popHeapRoot(OpenHeap);
    sez_sou=chooseNeighbours(bu->souuz);
    for(sez_sou_it=sez_sou.begin();sez_sou_it!=sez_sou.end();sez_sou_it++)
    {
      if (MARKMAP_CELL(gmmff,(*sez_sou_it))==false) // ZDE
      {
        costtemp=getCost(unit_id,bu->g,bu->souuz,(*sez_sou_it));
        if (costtemp>0) addToHeap(OpenHeap,createHeapNode((*sez_sou_it),(bu->g)+costtemp,0,bu),true);
      }
    }
    sez_sou.clear();
    addToHeap(ClosedHeap,bu,false);
    MARKMAP_CELL(gmmff,bu->souuz)=true; // ZDE
  } while ((!heapIsEmpty(OpenHeap)));

  freeHeap(OpenHeap);
  THeapNode* buhn=ClosedHeap.root;
  while (buhn)
  {
    rc[((buhn->values->souuz).y*(getMapWidth()))+(buhn->values->souuz).x]=buhn->values->g;
    buhn=buhn->next;
  }
  freeHeap(ClosedHeap);

// ZRUSIT TOTO ??
/* UNCOMMENT THIS IF PROBLEM APPEARS
  GLOBALLOGID(PRIORITY_AI_ALLOC, "for ln 569 pathfindskelet");
  for(hnmIt=0;hnmIt<getMapWidth()*getMapHeight();hnmIt++)
  { 
    HeapNodesMap[hnmIt]=0;
  }
*/
}

}
}