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

int32_t getMortonCode(Vector3 position)
{
  int32_t mortonCode = 0;
  int16_t x = roundf(position.x);
  int16_t z = roundf(position.z);
  
  for (int bit = 0; bit < 32; ++bit)
  {
    if (bit % 2)
    {
      mortonCode |= IS_BIT_SET(x, (bit / 2)) << bit;
    }
    else
    {
      mortonCode |= IS_BIT_SET(z, (bit / 2)) << bit;
    }
  }

  return mortonCode;
}

void sortEntitiesUID(WorldUID entities[WORLD_ENTITY_MAX], const World* world)
{
  // Lazy selection sort.
  for (int outer = 0; outer < WORLD_ENTITY_MAX - 1; ++outer)
  {
    int minIndex = outer;

    for (int inner = outer + 1; inner < WORLD_ENTITY_MAX; ++inner)
    {
      int32_t entityCode = getMortonCode(
        world->entities[entities[inner]].position);
      int32_t minCode = getMortonCode(
        world->entities[entities[minIndex]].position);

      if (entityCode < minCode)
      {
        minIndex = inner;
      }
    }

    WorldUID temp = entities[outer];
    entities[outer] = entities[minIndex];
    entities[minIndex] = temp;
  }

  // Test
  /* for (int index = 0; index < WORLD_ENTITY_MAX; ++index) */
  /* { */
  /*   printf("%d\n", getMortonCode(world->entities[entities[index]].position)); */
  /*   PRINT_VECTOR3(world->entities[entities[index]].position); */
  /* } */
}

// Very messy right now. Mostly been playing around.
void buildWorldBVH(World* world)
{
  Entity* entities = world->entities;
  bool grouped[WORLD_ENTITY_MAX];
  WorldUID sorted[WORLD_ENTITY_MAX];

  // This is a mess thats not going to work.
  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
  {
    grouped[index] = false;
    sorted[index] = index;
  }

  //sortEntitiesUID(sorted, world);

  for (int nodeIndex = 0; nodeIndex < BVH_LEAF_COUNT; ++nodeIndex)
  {
    for (int leafIndex = 0; leafIndex < BVH_MAX; ++leafIndex)
    {
      BVHNode* leaf = &world->bvhTest[nodeIndex];

      // First entity.
      if (leafIndex == 0)
      {
        for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
        {
          if (!grouped[index])
          {
            leaf->entities[0] = index;
            grouped[index] = true;
            break;
          }
        }
        
        continue;
      }

      // Find closest to entity one.
      int closest = -1;
      float closestDistance = world->size.x * world->size.z * 2.0;

      for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
      {
        if (grouped[index])
        {
          continue;
        }

        // Average out distances and find current bounding box.
        float distance = 0.0;
        Vector3 min = entities[index].position;
        Vector3 max = entities[index].position;

        bool overlaps = false;

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

        BoundingBox overlapBox = (BoundingBox){min, max};
        distance /= (float)leafIndex;

        // Check if laptop will be caused.
        for (int overlapIndex = 0; overlapIndex < WORLD_ENTITY_MAX;
             ++overlapIndex)
        {
          if (!grouped[overlapIndex])
          {
            continue;
          }
          
          bool isPartOf = false;

          for (int partOfIndex = 0; partOfIndex < leafIndex + 1; ++partOfIndex)
          {
            if (overlapIndex == leaf->entities[partOfIndex])
            {
              isPartOf = true;
              break;
            }
          }

          if (isPartOf)
          {
            continue;
          }

          // TODO: Make use of entity bounding boxes.
          if (CheckCollisionBoxSphere(
                overlapBox, entities[overlapIndex].position, 0.5))
          {
            overlaps = true;
            break;
          }
        }

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

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

      // Last entity
      if (leafIndex == BVH_MAX - 1)
      {
        // Create bounding box. */
        leaf->box.min = world->entities[leaf->entities[0]].position;
        leaf->box.max = world->entities[leaf->entities[0]].position;

        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]].position);
          leaf->box.max = Vector3Max(
            leaf->box.max,
            world->entities[leaf->entities[index]].position);
        }
      }
    }
  }

  // test
  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
  {
    if (!grouped[index])
    {
      printf("%d\n", index);
    }
  }
}

World createWorld(int seed)
{
  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);

  // Heightmap.
  Mesh mesh = GenMeshHeightmap(image, world.size);
  world.heightmap = LoadModelFromMesh(mesh);
  world.texture = LoadTextureFromImage(image);
  world.heightmap.materials[0].maps[MATERIAL_MAP_DIFFUSE].texture =
    world.texture;

  UnloadImage(image);

  // Entities.
  for (int index = 0; index < WORLD_ENTITY_MAX; ++index)
  {
    FT_RANDOM16(seed);
    
    Entity entity = createEntity(seed % ENTITY_COUNT, Vector3Zero());
    entity.position.x = FT_RANDOM16(seed) % (int)world.size.x;
    entity.position.z = FT_RANDOM16(seed) % (int)world.size.z;
    entity.position.y = getWorldHeightAtLocation(world, entity.position.x,
                                                 entity.position.z) + 1.0;
    world.entities[index] = entity;
  }

  buildWorldBVH(&world);

  return world;
}

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);
  }

  for (int index = 0; index < BVH_LEAF_COUNT; ++index)
  {
    Color colors[] = {RED, GREEN, BLUE, ORANGE, YELLOW, PINK};
    DrawBoundingBox(world->bvhTest[index].box, colors[index % 6]);
  }
}

void freeWorld(World world)
{
  UnloadTexture(world.texture);
  UnloadModel(world.heightmap);
}

float getWorldHeightAtLocation(World world, float x, float y)
{
  float toMapX = (float)world.texture.width / world.size.x;
  float toMapY = (float)world.texture.height / world.size.z;
  int pixelX = x * toMapX;
  int pixelY = y * toMapY;

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

  // Clamp x and y to prevent ray being out of bounds.
  Vector2 min = (Vector2){vertices[0], vertices[2]};
  Vector2 max = (Vector2){vertices[0], vertices[2]};

  for (int index = 0; index < 18; index += 3)
  {
    Vector2 vertex = (Vector2){vertices[index], vertices[index + 2]};
    min = Vector2Min(min, vertex);
    max = Vector2Max(max, vertex);
  }

  // Cast to triangles at pixel. Really hacky indeed.
  Ray ray = (Ray){
    .position = (Vector3){
      Clamp(x, min.x, max.x),
      FLT_MAX_EXP,
      Clamp(y, min.y, max.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;
  }
  else // Fall back.
  {
    float height = 0.0;
    
    for (int index = 1; index < 18; index += 3)
    {
      height += vertices[index];
    }
    
    return height / 6.0;
  }
}

// Abortions are good. Get more abortions.