#include "game/nav.h"
#include "core/types.h"

#include <stdio.h>

#define MAX_UNFINISHED 128

typedef struct navSearchNodeState navSearchNodeState;
struct navSearchNodeState
{
  bool visited;
  F64 distance;
  U32 shortest;
  bool addedToUnvisited;
};

typedef struct navSearchState navSearchState;
struct navSearchState
{
  navSearchNodeState nodeStates[NAV_MAX_NODES];
};

navSearchState initState(U32 start, U32 meshSize)
{
  navSearchState state;
  for (U32 i = 0; i < meshSize; i++)
  {
    state.nodeStates[i].visited = false;
    state.nodeStates[i].addedToUnvisited = false;
    state.nodeStates[i].distance = F64_MAX;
    state.nodeStates[i].shortest = 0;
  }
  state.nodeStates[start].distance = 0;
  return state;
}

U32 getLowestState(U32 unfinishedIndexes[128], U32 unfinishedCount, navSearchState state, U32 *offset)
{
  U32 lowest = U32_MAX;
  U32 lowestI = U32_MAX;
  bool startFound = false;
  for (U32 i = *offset; i < unfinishedCount; i++)
  {
    navSearchNodeState checkNode = state.nodeStates[unfinishedIndexes[i]];
    if (checkNode.visited)
    {
      if (!startFound)
      {
        *offset = i;
      }
      continue;
    }
    startFound = true;
    if (lowest > checkNode.distance)
    {
      lowest = cast(U32) checkNode.distance;
      lowestI = unfinishedIndexes[i];
    }
  }
  return lowestI;
}

// Generate a path to follow between the start and end node.
NavPath Nav_Path(NavMesh *mesh, U32 start, U32 end) {
  // This is a stupid fix, since it's easier to work backwards
  // to generate a path, I'm swapping the start / end, to generate
  // it backwards
  U32 tmp = end;
  end = start;
  start = tmp;
  navSearchState state = initState(start, mesh->nodeCount);
  U32 unfinishedCount = 1;
  U32 unfinishedIndexes[NAV_MAX_NODES] = {start};
  // I don't want to spend time removing items from
  // the unfinished nodes, so when checking for a lowest
  // if I find the first N items have been checked, I'll mark
  // an offset to skip the first N items.
  U32 unfinishedOffset = 0;
  U32 lowestNodeIndex = start;
  bool found = false;
  while(!found) {
    for(int connectionI = 0 ; connectionI < mesh->nodes[lowestNodeIndex].connectionCount; connectionI++) {
      NavConnection      *connection = &mesh->nodes[lowestNodeIndex].connections[connectionI];
      navSearchNodeState *testNode   = &state.nodeStates[connection->NodeIndex];
      if(testNode->visited) {continue;}
      F64 distance = cast(F64) (state.nodeStates[lowestNodeIndex].distance + connection->Cost);
      distance += cast(F64) Abs((mesh->nodes[end].pos.x - mesh->nodes[connection->NodeIndex].pos.x));
      distance += cast(F64) Abs((mesh->nodes[end].pos.y - mesh->nodes[connection->NodeIndex].pos.y));
      if(testNode->distance > distance) {
        testNode->distance = distance;
        testNode->shortest = lowestNodeIndex;
      }
      if (!testNode->addedToUnvisited)
      {
        unfinishedIndexes[unfinishedCount] = connection->NodeIndex;
        unfinishedCount++;
        testNode->addedToUnvisited = true;
      }
    }
    state.nodeStates[lowestNodeIndex].visited = true;
    lowestNodeIndex = getLowestState(unfinishedIndexes, unfinishedCount, state, &unfinishedOffset);
    if (lowestNodeIndex == end)
    {
      found = true;
    }
  }
  NavPath res_path = {0};
  U32 index = end;
  while (index != start)
  {
    res_path.indexes[res_path.nodeCount] = index;
    res_path.nodeCount++;
    index = state.nodeStates[index].shortest;
  }
  res_path.indexes[res_path.nodeCount] = start;
  res_path.nodeCount++;
  return res_path;
}
