package physics

import "core:log"
import "libs:tracy"

_ :: log

Body_Config_Inertia_Mode :: enum {
	From_Shape,
	Explicit,
}

Immedate_State :: struct($T: typeid) {
	handle:   T,
	// When was this referenced last time (frame number)
	last_ref: u32,
}

Immediate_Container :: struct($T: typeid) {
	items:     map[u32]Immedate_State(T),
	num_items: i32,
}

// When ok = true, it means we found an existing item
immediate_container_find_or_add :: proc(
	c: $T/^Immediate_Container($E),
	simulation_frame: u32,
	id: u32,
) -> (
	handle: ^E,
	ok: bool,
) {
	if id in c.items {
		item := &c.items[id]
		if item.last_ref != simulation_frame {
			item.last_ref = simulation_frame
			c.num_items += 1
		}
		handle = &item.handle
		ok = true
	} else {
		c.num_items += 1
		c.items[id] = {
			handle   = {},
			last_ref = simulation_frame,
		}
		item := &c.items[id]
		handle = &item.handle
		ok = false
	}

	return
}

// Returns removed handles, allocated using allocator
immediate_container_prune :: proc(
	c: $T/^Immediate_Container($E),
	simulation_frame: u32,
	allocator := context.temp_allocator,
) -> (
	removed_handles: []E,
) {
	if int(c.num_items) == len(c.items) {
		return
	}

	num_unreferenced := len(c.items) - int(c.num_items)
	assert(num_unreferenced >= 0)

	removed_handles = make([]E, num_unreferenced, allocator)
	items_to_remove := make([]u32, num_unreferenced, context.temp_allocator)

	i := 0
	for k, &v in c.items {
		if v.last_ref != simulation_frame {
			items_to_remove[i] = k
			removed_handles[i] = v.handle
			i += 1
		}
	}

	assert(i == len(items_to_remove))

	for k in items_to_remove {
		delete_key(&c.items, k)
	}

	return
}

copy_map :: proc(dst, src: $T/^map[$K]$V) {
	clear(dst)
	reserve_map(dst, len(src))

	for k, v in src {
		dst[k] = v
	}
}

immediate_container_copy :: proc(dst, src: $T/^Immediate_Container($E)) {
	copy_map(&dst.items, &src.items)
	dst.num_items = src.num_items
}

immediate_container_destroy :: proc(c: $T/^Immediate_Container($E)) {
	delete_map(c.items)
}

immediate_body :: proc(scene: ^Scene, id: u32, config: Body_Config) -> (handle: Body_Handle) {
	state := &scene.solver_state
	sim_state := get_sim_state(scene)
	h, ok := immediate_container_find_or_add(&state.immediate_bodies, state.simulation_frame, id)

	if ok {
		update_body_from_config(sim_state, get_body(sim_state, h^), config)
	} else {
		h^ = add_body(sim_state, config)
	}

	handle = h^

	return
}

immediate_suspension_constraint :: proc(
	scene: ^Scene,
	id: u32,
	config: Suspension_Constraint_Config,
) -> (
	handle: Suspension_Constraint_Handle,
) {
	state := &scene.solver_state
	h, ok := immediate_container_find_or_add(
		&state.immediate_suspension_constraints,
		state.simulation_frame,
		id,
	)

	if !ok {
		h^ = add_suspension_constraint(get_sim_state(scene), {})
	}

	handle = h^
	update_suspension_constraint_from_config(
		get_suspension_constraint(get_sim_state(scene), handle),
		config,
	)

	return
}

immediate_engine :: proc(
	scene: ^Scene,
	id: u32,
	config: Engine_Config,
) -> (
	handle: Engine_Handle,
) {
	state := &scene.solver_state
	sim_state := get_sim_state(scene)
	h, ok := immediate_container_find_or_add(&state.immediate_engines, state.simulation_frame, id)

	if ok {
		update_engine_from_config(sim_state, get_engine(sim_state, h^), config)
	} else {
		h^ = add_engine(sim_state, config)
	}
	handle = h^

	return
}

immediate_level_geom :: proc(
	scene: ^Scene,
	id: u32,
	config: Level_Geom_Config,
) -> (
	handle: Level_Geom_Handle,
) {
	state := &scene.solver_state
	sim_state := get_sim_state(scene)
	h, ok := immediate_container_find_or_add(
		&state.immediate_level_geoms,
		state.simulation_frame,
		id,
	)

	if ok {
		update_level_geom_from_config(sim_state, get_level_geom(sim_state, h^), config)
	} else {
		h^ = add_level_geom(sim_state, config)
	}
	return
}

prune_immediate :: proc(scene: ^Scene) {
	tracy.Zone()

	sim_state := get_sim_state(scene)
	state := &scene.solver_state
	removed_bodies := immediate_container_prune(&state.immediate_bodies, state.simulation_frame)
	removed_wheels := immediate_container_prune(
		&state.immediate_suspension_constraints,
		state.simulation_frame,
	)
	removed_engines := immediate_container_prune(&state.immediate_engines, state.simulation_frame)
	removed_level_geoms := immediate_container_prune(
		&state.immediate_level_geoms,
		state.simulation_frame,
	)
	for handle in removed_bodies {
		remove_body(sim_state, handle)
	}
	for handle in removed_wheels {
		remove_suspension_constraint(sim_state, handle)
	}
	for handle in removed_engines {
		remove_engine(sim_state, handle)
	}
	for handle in removed_level_geoms {
		remove_level_geom(sim_state, handle)
	}
}