package phys

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

import hm "core:container/handle_map"

import "../fw"

BIN_COUNT :: 2056
BIN_SIZE :: 64

Body_Handle :: hm.Handle64

world: struct {
  bodies: hm.Dynamic_Handle_Map(Body, Body_Handle),
  bins: [BIN_COUNT][dynamic]Body_Handle,
}

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

  hm.dynamic_destroy(&world.bodies)
}

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

@(private)
_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)
_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)
_add_to_bins :: proc(b: ^Body) {
  bin := _find_bin(b^)
  idx := i32(len(bin))
  append(bin, b.handle)
  b.bin_idx = idx
}

@(private)
_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 := hm.get(&world.bodies, bin[b.bin_idx])
    swapped_body.bin_idx = b.bin_idx
  }
}

add_body :: proc(b: Body) -> 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)
  }

  handle := hm.add(&world.bodies, b)
  body := hm.get(&world.bodies, handle)
  _add_to_bins(body)

  return handle
}

remove_body :: proc(h: Body_Handle) {
  b := hm.get(&world.bodies, h)
  _remove_from_bins(b^)
  hm.remove(&world.bodies, h)
}

@(require_results)
get_closest_raycast_collision :: proc(
  rc: Raycast,
) -> (Raycast_Collision, bool) {
  nearest_collision: Raycast_Collision
  nearest_dist := math.INF_F32

  bin_list := get_surrounding_bins(rc.start)

  for bin in bin_list {
    for b_h in bin {
      b := hm.get(&world.bodies, b_h)

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

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

      collision, collided := raycast_to_aabb(rc, b^)
      if collided {
        dist := linalg.length2(collision.enter - rc.start)
        if dist < nearest_dist {
          nearest_dist = dist
          nearest_collision = collision
        }
      }
    }
  }

  return nearest_collision, nearest_collision.body.gen > 0
}

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
) -> []Raycast_Collision {
  bodies := make([dynamic]Raycast_Collision, allocator)

  bin_list := get_surrounding_bins(rc.start)

  for bin in bin_list {
    for b_h in bin {
      b := hm.get(&world.bodies, b_h)

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

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

      collision, collided := raycast_to_aabb(rc, b^)
      if collided {
        append(&bodies, collision)
      }
    }
  }

  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 := hm.get(&world.bodies, 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 := hm.get(&world.bodies, 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 := hm.get(&world.bodies, 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 := hm.get(&world.bodies, 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)
}