#include "../nav.h"
#include "../../core/types.h"
#include <stdio.h>

#define MAX_UNFINISHED 128

typedef struct navSearchNodeState navSearchNodeState;
struct navSearchNodeState{
  bool visited;
  U64 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(int i = 0; i < meshSize; i++) {
    state.nodeStates[i].visited = false;
    state.nodeStates[i].addedToUnvisited = false;
    // underflow to the max :) 
    state.nodeStates[i].distance = -1;
    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 = -1;
  U32 lowestI = -1;
  bool startFound = false;
  for(int 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 = 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) {
  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;}
      U32 distance = state.nodeStates[lowestNodeIndex].distance + connection.Cost;
      distance += mesh.nodes[end].pos.x - mesh.nodes[connection.NodeIndex].pos.x;
      distance += 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;
}