#include "../kit/astar.h"

#define KIT_TEST_FILE astar
#include "../kit/test.h"

#include <stdio.h>

enum {
  WIDTH  = 8,
  HEIGHT = 5,
};

static astar_link_t neighbor(i8 *grid, i64 a, i64 n) {
  i64 x = a % WIDTH;
  i64 y = a / WIDTH;

  if (n == 0)
    --x;
  else if (n == 1)
    ++x;
  else if (n == 2)
    --y;
  else if (n == 3)
    ++y;
  else
    return (astar_link_t) {
      .stop = 1,
    };

  i64 m = (y * WIDTH) + x;

  if (x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT || m < 0 ||
      m >= WIDTH * HEIGHT || grid[m] == 1)
    return (astar_link_t) {
      .stop = 0,
      .skip = 1,
    };

  return (astar_link_t) {
    .stop     = 0,
    .skip     = 0,
    .neighbor = m,
    .distance = 1,
  };
}

static i64 heuristic(i64 a, i64 b) {
  i64 x0 = a % WIDTH;
  i64 y0 = a / WIDTH;

  i64 x1 = b % WIDTH;
  i64 y1 = b / WIDTH;

  i64 dx = x0 < x1 ? x1 - x0 : x0 - x1;
  i64 dy = y0 < y1 ? y1 - y0 : y0 - y1;

  return dx + dy;
}

#define ASTAR_TEMPLATE
#define ASTAR_SUFFIX _test
#define ASTAR_NEIGHBOR(data, a, n) neighbor((i8 *) data, a, n)
#define ASTAR_HEURISTIC(data, a, b) heuristic(a, b)
#include "../kit/astar.h"

static i64 xy(i64 x, i64 y) {
  return y * WIDTH + x;
}

TEST("astar path exists") {
  astar_node_t buf[200];

  astar_set_t sets[2] = {
    { .capacity = 100, .size = 0, .values = buf },
    { .capacity = 100, .size = 0, .values = buf + 100 },
  };

  i8 grid[WIDTH * HEIGHT] = {
    0, 0, 0, 0, 0, 0, 0, 0, //
    1, 1, 0, 1, 1, 0, 1, 0, //
    1, 1, 0, 0, 1, 0, 1, 0, //
    0, 0, 0, 1, 1, 1, 1, 0, //
    1, 1, 0, 0, 0, 0, 0, 0, //
  };

  astar_state_t state;
  REQUIRE_EQ(
      astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
      KIT_OK);

  s32 status;

  for (;;) {
    status = astar_iteration_test(&state);
    if (status != ASTAR_PROGRESS)
      break;
  }

  REQUIRE_EQ(status, ASTAR_SUCCESS);

  i64 path_size = 10;
  i64 path[10];

  REQUIRE_EQ(astar_path(&state, &path_size, path), KIT_OK);
  REQUIRE_EQ(path_size, 8);

  if (path_size == 8) {
    REQUIRE_EQ(path[0], xy(0, 0));
    REQUIRE_EQ(path[1], xy(1, 0));
    REQUIRE_EQ(path[2], xy(2, 0));
    REQUIRE_EQ(path[3], xy(3, 0));
    REQUIRE_EQ(path[4], xy(4, 0));
    REQUIRE_EQ(path[5], xy(5, 0));
    REQUIRE_EQ(path[6], xy(5, 1));
    REQUIRE_EQ(path[7], xy(5, 2));
  }
}

TEST("astar no path") {
  astar_node_t buf[200];

  astar_set_t sets[2] = {
    { .capacity = 100, .size = 0, .values = buf },
    { .capacity = 100, .size = 0, .values = buf + 100 },
  };

  i8 grid[WIDTH * HEIGHT] = {
    0, 0, 0, 0, 1, 0, 0, 0, //
    1, 1, 0, 1, 1, 0, 1, 1, //
    1, 1, 0, 0, 1, 0, 1, 0, //
    0, 0, 0, 1, 1, 1, 1, 0, //
    1, 1, 0, 0, 0, 0, 0, 0, //
  };

  astar_state_t state;
  REQUIRE_EQ(
      astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
      KIT_OK);

  s32 status;

  for (;;) {
    status = astar_iteration_test(&state);
    if (status != ASTAR_PROGRESS)
      break;
  }

  REQUIRE_EQ(status, ASTAR_FAIL);
}

TEST("astar empty path") {
  astar_node_t buf[200];

  astar_set_t sets[2] = {
    { .capacity = 100, .size = 0, .values = buf },
    { .capacity = 100, .size = 0, .values = buf + 100 },
  };

  i8 grid[WIDTH * HEIGHT] = {
    0, 0, 0, 0, 1, 0, 0, 0, //
    1, 1, 0, 1, 1, 0, 1, 1, //
    1, 1, 0, 0, 1, 0, 1, 0, //
    0, 0, 0, 1, 1, 1, 1, 0, //
    1, 1, 0, 0, 0, 0, 0, 0, //
  };

  astar_state_t state;
  REQUIRE_EQ(
      astar_init(&state, sets[0], sets[1], grid, xy(4, 3), xy(4, 3)),
      KIT_OK);

  s32 status;

  for (;;) {
    status = astar_iteration_test(&state);
    if (status != ASTAR_PROGRESS)
      break;
  }

  REQUIRE_EQ(status, ASTAR_SUCCESS);

  i64 path_size = 10;
  i64 path[10];

  REQUIRE_EQ(astar_path(&state, &path_size, path), KIT_OK);
  REQUIRE_EQ(path_size, 1);

  if (path_size == 1)
    REQUIRE_EQ(path[0], xy(4, 3));
}

#undef KIT_TEST_FILE