// LREF:
//
// c99 reference implementation of line rider,
// with the goal of exactly matching beta 6.2.
//
// ~moss7

// code todos
// TODO 6.1 and 6.0 grid registration
// TODO camera implementation
// TODO cleanup / deallocation logic
// TODO consider which functions actually need to be api vs internal


// documentation todos
// TODO equivalent flash decomp code in comments for reference
// TODO explain all of the physics functions in comments
// TODO explain parts that may differ in a less contrived engine
//      (NaN checks, optimizations not made here for readability, etc)

// # external includes

#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>

// these only need to be included by the implementation

#ifdef LREF_H_IMPLEMENTATION

#include <math.h>
#include <stdlib.h>
#include <string.h>

#endif

// # rider constants

const double endurance = 0.057;

const double gravity = 0.175;

#define POINT_COUNT 10
#define BONE_COUNT 22
#define ITERATION_COUNT 6

// # rider types

struct point {
  double x;
  double y;
  double previous_x;
  double previous_y;
  double momentum_x;
  double momentum_y;
  double friction;
};

struct bone {
  bool breakable;
  bool repel;
  uint8_t a;
  uint8_t b;
  double restLength;
};

struct rider {
  struct point points[POINT_COUNT];
  struct bone bones[BONE_COUNT];
  bool dead;
  bool sledbroken;
};

// # line constants

const double ZONE = 10;
const double ACC = 0.1;

// # line types

struct line_flags {
  bool red;
  bool invert;
  bool left_extended;
  bool right_extended;
};

struct line_values {
  double dx;
  double dy;
  double invSqrDst;
  double nx;
  double ny;
  double lim1;
  double lim2;
  double accx;
  double accy;
};

struct line {
  int32_t id;
  double x1;
  double y1;
  double x2;
  double y2;
  struct line_flags flags;
  struct line_values values;
};

// # grid constants

const uint8_t GRID_62 = 0;
const uint8_t GRID_61 = 1;
const uint8_t GRID_60 = 2;

const double GRIDSIZE = 14;

#define GRID_ARRAY_BASE_SIZE 65536

// # grid types

struct grid_coordinate {
  int64_t x;
  int64_t y;
};

struct grid_bucket {
  struct grid_coordinate key;
  struct grid_cell_entry* entry;
  struct grid_bucket* next;
};

struct grid_cell_entry {
  struct line* line;
  struct grid_cell_entry* next;
};

struct grid {
  struct grid_bucket** buckets;
  size_t capacity;
  size_t cells_total;
};

// integer grid coordinate plus the fractional remainder from truncation
struct grid_pos {
  struct grid_coordinate coord;
  double rx;
  double ry;
};

// # simulation types

struct simulation {
  double start_x;
  double start_y;
  struct grid* grid;
  struct line* lines;
  size_t line_count;
  struct rider bosh;
};

// # line function declarations

void line_calculate_values(struct line* line);

void line_collide(const struct line line, struct point* point);

// # grid function declarations

struct grid* grid_new(void);

void grid_free(struct grid* grid);

void grid_register_62(struct grid* grid, struct line* line);

void grid_register_61(struct grid* grid, struct line* line);

void grid_register_60(struct grid* grid, struct line* line);

struct grid_coordinate grid_coordinate_at(double x, double y);

struct grid_pos grid_pos_at(double x, double y);

// # rider function declarations

struct rider rider_new(double x, double y);

// # simulation function declarations

struct simulation simulation_new(void);

void simulation_prepare_line_capacity(struct simulation* simulation, size_t capacity);

void simulation_add_line(struct simulation* simulation, struct line line);

#ifdef LREF_H_IMPLEMENTATION

// # line function implementations

void line_calculate_values(struct line* line) {
  line->values.dx = line->x2 - line->x1;
  line->values.dy = line->y2 - line->y1;
  double sqrDst = (line->values.dx * line->values.dx + line->values.dy * line->values.dy);
  line->values.invSqrDst = 1.0 / sqrDst;
  double dst = sqrt(sqrDst);
  double invDst = 1.0 / dst;
  line->values.nx = line->values.dy * invDst * (line->flags.invert ? 1 : -1);
  line->values.ny = line->values.dx * invDst * (line->flags.invert ? -1 : 1);

  double lim = fmin(0.25, ZONE / dst);
  line->values.lim1 = line->flags.left_extended ? -lim : 0;
  line->values.lim2 = line->flags.right_extended ? 1 + lim : 1;
  if (!line->flags.red) return;

  line->values.accx = line->values.ny * ACC * (line->flags.invert ? 1 : -1);
  line->values.accy = line->values.nx * ACC * (line->flags.invert ? -1 : 1);
}

void line_collide(const struct line line, struct point* point) {
  if (point->momentum_x * line.values.nx + point->momentum_y * line.values.ny <= 0.0) return;

  double distx = point->x - line.x1;
  double disty = point->y - line.y1;
  double distance_perpendicular = line.values.nx * distx + line.values.ny * disty;
  double distance_along = (distx * line.values.dx + disty * line.values.dy) * line.values.invSqrDst;

  if (distance_perpendicular > 0 && distance_perpendicular < ZONE &&
      distance_along >= line.values.lim1 && distance_along <= line.values.lim2) {
    point->x -= distance_perpendicular * line.values.nx;
    point->y -= distance_perpendicular * line.values.ny;

    point->previous_x += line.values.ny * point->friction * distance_perpendicular
      * (point->previous_x < point->x ? 1 : -1) + (line.flags.red ? line.values.accx : 0);
    point->previous_y -= line.values.nx * point->friction * distance_perpendicular
      * (point->previous_y < point->y ? -1 : 1) + (line.flags.red ? line.values.accy : 0);
  }
}

// # grid hashtable function implementations

struct grid* grid_new(void) {
  struct grid* grid = calloc(1, sizeof(struct grid));
  grid->buckets = calloc(GRID_ARRAY_BASE_SIZE, sizeof(struct grid_bucket*));
  grid->capacity = GRID_ARRAY_BASE_SIZE;
  grid->cells_total = 0;
  return grid;
}

uint64_t grid_hash(const struct grid_coordinate coords) {
  uint64_t x = coords.x;
  uint64_t y = coords.y;
  x ^= y + 0x9e3779b9 + (x << 6) + (x >> 2);
  return x;
}

size_t grid_index(const struct grid grid, const struct grid_coordinate coords) {
  return (size_t)grid_hash(coords) & (grid.capacity - 1);
}

bool grid_keys_eql(const struct grid_coordinate a, const struct grid_coordinate b) {
  return a.x == b.x && a.y == b.y;
}

bool grid_contains_cell(const struct grid grid, const struct grid_coordinate coords) {
  size_t index = grid_index(grid, coords);
  struct grid_bucket* current = grid.buckets[index];
  while (current != NULL) {
    if (grid_keys_eql(current->key, coords)) return true;
    current = current->next;
  }
  return false;
}

void grid_put_ptr(struct grid* grid, struct grid_bucket* cell) {
  size_t index = grid_index(*grid, cell->key);
  struct grid_bucket* current = grid->buckets[index];
  if (current == NULL) {
    grid->buckets[index] = cell;
    cell->next = NULL;
    return;
  }
  while (current->next != NULL) {
    // if we are overwriting the old pointer
    // (i dont think this ever actually happens
    // in lref but for correctness, whatever)
    if (grid_keys_eql(current->next->key, cell->key)) {
      cell->next = current->next->next;
      current->next = cell;
      return;
    }
    current = current->next;
  }
  current->next = cell;
  cell->next = NULL;
  grid->cells_total += 1;
}

bool grid_should_double_capacity(const struct grid grid) {
  return (grid.cells_total * 4 > grid.capacity * 3);
}

void grid_double_capacity(struct grid* grid) {
  size_t old_capacity = grid->capacity;
  struct grid_bucket** old_buckets = grid->buckets;
  grid->capacity *= 2;
  grid->buckets = calloc(grid->capacity, sizeof(struct grid_bucket*));
  for (unsigned int i = 0; i < old_capacity; i++) {
    struct grid_bucket* next_bucket = old_buckets[i];
    while (next_bucket != NULL) {
      struct grid_bucket* current_bucket = next_bucket;
      next_bucket = current_bucket->next;
      grid_put_ptr(grid, current_bucket);
    }
  }
  free(old_buckets);
}

struct grid_bucket* grid_cell_new(struct grid_coordinate coords) {
  struct grid_bucket* cell = calloc(1, sizeof(struct grid_bucket));
  cell->key = coords;
  cell->next = NULL;
  cell->entry = NULL;
  return cell;
}

// get the cell at a given coordinate, creating it if it doesnt exist
struct grid_bucket* grid_get_cell(struct grid* grid, const struct grid_coordinate coords) {
  size_t index = grid_index(*grid, coords);
  struct grid_bucket* current = grid->buckets[index];
  if (current == NULL) {
    grid->buckets[index] = grid_cell_new(coords);
    return grid->buckets[index];
  }

  if (grid_keys_eql(current->key, coords)) return current;

  while (current->next != NULL) {
    if (grid_keys_eql(current->next->key, coords)) return current->next;
    current = current->next;
  }

  current->next = grid_cell_new(coords);
  return current->next;
}

struct grid_coordinate grid_coordinate_at(double x, double y) {
  struct grid_coordinate coord = { .x = (int64_t)floor(x / GRIDSIZE), .y = (int64_t)floor(y / GRIDSIZE) };
  return coord;
}

struct grid_pos grid_pos_at(double x, double y) {
  struct grid_coordinate coord = grid_coordinate_at(x, y);
  struct grid_pos pos = {
    .coord = coord,
    .rx = x - GRIDSIZE * coord.x,
    .ry = y - GRIDSIZE * coord.y,
  };
  return pos;
}

// # grid line registry function implementations

void grid_cell_add_line(struct grid_bucket* cell, struct line* line) {
  if (cell->entry == NULL) {
    struct grid_cell_entry* entry = calloc(1, sizeof(struct grid_cell_entry));
    entry->line = line;
    cell->entry = entry;
    return;
  }

  struct grid_cell_entry* current = cell->entry;

  if (current->line->id < line->id) {
    struct grid_cell_entry* entry = calloc(1, sizeof(struct grid_cell_entry));
    entry->line = line;
    entry->next = current;
    cell->entry = entry;
    return;
  }

  while (current->next != NULL) {
    if (current->next->line->id < line->id) {
      struct grid_cell_entry* entry = calloc(1, sizeof(struct grid_cell_entry));
      entry->line = line;
      entry->next = current->next;
      current->next = entry;
      return;
    }
    current = current->next;
  }
  struct grid_cell_entry* entry = calloc(1, sizeof(struct grid_cell_entry));
  entry->line = line;
  current->next = entry;
}

void grid_register_62(struct grid* grid, struct line* line) {
  struct grid_pos start = grid_pos_at(line->x1, line->y1);
  struct grid_pos end = grid_pos_at(line->x2, line->y2);

  double right = line->values.dx > 0 ? end.coord.x : start.coord.x;
  double left = line->values.dx > 0 ? start.coord.x : end.coord.x;
  double bottom = line->values.dy > 0 ? end.coord.y : start.coord.y;
  double top = line->values.dy > 0 ? start.coord.y : end.coord.y;

  grid_cell_add_line(grid_get_cell(grid, start.coord), line);

  if ((line->values.dx == 0 && line->values.dy == 0) ||
    (start.coord.x == end.coord.x && start.coord.y == end.coord.y)) return;

  double x = line->x1;
  double y = line->y1;
  double invDx = 1.0 / line->values.dx;
  double invDy = 1.0 / line->values.dy;

  double difX;
  double difY;

  while (true) {
    if (start.coord.x < 0) {
      difX = line->values.dx > 0 ? (GRIDSIZE + start.rx) : (-GRIDSIZE - start.rx);
    } else {
      difX = line->values.dx > 0 ? (GRIDSIZE - start.rx) : (-(start.rx + 1));
    }
    if (start.coord.y < 0) {
      difY = line->values.dy > 0 ? (GRIDSIZE + start.ry) : (-GRIDSIZE - start.ry);
    } else {
      difY = line->values.dy > 0 ? (GRIDSIZE - start.ry) : (-(start.ry + 1));
    }
    if (line->values.dx == 0) {
      y += difY;
    } else if (line->values.dy == 0) {
      x += difX;
    } else {
      double step = y + line->values.dy * difX * invDx;
      if (fabs(step - y) < fabs(difY)) {
        x += difX;
        y = step;
      } else if (fabs(step - y) == fabs(difY)) {
        x += difX;
        y += difY;
      } else {
        x += line->values.dx * difY * invDy;
        y += difY;
      }
    }
    start = grid_pos_at(x, y);
    if (start.coord.x >= left && start.coord.x <= right && start.coord.y >= top && start.coord.y <= bottom) {
      grid_cell_add_line(grid_get_cell(grid, start.coord), line);
      continue;
    }
    return;
  }
}

// # point function implementations

void point_apply_momentum(struct point* point) {
  point->momentum_x = point->x - point->previous_x;
  point->momentum_y = point->y - point->previous_y + gravity;
  point->previous_x = point->x;
  point->previous_y = point->y;
  point->x += point->momentum_x;
  point->y += point->momentum_y;
}

// # rider function implementations

void rider_init_point(struct rider* bosh, uint8_t index, double x, double y, double friction) {
  bosh->points[index].x = x;
  bosh->points[index].y = y;
  bosh->points[index].previous_x = x - 0.4;
  bosh->points[index].previous_y = y;
  bosh->points[index].friction = friction;
}

void rider_init_bone(struct rider* bosh, uint8_t index, uint8_t a, uint8_t b, bool breakable, bool repel) {
  bosh->bones[index].a = a;
  bosh->bones[index].b = b;
  bosh->bones[index].breakable = breakable;
  bosh->bones[index].repel = repel;
  double x = bosh->points[a].x - bosh->points[b].x;
  double y = bosh->points[a].y - bosh->points[b].y;
  bosh->bones[index].restLength = sqrt(x * x + y * y) * (repel ? 0.5 : 1.0);
}

void rider_apply_start_offset(struct rider* bosh, double x, double y) {
  for (int i = 0; i < POINT_COUNT; i++) {
    struct point* point = &bosh->points[i];

    point->x += x;
    point->y += y;
    point->previous_x = point->x - 0.4;
    point->previous_y = point->y;
  }
}

struct rider rider_new(double x, double y) {
  struct rider bosh;
  memset(&bosh, 0, sizeof(bosh));

  rider_init_point(&bosh, 0, 0.0, 0.0, 0.8);
  rider_init_point(&bosh, 1, 0.0, 5.0, 0.0);
  rider_init_point(&bosh, 2, 15.0, 5.0, 0.0);
  rider_init_point(&bosh, 3, 17.5, 0.0, 0.0);
  rider_init_point(&bosh, 4, 5.0, 0.0, 0.8);
  rider_init_point(&bosh, 5, 5.0, -5.5, 0.8);
  rider_init_point(&bosh, 6, 11.5, -5.0, 0.1);
  rider_init_point(&bosh, 7, 11.5, -5.0, 0.1);
  rider_init_point(&bosh, 8, 10.0, 5.0, 0.0);
  rider_init_point(&bosh, 9, 10.0, 5.0, 0.0);

  rider_init_bone(&bosh, 0, 0, 1, false, false);
  rider_init_bone(&bosh, 1, 1, 2, false, false);
  rider_init_bone(&bosh, 2, 2, 3, false, false);
  rider_init_bone(&bosh, 3, 3, 0, false, false);
  rider_init_bone(&bosh, 4, 0, 2, false, false);
  rider_init_bone(&bosh, 5, 3, 1, false, false);
  rider_init_bone(&bosh, 6, 0, 4, true, false);
  rider_init_bone(&bosh, 7, 1, 4, true, false);
  rider_init_bone(&bosh, 8, 2, 4, true, false);
  rider_init_bone(&bosh, 9, 5, 4, false, false);
  rider_init_bone(&bosh, 10, 5, 6, false, false);
  rider_init_bone(&bosh, 11, 5, 7, false, false);
  rider_init_bone(&bosh, 12, 4, 8, false, false);
  rider_init_bone(&bosh, 13, 4, 9, false, false);
  rider_init_bone(&bosh, 14, 5, 7, false, false);
  rider_init_bone(&bosh, 15, 5, 0, true, false);
  rider_init_bone(&bosh, 16, 3, 6, true, false);
  rider_init_bone(&bosh, 17, 3, 7, true, false);
  rider_init_bone(&bosh, 18, 8, 2, true, false);
  rider_init_bone(&bosh, 19, 9, 2, true, false);
  rider_init_bone(&bosh, 20, 5, 8, false, true);
  rider_init_bone(&bosh, 21, 5, 9, false, true);

  rider_apply_start_offset(&bosh, x, y);

  return bosh;
}

void rider_bone_satisfy_single(struct rider* bosh, uint8_t bone_idx) {
  struct bone bone = bosh->bones[bone_idx];
  struct point* a = &bosh->points[bone.a];
  struct point* b = &bosh->points[bone.b];
  double x = a->x - b->x;
  double y = a->y - b->y;
  double dist = sqrt(x * x + y * y);
  if (bone.repel && dist >= bone.restLength) return;
  double effect = (dist - bone.restLength) / dist * 0.5;
  if (bone.breakable) {
    if (effect > endurance * bone.restLength * 0.5) {
      bosh->dead = true;
      return;
    }
    if (bosh->dead) return;
  }
  double xEffect = x * effect;
  double yEffect = y * effect;
  a->x -= xEffect;
  a->y -= yEffect;
  b->x += xEffect;
  b->y += yEffect;
}

void rider_bone_satisfy_all(struct rider* bosh) {
  for (uint8_t i = 0; i < BONE_COUNT; i++) {
    rider_bone_satisfy_single(bosh, i);
  }
}

void rider_apply_momentum(struct rider* bosh) {
  for (uint8_t i = 0; i < POINT_COUNT; i++) {
    point_apply_momentum(&bosh->points[i]);
  }
}

void rider_fakie_check(struct rider* bosh) {
  double along_sled_x = bosh->points[3].x - bosh->points[0].x;
  double along_sled_y = bosh->points[3].y - bosh->points[0].y;
  if (along_sled_x * (bosh->points[1].y - bosh->points[0].y) - along_sled_y * (bosh->points[1].x - bosh->points[0].x) < 0) {
    bosh->dead = true;
    bosh->sledbroken = true;
  }
  if (along_sled_x * (bosh->points[5].y - bosh->points[4].y) - along_sled_y * (bosh->points[5].x - bosh->points[4].x) > 0) {
    bosh->dead = true;
  }
}

// # simulation function implementations

struct simulation simulation_new(void) {
  struct simulation sim = {
    .grid = grid_new(),
    .bosh = rider_new(0, 0),
  };

  return sim;
}

void simulation_collide_single_point_and_grid_cell(struct point* point, struct grid_bucket* cell) {
  struct grid_cell_entry* current = cell->entry;
  while (current != NULL) {
    line_collide(*current->line, point);
    current = current->next;
  }
}

void simulation_collide_all(struct simulation* simulation) {
  for (uint8_t i = 0; i < POINT_COUNT; i++) {
    struct point* point = &simulation->bosh.points[i];
    struct grid_coordinate base_coords = grid_pos_at(point->x, point->y).coord;
    for (int64_t x = -1; x < 2; x++) {
      for (int64_t y = -1; y < 2; y++) {
        struct grid_coordinate cell_coords = base_coords;
        cell_coords.x += x;
        cell_coords.y += y;
        struct grid_bucket* cell = grid_get_cell(simulation->grid, cell_coords);
        simulation_collide_single_point_and_grid_cell(point, cell);
      }
    }
  }
}

void simulation_step_frame(struct simulation* simulation) {
  rider_apply_momentum(&simulation->bosh);
  for (uint8_t i = 0; i < ITERATION_COUNT; i++) {
    rider_bone_satisfy_all(&simulation->bosh);
    simulation_collide_all(simulation);
  }
  rider_fakie_check(&simulation->bosh);
}

void simulation_prepare_line_capacity(struct simulation* simulation, size_t capacity) {
  simulation->lines = realloc(simulation->lines, sizeof(struct line) * capacity);
}

void simulation_add_line(struct simulation* simulation, struct line line) {
  line_calculate_values(&line);
  simulation->lines[simulation->line_count] = line;
  // TODO 61 & 60
  grid_register_62(simulation->grid, &simulation->lines[simulation->line_count]);
  simulation->line_count++;
}

#endif