package phys

import "core:log"
import "core:math"
import "core:math/linalg"

import "../fw"

BIN_COUNT :: 2056
BIN_SIZE :: 64

Body_Handle :: struct {
  idx: u32,
  uses: u32,
}

world: struct {
  bodies: [dynamic]Body,
  holes: [dynamic]Body_Handle,
  bins: [BIN_COUNT][dynamic]Body_Handle,
}

destroy_world :: proc() {
  for bin in world.bins {
    delete(bin)
  }
  delete(world.holes)
  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))
}

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)
  b.bin_idx = idx
}

@(private = "file")
_remove_from_bins :: proc(b: Body) {
  bin := _find_bin(b)

  assert(len(bin) > 0)
  assert(bin[b.bin_idx] == b.handle)

  unordered_remove(bin, b.bin_idx)
  if int(b.bin_idx) != len(bin) {
    swapped_body := _get_body(bin[b.bin_idx])
    swapped_body.bin_idx = b.bin_idx
  }
}

@(private = "file")
_get_body :: proc(h: Body_Handle, location := #caller_location) -> ^Body {
  if h.idx >= u32(len(world.bodies)) {
    log.warn("Handle is out of range", location = location)
    return nil
  }
  if world.bodies[h.idx].handle != h {
    log.warnf(
      "Handle isn't yours (querying %v, got %v)",
      h,
      world.bodies[h.idx].handle,
      location = location,
    )
  }
  return &world.bodies[h.idx]
}

@(require_results)
iterate_bodies :: proc(it: ^int) -> (Body_Handle, bool) {
  for it^ < len(world.bodies) {
    if world.bodies[it^].handle.uses > 0 {
      it^ += 1
      return world.bodies[it^ - 1].handle, true
    }
    it^ += 1
  }
  return {}, false
}

add_body :: proc(b: Body) -> Body_Handle {
  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.holes) > 0 {
    handle = pop(&world.holes)
    handle.uses += 1
  } else {
    resize(&world.bodies, len(world.bodies) + 1)
    handle.idx = u32(len(world.bodies) - 1)
    handle.uses = 1
  }
  
  world.bodies[handle.idx] = b
  world.bodies[handle.idx].handle = handle

  _add_to_bins(_get_body(handle))

  return handle
}

remove_body :: proc(h: Body_Handle) {
  if h.idx >= u32(len(world.bodies)) ||
     world.bodies[h.idx].handle != h {
    log.warn("Handle isn't yours, or is out of range")
    return
  }

  b := _get_body(h)

  _remove_from_bins(b^)
  
  append(&world.holes, b.handle)
  world.bodies[h.idx] = {}
}

is_colliding :: proc{
  is_body_colliding,
  is_raycast_colliding,
}

@(require_results)
is_raycast_colliding :: proc(rc: Raycast) -> bool {
  // TODO: TEMP
  return len(get_colliding_bodies(rc, allocator = context.temp_allocator)) > 0
}

@(require_results)
is_body_colliding :: proc(h: Body_Handle) -> bool {
  // TODO: TEMP
  return len(get_colliding_bodies(h, allocator = context.temp_allocator)) > 0
}

get_colliding_bodies :: proc{
  get_colliding_bodies_body_vs_body,
  get_colliding_bodies_raycast_vs_body,
}

@(require_results)
get_colliding_bodies_raycast_vs_body :: proc(
  rc: Raycast,
  allocator := context.allocator
) -> []Body_Handle {
  bodies := make([dynamic]Body_Handle, allocator)

  bin_list := get_surrounding_bins(rc.start)

  for bin in bin_list {
    for b_h in bin {
      b := _get_body(b_h)

      if b.layers & rc.mask == nil {
        continue
      }

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

      if raycast_to_aabb(rc, b^) {
        append(&bodies, b_h)
      }
    }
  }

  return bodies[:]
}

@(require_results)
get_colliding_bodies_body_vs_body :: proc(
  h: Body_Handle,
  allocator := context.allocator
) -> []Body_Handle {
  bodies := make([dynamic]Body_Handle, allocator)

  b := _get_body(h)

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

  bin_list := get_surrounding_bins(rect.start)

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

      c := _get_body(c_h)

      if b.mask & c.layers == nil {
        continue
      }

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

      if aabb(rect, c_rect) {
        append(&bodies, c_h)
      }
    }
  }

  return bodies[:]
}

update_body :: proc(h: Body_Handle) {
  dt := f32(fw.get_delta_time())

  b := _get_body(h)

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

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

  small_side := min(rect.size.x, rect.size.y)
  steps := i32(math.ceil(linalg.length(b.vel * dt) / small_side))

  b.collisions = nil

  for i in 0..<steps {
    p := f32(i + 1) / f32(steps)

    res_rect := b.rect
    res_rect.start += b.pos + b.vel * dt * p

    bin_list := get_surrounding_bins(res_rect.start)

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

        c := _get_body(c_h)

        if b.mask & c.layers == nil {
          continue
        }

        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
    }

    if b.collisions != nil {
      break
    }
  }

  set_position(h, res_pos)
}

@(require_results)
get_velocity :: proc(h: Body_Handle) -> Vec2 {
  return _get_body(h).vel
}

set_velocity :: proc(h: Body_Handle, vel: Vec2) {
  _get_body(h).vel = vel
}

@(require_results)
get_position :: proc(h: Body_Handle) -> Vec2 {
  return _get_body(h).pos
}

set_position :: proc(h: Body_Handle, new_pos: Vec2) {
  b := _get_body(h)

  prev_bin := _hash_bin(_world_to_bin(b.pos + b.rect.start))
  res_bin := _hash_bin(_world_to_bin(new_pos + b.rect.start))

  if prev_bin != res_bin {
    _remove_from_bins(b^)
    b.pos = new_pos
    _add_to_bins(b)
  } else {
    b.pos = new_pos
  }
}

@(require_results)
get_collisions :: proc(h: Body_Handle) -> bit_set[Collision_Type;u8] {
  return _get_body(h).collisions
}

@(require_results)
get_rect :: proc(h: Body_Handle) -> Rect {
  return _get_body(h).rect
}