// This is a stand-alone test of the ghost algorithms.
// Not 100% finished yet but the hard parts are in place.
// This should be relatively easy to add to the 6809 code
// as long as it doesn't use too much RAM.

#include <limits.h>  // For INT_MAX
#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>  // For malloc, free

static const char* wallmap0[] = {
    "############################",
    "#      ##          ##      #",
    "# #### ## ######## ## #### #",
    "# #### ## ######## ## #### #",
    "#                          #",
    "### ## ##### ## ##### ## ###",
    "  # ## #   # ## #   # ## #  ",
    "### ## ##### ## ##### ## ###",
    "    ##       ##       ##    ",
    "### ##### ######## ##### ###",
    "  # ##### ######## ##### #  ",
    "  #                      #  ",
    "  # ##### ###  ### ##### #  ",
    "  # ##### #      # ##### #  ",
    "  # ##    #      #    ## #  ",
    "  # ## ## #      # ## ## #  ",
    "### ## ## ######## ## ## ###",
    "       ##          ##       ",
    "### ######## ## ######## ###",
    "  # ######## ## ######## #  ",
    "  #          ##          #  ",
    "  # ##### ######## ##### #  ",
    "### ##### ######## ##### ###",
    "#                          #",
    "# #### ##### ## ##### #### #",
    "# #  # ##### ## ##### #  # #",
    "# #  # ##    ##    ## #  # #",
    "# #  # ## ######## ## #  # #",
    "# #### ## ######## ## #### #",
    "#                          #",
    "############################",
};

#define ROWS (sizeof(wallmap0) / sizeof(wallmap0[0]))
#define LASTROW (ROWS - 1)
#define COLS strlen(wallmap0[0])
#define LASTCOL (COLS - 1)

#define LEFT 0
#define RIGHT 1
#define UP 2
#define DOWN 3
const int directions[4] = {LEFT, RIGHT, UP, DOWN};

// Movements for BFS traversal (LEFT, RIGHT, UP, DOWN)
const int dx[4] = {-1, 1, 0, 0};
const int dy[4] = {0, 0, -1, 1};

// position in the maze plus current direction vector
typedef struct Actor {
  int X, Y;
  int dx, dy;
} Actor;

Actor Player;    // Player's X,Y coordinate
Actor Ghost[4];  // Ghosts' positions

// A basic queue implementation for BFS
typedef struct {
  Actor* data;
  int front, rear, size, capacity;
} Queue;

#ifdef DEBUG
int max = 0; // We can't afford 30x31 matrix in Ram on the 6809.  How much do we actually need?
#endif

static inline int Path(Actor *M, int direction) {  // Is movement possible in the requested direction?
  return wallmap0[M->Y + dy[direction]][M->X + dx[direction]] == ' ';
}

// Our modified Dijkstra algorithm produces truly deadly behaviour by the ghosts -
// but we do not always want them to be so accurate.  I will be writing code to
// implement a less deadly algorithm, roughly based on the original games.

// Code to decide when to switch algorithms remains to be designed.

// Expedient queue management for now - when I move this to the Vectrex it will need to
// be a static array of fixed length, implementing a cyclic buffer.  By then I should have
// a reasonable estimate of the largest size of the array that's needed.

// Function to create a queue
Queue* CreateQueue(int capacity) {
  Queue* queue = (Queue*)malloc(sizeof(Queue));
  queue->capacity = capacity;
  queue->front = queue->size = 0;    // PUT AND GET POINTERS WOULD BE BETTER.  size should not be needed.
  queue->rear = capacity - 1;
  queue->data = (Actor*)malloc(queue->capacity * sizeof(Actor));
  return queue;
}

// Function to enqueue a position into the queue
void Enqueue(Queue* queue, Actor pos) {
  if (queue->size == queue->capacity) {
    fprintf(stderr, "Queue is full.\n");
    exit(EXIT_FAILURE);
  }
  queue->rear = (queue->rear + 1) % queue->capacity;
  queue->data[queue->rear] = pos;
  queue->size++;
#ifdef DEBUG
  if (queue->size > max) max = queue->size;
#endif
}

// Function to dequeue a position from the queue
Actor Dequeue(Queue* queue) {    // returns whole struct on the stack.  Dangerous on the Vectrex with so little RAM
  Actor pos;
  pos = queue->data[queue->front];
  queue->front = (queue->front + 1) % queue->capacity;
  queue->size--;
  return pos;
}

// Function to check if the queue is empty
bool IsQueueEmpty(Queue* queue) { return queue->size == 0; }

int DistanceToPlayer(int ghostno) {  // DistanceToPlayer function using BFS
  int i, j, dir, newX, newY, dist;
  Actor current;
  
  if (wallmap0[Ghost[ghostno].Y][Ghost[ghostno].X] != ' ') {
    fprintf(stderr, "Ghost at %d,%d is not on a path\n", Ghost[ghostno].X, Ghost[ghostno].Y);
    exit(EXIT_FAILURE);
  }
  // If the ghost is already at the player's position, return 0
  if (Ghost[ghostno].X == Player.X && Ghost[ghostno].Y == Player.Y) return 0;

  // Create a queue for BFS and a visited array, plus a distance array to store
  // the number of steps to reach each position - unfortunately distances have
  // to be at least a byte per entry which will take up some space.  At least
  // the 'visited' array can be an array of bits, though if merged with Distance
  // it could be done away with altogether.
  
  Queue* queue = CreateQueue(ROWS * COLS);
  int** distance = (int**)malloc(ROWS * sizeof(int*));
  bool** visited = (bool**)malloc(ROWS * sizeof(bool*));
  for (i = 0; i < ROWS; i++) {
    distance[i] = (int*)malloc(COLS * sizeof(int));
    visited[i] = (bool*)malloc(COLS * sizeof(bool));
    for (j = 0; j < COLS; j++) {
      distance[i][j] = INT_MAX;  // Initialize distances to infinity
      visited[i][j] = false;
    }
  }

  // Enqueue the starting ghost position
  Enqueue(queue, Ghost[ghostno]);
  visited[Ghost[ghostno].Y][Ghost[ghostno].X] = true; // Visited and distance could be merged... (INT_MAX: false  >=0: true)
  distance[Ghost[ghostno].Y][Ghost[ghostno].X] = 0;   // Distance to the starting point is 0
                                                      // to do: check that distances never exceed 255? 127? and see if we can use bytes.

  // Perform BFS
  while (!IsQueueEmpty(queue)) {
    current = Dequeue(queue);

    // Explore all 4 possible directions
    for (dir = 0; dir < 4; dir++) {
      newX = current.X + dx[dir]; newY = current.Y + dy[dir];

      // Check if it's a valid move and if there's a path in that direction
      if ((!visited[newY][newX]) && Path(&current, dir)) {
        
        visited[newY][newX] = true;  // Mark the position as visited
        distance[newY][newX] = distance[current.Y][current.X] + 1;

        // If we reach the player's position, return the distance
        if (newX == Player.X && newY == Player.Y) {
          dist = distance[newY][newX];

#ifdef DEBUG
          fprintf(stderr, "Maximum number of queue elements that will need to be allocated = %d\n", max);
#endif
          // Clean up memory
          free(queue->data);
          free(queue);
          for (i = 0; i < ROWS; i++) {
            free(visited[i]); free(distance[i]);
          }
          free(visited); free(distance);

          return dist;  // Return the shortest distance to the player
        }

        // Enqueue the new position
        Actor newPos = {newX, newY, 0, 0};
        Enqueue(queue, newPos);
      }
    }
  }
  /* NOT REACHED */ // C really should not complain about this.
  return INT_MAX; // bleh
}

// Code to assign each ghost to a junction that the player must pass through, in order
// to fence him in.

/*
      Given 4 starting points and 4 destination points, and a 4x4 matrix of distance
   from each starting point to each destination point, connect each starting point
   to a unique destination point while minimizing the total distance travelled:

   This is a minimum cost assignment problem.  A suitable algorithm to solve this
   could be the Hungarian Algorithm (also known as the Kuhn-Munkres Algorithm):

   (If this approach doesn't play well, there's an alternative algorithm which
    minimizes the longest distance travelled that may work better)

   The Hungarian Algorithm is an efficient method for solving the assignment problem
   in polynomial time, particularly when the number of starting and destination points
   is the same, as in our 4x4 matrix. It operates on a cost matrix (in our case, the
   distance matrix), and it guarantees finding the minimum cost assignment.

   Create a cost matrix.

   Subtract the smallest value in each row from all the elements of that row; then
   subtract the smallest value in each column from all the elements of that column.

   Try to cover all zeros in the matrix using the minimum number of horizontal and
   vertical lines. If the number of lines equals the number of rows (or columns),
   an optimal assignment can be made using the zeros.

   If the number of lines is less than the number of rows (or columns), adjust the
   matrix by subtracting the smallest uncovered element from all uncovered elements
   and adding it to elements that are covered by two lines.

   Repeat the covering and adjustment process until the number of lines equals the
   number of rows (or columns), at which point an optimal assignment can be made.

   The Hungarian Algorithm has a time complexity of O(n^3), which is efficient enough
   for small problems like this.
*/

#define N 4 // Matrix size (4x4 - 4 ghosts, 4 junctions)

// Utility functions
void printMatrix(int matrix[N][N]) {
  int i, j;
  for (i = 0; i < N; i++) {
    for (j = 0; j < N; j++) printf("%d ", matrix[i][j]);
    printf("\n");
  }
}

// Step 1: Row reduction - subtract the minimum value in each row
void rowReduction(int costMatrix[N][N]) {
  int i, j, minVal;
  for (i = 0; i < N; i++) {
    minVal = INT_MAX;
    for (j = 0; j < N; j++) if (costMatrix[i][j] < minVal) minVal = costMatrix[i][j];
    for (j = 0; j < N; j++) costMatrix[i][j] -= minVal;
  }
}

// Step 2: Column reduction - subtract the minimum value in each column
void columnReduction(int costMatrix[N][N]) {
  int i, j, minVal;
  for (j = 0; j < N; j++) {
    minVal = INT_MAX;
    for (i = 0; i < N; i++) if (costMatrix[i][j] < minVal) minVal = costMatrix[i][j];
    for (i = 0; i < N; i++) costMatrix[i][j] -= minVal;
  }
}

// Step 3: Cover all zeros in the matrix using a minimum number of lines
// Use an auxiliary matrix to mark rows and columns
bool rowCovered[N];
bool colCovered[N];

void clearCovers(void) {
  int i;
  for (i = 0; i < N; i++) rowCovered[i] = colCovered[i] = false;
}

int countCoveredZeros(int costMatrix[N][N]) {
  int i, j, coverCount = 0;
  clearCovers();
  for (i = 0; i < N; i++) {
    for (j = 0; j < N; j++) {
      if (costMatrix[i][j] == 0 && !rowCovered[i] && !colCovered[j]) {
        rowCovered[i] = colCovered[j] = true;
        coverCount++;
      }
    }
  }
  return coverCount;
}

// Step 4: Adjust the matrix by subtracting and adding the smallest uncovered value
void adjustMatrix(int costMatrix[N][N]) {
  int i, j, minUncoveredVal = INT_MAX;

  // Find the smallest uncovered value
  for (i = 0; i < N; i++)
    for (j = 0; j < N; j++)
      if (!rowCovered[i] && !colCovered[j] && costMatrix[i][j] < minUncoveredVal) minUncoveredVal = costMatrix[i][j];

  // Adjust the matrix by subtracting from uncovered and adding to double-covered positions
  for (i = 0; i < N; i++)
    for (j = 0; j < N; j++)
      costMatrix[i][j] += ((!rowCovered[i] && !colCovered[j]) ? -1 : 1) * minUncoveredVal;
}

// Step 5: Find an optimal assignment by using the marked zeros
void findAssignments(int costMatrix[N][N], int assignments[N]) {
  int i, j;
  bool assigned[N] = { false, false, false, false };

  for (i = 0; i < N; i++) {
    for (j = 0; j < N; j++) {
      if (costMatrix[i][j] == 0 && !assigned[j]) {
        assignments[i] = j;
        assigned[j] = true;
        break;
      }
    }
  }
}

// Hungarian Algorithm to find the minimum cost assignment
void hungarianAlgorithm(int costMatrix[N][N], int assignments[N]) {
  int coverCount = 0;

  rowReduction(costMatrix);
  columnReduction(costMatrix);
  while (coverCount < N) if ((coverCount = countCoveredZeros(costMatrix)) < N) adjustMatrix(costMatrix);
  findAssignments(costMatrix, assignments);
}

// Example distance matrix (cost matrix)
static int distanceMatrixInit[N][N] = {
  {9, 2, 7, 8},
  {6, 4, 3, 7},
  {5, 8, 1, 8},
  {7, 6, 9, 4}
};

static int distanceMatrix[N][N] = {
  {9, 2, 7, 8},
  {6, 4, 3, 7},
  {5, 8, 1, 8},
  {7, 6, 9, 4}
};

int assignment_test(void) {
  int i, assignments[N];  // To store the optimal assignments

  // Apply Hungarian Algorithm
  hungarianAlgorithm(distanceMatrix, assignments);

  // Output the assignments and the minimized cost
  printf("Assignment of ghosts to target junctions:\n");
  for (i = 0; i < N; i++) printf("Ghost %d -> Junction %d  distance = %d\n", i + 1, assignments[i] + 1, distanceMatrixInit[i][assignments[i]]);

  return 0;
}

/*
      Why we need a complex ghost algorithm: let's assume we're using a 'simple'
   algorithm: find the ghost whose shortest path to any target is the longest.
   Assign that one, then repeat for the next 3.  However the problem occurs when
   you did that assignment - it removed that target from the available pool, so
   the shortest paths for the remaining ghosts just got longer.  What we end up
   with is a good option for the one farthest ghost, but poorer options for the
   closest ghosts.  The Hungarian algorithm (and the alternative one not yet
   described here which uses the Hungarian algorithm as one internal step in
   a binary search) avoids making pessimistic assignments for the nearest ghosts.
*/

int main(void) {
  int SimplePlayerDist;
  // Actor Target[4];

  Player.X = 13;  // Player's X coordinate
  Player.Y = 23;  // Player's Y coordinate
  Player.dx = -1;
  Player.dy = 0;
  
  Ghost[0].X = 6;
  Ghost[0].Y = 2;
  Ghost[0].dx = 0;
  Ghost[0].dy = 1;

  SimplePlayerDist = DistanceToPlayer(0);

  // When there are no junctions left between a ghost and the player, the ghost will
  // switch to direct pursuit mode.  This has not yet been written.

  // To implement the identification of targeted junctions (outlined below), we need
  // to write some support functions that will determine the ends of corridors when
  // travelling in the current direction, taking into account forced L-shaped corner
  // turns.  (No algorithmic comlexity here, just tricky and careful code writing).

  // For example, fork = GetFork(Player.X, Player.Y, Player.dx, Player.dy)
  
  if ((Path(&Player, LEFT)?1:0) + (Path(&Player, RIGHT)?1:0) + (Path(&Player, UP)?1:0) + (Path(&Player, DOWN)?1:0) == 2) {
    // Player is in a corridor
    // Find 3 places to target in the directon the player is moving
    // First, identify the next fork in the road, but don't target it itself:
//  fork1 = EndOfCorridor(Player.X, Player.Y, Player.dx, Player.dy);
      // fork2 = end of corridor for first exit at fork1 excluding where we just came from 
      // fork3 = end of corridor for second exit at fork1 excluding path immediately above, and where we just came from
    // choose one of fork2 or fork3, and set it as a target
    //Target[0] = fork2;
    // set fork4 and fork5 to the end of the corridors on the other fork (say fork3) and set them as targets
    //Target[1] = fork4;
    //Target[2] = fork5;
//  Target[3] = EndOfCorridor(Player.X, Player.Y, -Player.dx, -Player.dy);
    // and one place to target if he reverses direction
//} else if (... == 4) {
    // at a crossroads.  Easy solution - target the ends of all 4 corridors with all 4 ghosts.
  } else {
    // Player is at a junction
    // Target[0] = end of corridor for first exit here excluding where we just came from 
    // Target[1] = end of corridor for second exit here excluding path immediately above, and where we just came from
    // for Target[2], look at Target[0] and Target[1], and choose the target which is closer.
    // Then set target 0 or 1 to one of the exists at that point and target 2 to the other
    // Target 3 remains behind us, in case the player reverses direction
//  Target[3] = EndOfCorridor(Player.X, Player.Y, -Player.dx, -Player.dy);
  }
  // Having identified 4 targets, assign each one to a ghost ... intelligently, using the 4x4 matrix of distance from
  // target to ghost.
  
  fprintf(stderr, "Shortest Distance from ghost 0 to player is %d\n", SimplePlayerDist);

  assignment_test();
  
  exit(EXIT_SUCCESS);
}