package phys

import "core:log"
import "core:math"

import rl "vendor:raylib"

BIN_COUNT :: 2056
BIN_SIZE :: 64

Body_Handle :: u32

world: struct {
  handles: [dynamic]u32,
  unused_handles: [dynamic]Body_Handle,
  bodies: [dynamic]Body,
  bins: [BIN_COUNT][dynamic]Body_Handle,
}

destroy_world :: proc() {
  for bin in world.bins {
    delete(bin)
  }
  delete(world.handles)
  delete(world.unused_handles)
  delete(world.bodies)
}

@(private="file")
hash_bin :: proc(x: i32, y: i32) -> u32 {
  return transmute(u32)((x * 73856093) ~ (y * 19349663))
}

@(private="file")
world_to_bin :: proc(point: Vec2) -> (i32, i32) {
  return \
    i32(math.floor(point.x / BIN_SIZE)), 
    i32(math.floor(point.y / BIN_SIZE))
}

@(private="file")
get_surrounding_bins :: proc(
  pos: Vec2,
  allocator := context.temp_allocator,
) -> []^[dynamic]Body_Handle {
  neighbors := make([]^[dynamic]Body_Handle, 9, allocator)

  center_x, center_y := world_to_bin(pos)

  idx := 0

  for offset_x in -1..=1 {
    for offset_y in -1..=1 {
      bin_idx := hash_bin(center_x + i32(offset_x), center_y + i32(offset_y))
      bin := &world.bins[bin_idx % BIN_COUNT]
      neighbors[idx] = bin
      idx += 1
    }
  }

  return neighbors
}

@(private="file")
find_bin :: proc(b: Body) -> ^[dynamic]Body_Handle {
  bin_x, bin_y := world_to_bin(b.pos + b.rect.start)
  return &world.bins[hash_bin(bin_x, bin_y) % BIN_COUNT]
}

@(private="file")
add_to_bins :: proc(b: Body) {
  bin := find_bin(b)
  idx := i32(len(bin))
  append(bin, b.handle)
  world.bodies[bin[idx]].bin_idx = idx
}

@(private="file")
remove_from_bins :: proc(b: Body) {
  bin := find_bin(b)

  assert(bin[b.bin_idx] == b.handle)

  last := pop(bin)
  if last != b.handle {
    bin[b.bin_idx] = last
    world.bodies[last].bin_idx = b.bin_idx
  }
}

get_body :: proc(h: Body_Handle) -> ^Body {
  return &world.bodies[world.handles[h]]
}

add_body :: proc(b: Body) -> (Body_Handle, ^Body) {
  handle: Body_Handle

  if b.rect.size.x > BIN_SIZE || b.rect.size.y > BIN_SIZE {
    log.warnf("Body size is too big (%vx%v)", b.rect.size.x, b.rect.size.y)
  }

  if len(world.unused_handles) > 0 {
    handle = pop(&world.unused_handles)
  } else {
    handle = cast(Body_Handle)len(world.handles)
    append(&world.handles, 0)
  }

  world.handles[handle] = u32(len(world.bodies))
  append(&world.bodies, b)

  body := &world.bodies[world.handles[handle]]
  body.handle = handle

  add_to_bins(body^)

  return handle, body
}

remove_body :: proc(h: Body_Handle) {
  b := get_body(h)

  remove_from_bins(b^)

  last := pop(&world.bodies)
  if last.handle != h {
    world.bodies[h] = last
    world.handles[last.handle] = world.handles[h]
  }

  append(&world.unused_handles, b.handle)
}

update_body :: proc(h: Body_Handle) {
  dt := rl.GetFrameTime()

  b := get_body(h)

  res_pos := b.pos + b.vel * dt

  rect := b.rect
  rect.start += b.pos

  res_rect := b.rect
  res_rect.start += res_pos

  bin_list := get_surrounding_bins(res_rect.start)

  b.collisions = {}

  total_len := 0

  for bin in bin_list {
    total_len += len(bin)

    for c_h in bin {
      if c_h == h {
        continue
      }

      c := get_body(c_h)

      c_rect := c.rect
      c_rect.start += c.pos

      if aabb(res_rect, c_rect) {
        if aabb_hori(rect, c_rect) {
          if b.vel.y > 0 {
            res_pos.y = c_rect.start.y - b.rect.size.y - b.rect.start.y
            b.collisions += {.DOWN}
          } else {
            res_pos.y = c_rect.start.y + c_rect.size.y - b.rect.start.y
            b.collisions += {.UP}
          }
          b.collisions += {.VERTICAL}
        } else if aabb_vert(rect, c.rect) {
          if b.vel.x > 0 {
            res_pos.x = c_rect.start.x - b.rect.size.x - b.rect.start.x
            b.collisions += {.LEFT}
          } else {
            res_pos.x = c_rect.start.x + c_rect.size.x - b.rect.start.x
            b.collisions += {.RIGHT}
          }
          b.collisions += {.HORIZONTAL}
        }
      }
    }
  }

  if .HORIZONTAL in b.collisions {
    b.vel.x = 0
  }
  if .VERTICAL in b.collisions {
    b.vel.y = 0
  }

  b.pos = res_pos

  prev_bin := hash_bin(world_to_bin(b.pos + b.rect.start))
  res_bin := hash_bin(world_to_bin(res_pos + b.rect.start))

  if prev_bin != res_bin {
    remove_from_bins(b^)
    add_to_bins(b^)
  }
}