#include "world.h"
#include "game.h"

size_t buildWorldBVHLeafs(BVHNode leafs[WORLD_ENTITY_MAX], const World* world)
{
  size_t leafsSize = 0;
  const Entity* entities = world->entities;
  bool grouped[WORLD_ENTITY_MAX];
  int ungroupedCount = WORLD_ENTITY_MAX;
  memset(grouped, 0, sizeof(grouped));

  while (ungroupedCount > 0)
  {
    BVHNode leaf;

    for (int leafIndex = 0; leafIndex < BVH_MAX; ++leafIndex)
    {
      int closest = -1;
      float closestDistance = world->size.x;

      // Find closest.
      for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
      {
        if (grouped[index])
        {
          continue;
        }
        
        // First entity.
        if (leafIndex == 0)
        {
          closest = index;
          break;
        }

        float distance = 0.0;
        BoundingBox overlapBox;
        overlapBox.min = entities[index].position;
        overlapBox.max = overlapBox.min;

        for (int innerIndex = 0; innerIndex < leafIndex; ++innerIndex)
        {
          distance += Vector3Distance(
            entities[leaf.entities[innerIndex]].position,
            entities[index].position);
          overlapBox.min = Vector3Min(
            overlapBox.min,
            entities[leaf.entities[innerIndex]].box.min);
          overlapBox.min = Vector3Min(
            overlapBox.min,
            entities[index].box.min);
          overlapBox.max = Vector3Max(
            overlapBox.max,
            entities[leaf.entities[innerIndex]].box.max);
          overlapBox.max = Vector3Max(
            overlapBox.max,
            entities[index].box.max);
        }

        distance /= (float)leafIndex;

        bool overlaps = false;

        // Too big (will count it as a overlap).
        if (Vector3Distance(overlapBox.min, overlapBox.max) >= BVH_BOX_MAX)
        {
          overlaps = true;
        }

        // Check if overlap is already happening.
        for (int nodeIndex = 0; nodeIndex < leafsSize && !overlaps;
             ++nodeIndex)
        {
          if (CheckCollisionBoxes(overlapBox, leafs[nodeIndex].box))
          {
            overlaps = true;
            break;
          }
        }

        // Update distance.
        if (!overlaps && distance < closestDistance)
        {
          closestDistance = distance;
          closest = index;
        }
      }

      if (closest == -1)
      {
        leaf.entities[leafIndex] = -1;
      }
      else
      {
        leaf.entities[leafIndex] = closest;
        grouped[closest] = true;
        --ungroupedCount;
      }
    }

    // Get bounding box.
    leaf.box = world->entities[leaf.entities[0]].box;

    for (int index = 1; index < BVH_MAX; ++index)
    {
      if (leaf.entities[index] == -1)
      {
        continue;
      }

      leaf.box.min = Vector3Min(
        leaf.box.min,
        world->entities[leaf.entities[index]].box.min);
      leaf.box.max = Vector3Max(
        leaf.box.max,
        world->entities[leaf.entities[index]].box.max);
    }

    leaf.position = Vector3Scale(Vector3Add(leaf.box.min, leaf.box.max), 0.5);

    memset(leaf.branches, 0, BVH_MAX_BRANCH_COUNT * sizeof(BVHNode*));
    leafs[leafsSize] = leaf;
    ++leafsSize;
  }

#ifdef FT_DEBUG_MODE
  // Test if everything is grouped.
  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
  {
    if (!grouped[index])
    {
      printf("Ungrouped: %d\n", index);
    }
  }

  // Test for leaf collision.
  for (int outer = 0; outer < leafsSize; ++outer)
  {
    for (int inner = 0; inner < leafsSize; ++inner)
    {
      if (outer != inner &&
          CheckCollisionBoxes(leafs[outer].box, leafs[inner].box))
      {
        printf("Leaf collision: %d and %d\n", outer, inner);
      }
    }
  }

  printf("leaf count: %ld\n", leafsSize);
#endif

  return leafsSize;
}

size_t buildWorldBVHSubtree(BVHNode* subtree,
                            const BVHNode* nodes, size_t nodesSize,
                            const World* world)
{
  size_t subtreeSize = 0;
  bool grouped[nodesSize];
  int ungroupedCount = nodesSize;
  memset(grouped, 0, sizeof(grouped));

  while (ungroupedCount > 0)
  {
    BVHNode node;
    memset(&node, 0, sizeof(BVHNode));

    for (int branchIndex = 0; branchIndex < BVH_MAX_BRANCH_COUNT;
         ++branchIndex)
    {
      int closest = -1;
      float closestDistance = Vector3LengthSqr(world->size);

      // Find closest.
      for (int index = 0; index < nodesSize; ++index)
      {
        if (grouped[index])
        {
          continue;
        }
        
        // First branch.
        if (branchIndex == 0)
        {
          closest = index;
          break;
        }

        float distance = 0.0;
        BoundingBox overlapBox = nodes[index].box;

        for (int innerIndex = 0; innerIndex < node.branchCount; ++innerIndex)
        {
          distance += Vector3Distance(node.branches[innerIndex]->position,
                                      nodes[index].position);
          overlapBox.min = Vector3Min(overlapBox.min,
                                      node.branches[innerIndex]->box.min);
          overlapBox.min = Vector3Min(overlapBox.min, nodes[index].box.min);
          overlapBox.max = Vector3Max(overlapBox.max,
                                      node.branches[innerIndex]->box.max);
          overlapBox.max = Vector3Max(overlapBox.max, nodes[index].box.max);
        }

        distance /= (float)node.branchCount;

        bool overlaps = false;

        // Check for overlap.
        for (int subtreeIndex = 0; subtreeIndex < subtreeSize; ++subtreeIndex)
        {
          if (CheckCollisionBoxes(overlapBox, subtree[subtreeIndex].box))
          {
            overlaps = true;
            break;
          }
        }

        BoundingBox currentBox = node.branches[0]->box;

        for (int innerIndex = 1; innerIndex < node.branchCount; ++innerIndex)
        {
          currentBox.min = Vector3Min(currentBox.min,
                                      node.branches[innerIndex]->box.min);
          currentBox.max = Vector3Max(currentBox.max,
                                      node.branches[innerIndex]->box.max);
        }
        
        // Update distance.
        if (CheckCollisionBoxes(currentBox, nodes[index].box))
        {
          closestDistance = Vector3Distance(
            nodes[index].position,
            Vector3Scale(Vector3Add(currentBox.min, currentBox.max), 0.5));
          closest = index;
        }
        else if (!overlaps && distance < closestDistance)
        {
          closestDistance = distance;
          closest = index;
        }
      }

      if (closest == -1)
      {
        continue;
      }

      // Add closest as a branch.
      node.branches[node.branchCount] = (BVHNode*)FT_MALLOC(sizeof(BVHNode));

      if (node.branches[node.branchCount] == NULL)
      {
        ALLOCATION_ERROR;
        break;
      }

      memcpy(node.branches[node.branchCount], &nodes[closest],
             sizeof(BVHNode));

      ++node.branchCount;
      grouped[closest] = true;
      --ungroupedCount;
    }

    // Get bounding box.
    node.box = node.branches[0]->box;

    for (int index = 1; index < node.branchCount; ++index)
    {
      node.box.min = Vector3Min(node.box.min, node.branches[index]->box.min);
      node.box.max = Vector3Max(node.box.max, node.branches[index]->box.max);
    }

    node.position = Vector3Scale(Vector3Add(node.box.min, node.box.max), 0.5);

    subtree[subtreeSize] = node;
    ++subtreeSize;
  }

  #ifdef FT_DEBUG_MODE
  // Test if everything is grouped.
  for (int index = 0; index < nodesSize; ++index)
  {
    if (!grouped[index])
    {
      printf("Ungrouped: %d\n", index);
    }
  }

  // Check for collisions
  int overlapCount = 0;
  
  for (int outer = 0; outer < subtreeSize - 1; ++outer)
  {
    for (int inner = outer + 1; inner < subtreeSize; ++inner)
    {
      if (CheckCollisionBoxes(subtree[outer].box, subtree[inner].box))
      {
        ++overlapCount;
      }
    }
  }

  printf("subtree size: %ld, overlap count: %d\n", subtreeSize, overlapCount);
#endif

  return subtreeSize;
}

void buildWorldBVH(World* world)
{
  Entity* entities = world->entities;

  // Get leafs.
  BVHNode tree[WORLD_ENTITY_MAX];
  size_t treeSize = buildWorldBVHLeafs(tree, world);

  while (treeSize > 1)
  {
    BVHNode subtree[treeSize];
    treeSize = buildWorldBVHSubtree(subtree, tree, treeSize, world);
    memcpy(tree, subtree, treeSize * sizeof(BVHNode));
  }

  world->bvh = tree[0];

  world->bvhDebugSelect = 0;
}

// Doesnt set y to a useful value.
Vector3 getRandomPositionFromCenter(const World* world, Seed* seed,
                                    float min, float max)
{
  Vector3 position = Vector3Scale(world->size, 0.5);
  Vector2 direction = randomDirection2(*seed, seed);
  float distance = Wrap(FT_RANDOM16(*seed), min, max);
  direction = Vector2Scale(direction, distance);
  position.x += direction.x;
  position.z += direction.y;

  return position;
}

bool checkForPlaceOverlap(const World* world, BoundingBox box)
{
  for (int index = 0; index < WORLD_PLACE_COUNT; ++index)
  {
    if (CheckCollisionBoxes(world->entities[world->places[index]].box, box))
    {
      return true;
    }
  }

  return false;
}

// Ensures entity will not overlap with a place.
Seed putEntityInRandomPlace(const World* world, Seed seed, Entity* entity)
{
  while (true)
  {
    Vector3 position;
    position.x = FT_RANDOM16(seed) % (int)world->size.x;
    position.y = 0.0;
    position.z = FT_RANDOM16(seed) % (int)world->size.z;

    setEntityPosition(entity, position);
    placeEntityOnGround(entity, world);

    if (!checkForPlaceOverlap(world, entity->box))
    {
      break;
    }
  }

  return seed;
}

void setWorldAreaToHeight(Color* heightmap, Rectangle area, int height)
{
  for (int y = area.y; y < area.y + area.height; ++y)
  {
    for (int x = area.x; x < area.x + area.width; ++x)
    {
      heightmap[y * WORLD_IMAGE_WIDTH + x] =
        (Color){height, height, height, 255};
    }
  }
}

void averageOutAreaWorld(const World* world, Color* heightmap, Rectangle area)
{
  int height = 0;
  int count = 0;

  // Get average height.
  for (int y = area.y; y < area.y + area.height; ++y)
  {
    for (int x = area.x; x < area.x + area.width; ++x)
    {
      height += heightmap[y * WORLD_IMAGE_WIDTH + x].r;
      ++count;
    }
  }

  height /= count;

  // Set to that height.
  setWorldAreaToHeight(heightmap, area, height);
}

Seed generatePond(World* world, Color* heightmap, WorldUID id, Seed seed)
{
  // Create pond entity.
  Vector3 position = getRandomPositionFromCenter(world, &seed,
                                                 PLACE_POND_MIN_DISTANCE,
                                                 PLACE_POND_MAX_DISTANCE);
  Entity pond = createEntity(POND, position);
  world->entities[id] = pond;

  // Get lowest height.
  int height = 255;

  Vector2 start = worldPositionToPixel(world, pond.box.min.x, pond.box.min.z);
  Vector2 end = worldPositionToPixel(world, pond.box.max.x, pond.box.max.z);
  
  for (int y = start.y; y < end.y; ++y)
  {
    for (int x = start.x; x < end.x; ++x)
    {
      int pixelHeight = heightmap[y * WORLD_IMAGE_WIDTH + x].r;

      if (pixelHeight < height)
      {
        height = pixelHeight;
      }
    }
  }

  height -= PLACE_POND_DEPTH * (world->size.y / 255.0 * 2.0);

  // Set new height.
  int area = PLACE_POND_OUTER_AREA;
  int edge = PLACE_POND_DEPTH * (world->size.y / 255.0 * 2.0) / 4.0 + height;
  
  for (int y = start.y - area; y < end.y + area; ++y)
  {
    for (int x = start.x - area; x < end.x + area; ++x)
    {
      if ((x == start.x || x == end.x || // Closest edge.
           y == start.y || y == end.y) &&
          (x >= start.x && x <= end.x &&
           y >= start.y && y <= end.y))
      {
        heightmap[y * WORLD_IMAGE_WIDTH + x] = (Color){edge, edge, edge, 255};
      }
      else if (x < start.x || x > end.x || // Smooth out rest of the edge.
               y < start.y || y > end.y)
      {
        int distance = Vector2Distance(
          (Vector2){x, y},
          Vector2Scale(Vector2Add(start, end), 0.5));
        int edgeHeight = distance + edge;

        // Average out the heights.
        if (distance > Vector2Distance(start, end) / 2.0 +
            PLACE_POND_WALKING_AREA)
        {
          edgeHeight += heightmap[y * WORLD_IMAGE_WIDTH + x].r;
          edgeHeight /= 2;
        }

        heightmap[y * WORLD_IMAGE_WIDTH + x] =
          (Color){edgeHeight, edgeHeight, edgeHeight, 255};
      }
      else
      {
        heightmap[y * WORLD_IMAGE_WIDTH + x] =
          (Color){height, height, height, 255};
      }
    }
  }
  
  return seed;
}

void generateWorldSamanthasPlace(World* world, Color* heightmap)
{
  Rectangle area = (Rectangle){
    .x = WORLD_IMAGE_WIDTH / 2.0 - PLACE_SAMANTHAS_SPOT_SIZE / 2.0,
    .y = WORLD_IMAGE_HEIGHT / 2.0 - PLACE_SAMANTHAS_SPOT_SIZE / 2.0,
    .width = PLACE_SAMANTHAS_SPOT_SIZE,
    .height = PLACE_SAMANTHAS_SPOT_SIZE
  };

  averageOutAreaWorld(world, heightmap, area);
}

Seed generateWorldPlants(World* world, Seed seed, int start, int end)
{
  for (int index = start; index < end; ++index)
  {
    FT_RANDOM16(seed);
    
    // Get id for plant.
    EntityId plants[] = {TREE, BUSH, FLOWER};
    size_t plantsSize = 3;
    EntityId id = plants[seed % plantsSize];
    
    // Create entity.
    Entity entity = createEntity(id, Vector3Zero());
    seed = putEntityInRandomPlace(world, seed, &entity);

    world->entities[index] = entity;
  }

  return seed;
}

// Use sin wave for computing pole positions.
// Does not set y position.
// Index as in World.utilityPoleTransforms index.
Vector3 utilityPoleIndexToPosition(int index, const World* world)
{
  float centerZ = (float)world->size.z / 2.0;
  float spacing = (float)world->size.x / WORLD_UTILITY_POLE_COUNT;

  Vector3 position;
  position.x = index * spacing;
  position.y = 0.0;
  position.z = sinf(position.x) * centerZ + centerZ;

  return position;
}

void generateWorldUtilityPoleLines(
  World* world, Quaternion rotations[WORLD_UTILITY_POLE_COUNT],
  int start, int end)
{
  Vector3 linePositions[UTILITY_LINE_COUNT] = {
    (Vector3){40.4336, 98.288, -4.18579},
    (Vector3){40.4336, 82.729, -4.18579},
    (Vector3){-40.4336, 98.288, -4.18579},
    (Vector3){-40.4336, 82.729, -4.18579}
  };

  for (int index = start; index < end - 1; ++index)
  {
    Vector3 position = world->entities[index].position;
    Vector3 nextPosition = world->entities[index + 1].position;
    UtilityPoleLines lines;

    for (int lineIndex = 0; lineIndex < UTILITY_LINE_COUNT; ++lineIndex)
    {
      // Line start.
      Vector3 linePosition = Vector3RotateByQuaternion(
        linePositions[lineIndex], rotations[index - start]);
      lines[lineIndex].start = Vector3Add(linePosition, position);

      // Line end.
      linePosition = Vector3RotateByQuaternion(
        linePositions[lineIndex], rotations[index - start + 1]);
      lines[lineIndex].end = Vector3Add(linePosition, nextPosition);
    }

    memcpy(&world->utilityPoleLines[index - start], &lines,
           sizeof(UtilityPoleLines));
  }
}

Seed generateWorldUtilityPoles(World* world, const Assets* assets, Seed seed,
                               int start, int end)
{
  assets->models[UTILITY_POLE_MODEL].materials[0].shader =
    assets->shaders[INSTANCING_SHADER];
  assets->models[UTILITY_POLE_MODEL].materials[0]
    .maps[MATERIAL_MAP_DIFFUSE].color = BROWN;

  Quaternion poleRotations[WORLD_UTILITY_POLE_COUNT];
  
  for (int index = start; index < end; ++index)
  {
    FT_RANDOM16(seed);

    Vector3 position = utilityPoleIndexToPosition(index - start, world);

    // Create pole.
    Entity entity = createEntity(UTILITY_POLE, position);
    placeEntityOnGround(&entity, world);
    world->entities[index] = entity;

    // Get direction to next pole.
    Vector3 nextPosition = utilityPoleIndexToPosition(index - start + 1,
                                                      world);
    Matrix lookat = MatrixLookAt(position, nextPosition,
                                 (Vector3){0.0, 1.0, 0.0});
    Quaternion rotation = QuaternionFromMatrix(MatrixInvert(lookat));
    poleRotations[index - start] = rotation;
    // Hack for it to not effect position. Luckily this isn't in a update loop.
    lookat = QuaternionToMatrix(rotation);
    
    // Add pole to instancing data.
    Matrix translation = MatrixTranslate(entity.position.x,
                                         entity.position.y,
                                         entity.position.z);
    Matrix matrix = MatrixMultiply(lookat, translation);
    world->utilityPoleTransforms[index - start] = matrix;   
  }

  generateWorldUtilityPoleLines(world, poleRotations, start, end);

  return seed;
}

void drawUtilityPoleLines(const World* world)
{
  for (int index = 0; index < WORLD_UTILITY_POLE_COUNT - 1; ++index)
  {
    for (int innerIndex = 0; innerIndex < UTILITY_LINE_COUNT; ++innerIndex)
    {
      DrawLine3D(world->utilityPoleLines[index][innerIndex].start,
                 world->utilityPoleLines[index][innerIndex].end,
                 BLACK);
    }
  }
}

Seed generateWorldItems(World* world, Seed seed, int start, int end)
{
  for (int index = start; index < end; ++index)
  {
    FT_RANDOM16(seed);

    EntityId items[] = {OLD_MINT, STICKY_NICKEL};
    size_t itemsSize = 2;
    EntityId id = items[seed % itemsSize];

    Entity entity = createEntity(id, Vector3Zero());
    seed = putEntityInRandomPlace(world, seed, &entity);
    
    world->entities[index] = entity;
  }

  return seed;
}

Texture generateGroundTexture()
{
  Image image = GenImageWhiteNoise(WORLD_IMAGE_WIDTH, WORLD_IMAGE_HEIGHT,
                                   WORLD_GROUND_IMAGE_NOISE);
  ImageBlurGaussian(&image, WORLD_GROUND_BLUR);
  ImageColorContrast(&image, WORLD_GROUND_CONTRAST);
  ImageColorTint(&image, WORLD_GROUND_COLOR);

  Texture texture = LoadTextureFromImage(image);
  UnloadImage(image);

  return texture;
}

Seed generateWorldCharacters(World* world, Seed seed, int index)
{
  // Create samantha.
  Entity samantha = createEntity(
    SAMANTHA,
    Vector3Add(Vector3Scale(world->size, 0.5), SAMANTHA_OFFSET));
  placeEntityOnGround(&samantha, world);
  world->entities[index] = samantha;

  return seed;
}

World createWorld(Seed seed, const Assets* assets)
{
  World world;
  world.size = WORLD_SIZE;

  // Heightmap image.
  int offsetX = FT_RANDOM16(seed);
  int offsetY = FT_RANDOM16(seed);
  Image image = GenImagePerlinNoise(WORLD_IMAGE_WIDTH, WORLD_IMAGE_HEIGHT,
                                    offsetX, offsetY, WORLD_IMAGE_SCALE);
  Color* colors = LoadImageColors(image);
  UnloadImage(image);

  // Places.
  WorldUID placeId = 0;

  // Pond.
  seed = generatePond(&world, colors, placeId, seed);
  world.places[0] = placeId;

  generateWorldSamanthasPlace(&world, colors);

  // Heightmap model.
  image = (Image){
    .data = colors,
    .width = WORLD_IMAGE_WIDTH,
    .height = WORLD_IMAGE_HEIGHT,
    .format = PIXELFORMAT_UNCOMPRESSED_R8G8B8A8,
    .mipmaps = 1
  };
  
  Mesh mesh = GenMeshHeightmap(image, world.size);
  world.heightmap = LoadModelFromMesh(mesh);
  world.heightmapTexture = LoadTextureFromImage(image);
  UnloadImage(image);

  // Put places on ground.
  for (int index = 0; index < WORLD_PLACE_COUNT; ++index)
  {
    placeEntityOnGround(&world.entities[index], &world);
  }

  // Model texture.
  world.heightmap.materials[0].maps[MATERIAL_MAP_DIFFUSE].texture =
    generateGroundTexture();

  // Plants
  int start = placeId + 1;
  int end = WORLD_PLANT_COUNT + start;
  seed = generateWorldPlants(&world, seed, start, end);

  // Utility poles.
  start = end;
  end = WORLD_UTILITY_POLE_COUNT + start;
  seed = generateWorldUtilityPoles(&world, assets, seed, start, end);

  // Characters.
  start = end;
  end = WORLD_CHARACTER_COUNT + start;
  seed = generateWorldCharacters(&world, seed, start);

  // Items.
  start = end;
  end = WORLD_ENTITY_MAX;
  seed = generateWorldItems(&world, seed, start, end);

  // Generate BVH.
  double currentTime = GetTime();
  buildWorldBVH(&world);

#ifdef FT_DEBUG_MODE
  printf("BVH build time: %lf\n", GetTime() - currentTime);
#endif

  return world;
}

void drawBVHDebug(BVHNode bvh, int level, int selected)
{
  Color colors[] = {RED, GREEN, BLUE, ORANGE, YELLOW, PINK};
  int colorSize = 6;

  if (level == selected)
  {
    DrawBoundingBox(bvh.box, colors[level % colorSize]);
    return;
  }

  for (int index = 0; index < bvh.branchCount; ++index)
  {
    drawBVHDebug(*bvh.branches[index], level + 1, selected);
  }
}

void updateWorld(World* world, Game* game)
{
  DrawModel(world->heightmap, Vector3Zero(), 1.0, WHITE);

  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
  {
    updateEntity(&world->entities[index], game);
  }

  DrawMeshInstanced(game->assets.models[UTILITY_POLE_MODEL].meshes[0],
                    game->assets.models[UTILITY_POLE_MODEL].materials[0],
                    world->utilityPoleTransforms,
                    WORLD_UTILITY_POLE_COUNT);
  drawUtilityPoleLines(world);

  // Draw BVH leafs.
#ifdef FT_DEBUG_MODE
  if (IsKeyPressed(KEY_RIGHT))
  {
    ++world->bvhDebugSelect;
  }
  if (IsKeyPressed(KEY_LEFT))
  {
    --world->bvhDebugSelect;
  }
  
  drawBVHDebug(world->bvh, 0, world->bvhDebugSelect);
#endif
}

void freeWorldBVH(BVHNode bvh)
{
  for (int index = 0; index < BVH_MAX_BRANCH_COUNT; ++index)
  {
    if (bvh.branches[index] != NULL)
    {
      freeWorldBVH(*bvh.branches[index]);
      FT_FREE(bvh.branches[index]);
    }
  }
}

void freeWorld(World world)
{
  UnloadTexture(world.heightmapTexture);
  UnloadTexture(
    world.heightmap.materials[0].maps[MATERIAL_MAP_DIFFUSE].texture);
  UnloadModel(world.heightmap);
  freeWorldBVH(world.bvh);
}

Vector2 worldPositionToPixel(const World* world, float x, float y)
{
  return (Vector2){
    roundf((float)WORLD_IMAGE_WIDTH / world->size.x * x),
    roundf((float)WORLD_IMAGE_HEIGHT / world->size.z * y)
  };
}

float getWorldHeightAtLocation(const World* world, float x, float y)
{
  float mapX = (float)WORLD_IMAGE_WIDTH / world->size.x * x;
  float mapY = (float)WORLD_IMAGE_HEIGHT / world->size.z * y;

  for (int yOffset = -1; yOffset < 2; ++yOffset)
  {
    for (int xOffset = -1; xOffset < 2; ++xOffset)
    {
      int pixelX = mapX + xOffset;
      int pixelY = mapY + yOffset;

      if (pixelX < 0 || pixelX >= WORLD_IMAGE_WIDTH ||
          pixelY < 0 || pixelY >= WORLD_IMAGE_HEIGHT)
      {
        continue;
      }

      int verticeStart = (pixelY * (WORLD_IMAGE_WIDTH - 1) + pixelX) * 18;
      float* vertices = &world->heightmap.meshes[0].vertices[verticeStart];

      // Cast to triangles at pixel. Really hacky indeed.
      Ray ray = (Ray){
        .position = (Vector3){x, world->size.y * 2.0, y},
        .direction = (Vector3){0.0, -1.0, 0.0}
      };

      RayCollision result = GetRayCollisionTriangle(
        ray,
        (Vector3){vertices[0], vertices[1], vertices[2]},
        (Vector3){vertices[3], vertices[4], vertices[5]},
        (Vector3){vertices[6], vertices[7], vertices[8]});

      // Test other triangle.
      if (!result.hit)
      {
        result = GetRayCollisionTriangle(
          ray,
          (Vector3){vertices[9], vertices[10], vertices[11]},
          (Vector3){vertices[12], vertices[13], vertices[14]},
          (Vector3){vertices[15], vertices[16], vertices[17]});
      }

      if (result.hit)
      {
        return result.point.y;
      }
    }
  }

  return 0.0;
}

void castRayBVH(const World* world, BVHNode node, Ray ray, int* tests,
                WorldUID* closest, float* closestDistance)
{
  if (!GetRayCollisionBox(ray, node.box).hit)
  {
    return;
  }

  if (tests != NULL)
  {
    ++*tests;
  }

  // Leaf node.
  if (node.branchCount == 0)
  {
    for (int index = 0; index < BVH_MAX; ++index)
    {
      if (node.entities[index] != -1 &&
          GetRayCollisionBox(
            ray,
            world->entities[node.entities[index]].box).hit)
      {
        float distance = Vector3Distance(
          world->entities[node.entities[index]].position,
          ray.position);

        if (distance < *closestDistance)
        {
          *closest = node.entities[index];
          *closestDistance = distance;
        }
      }
    }
  }
  else // Subtree.
  {
    for (int index = 0; index < node.branchCount; ++index)
    {
      castRayBVH(world, *node.branches[index], ray, tests, closest,
                 closestDistance);
    }
  }

  return;
}

WorldUID castRayAtWorld(const World* world, Ray ray, int* tests)
{
  WorldUID uid = -1;
  float distance = Vector3LengthSqr(world->size);
  castRayBVH(world, world->bvh, ray, tests, &uid, &distance);
  return uid;
}

// Abortions are good. Get more abortions.