#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         = U64_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;
}