package physics

import "bvh"
import "collision"
import "common:name"
import lg "core:math/linalg"
import "core:strings"
import "game:assets"
import "game:container/spanpool"
import "libs:tracy"

MAX_CONTACTS :: 1024 * 16

Vec3 :: [3]f32
Vec2 :: [2]f32
Quat :: quaternion128
Matrix3 :: # row_major matrix[3, 3]f32
AABB :: struct {
	center: Vec3,
	extent: Vec3,
}

Contact_Pair_Bodies :: struct {
	a, b:             Body_Handle,
	shape_a, shape_b: i32,
}
Contact_Pair_Body_Level :: struct {
	a:       Body_Handle,
	b:       Level_Geom_Handle,
	shape_a: i32,
	tri_idx: i32,
}

Contact_Pair :: union {
	Contact_Pair_Bodies,
	Contact_Pair_Body_Level,
}

make_contact_pair_bodies :: proc(
	body_a: int,
	body_b: int,
	shape_a: i32,
	shape_b: i32,
) -> Contact_Pair_Bodies {
	return {
		index_to_body_handle(min(body_a, body_b)),
		index_to_body_handle(max(body_a, body_b)),
		shape_a,
		shape_b,
	}
}

make_contact_pair_body_level :: proc(
	body: Body_Handle,
	level_geom_handle: Level_Geom_Handle,
	shape_a: i32,
	tri_idx: i32,
) -> Contact_Pair_Body_Level {
	return {body, level_geom_handle, shape_a, tri_idx}
}

Contact_Container :: struct {
	// body index pair to contact index
	lookup:   map[Contact_Pair]i32,
	contacts: #soa[dynamic]Contact,
}

contact_container_copy :: proc(dst: ^Contact_Container, src: Contact_Container) {
	clear(&dst.lookup)
	reserve(&dst.lookup, cap(src.lookup))
	resize_soa(&dst.contacts, len(src.contacts))

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

	for i in 0 ..< len(src.contacts) {
		dst.contacts[i] = src.contacts[i]
	}
}

Sim_State :: struct {
	scene:                                     ^Scene,
	bodies:                                    #soa[dynamic]Body,
	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
	engines:                                   [dynamic]Engine,
	level_geoms:                               [dynamic]Level_Geom,
	// Number of alive bodies
	num_bodies:                                i32,
	num_engines:                               i32,
	num_level_geoms:                           i32,

	// Slices. When you call get_body or get_suspension_constraint you will get a pointer to an element in this slice
	bodies_slice:                              #soa[]Body,
	suspension_constraints_slice:              #soa[]Suspension_Constraint,
	shapes:                                    [dynamic]Collision_Shape,
	first_free_body_plus_one:                  i32,
	first_free_suspension_constraint_plus_one: i32,
	first_free_engine_plus_one:                i32,
	first_free_level_geom_plus_one:            i32,
	first_free_shape_plus_one:                 i32,

	// Persistent stuff for simulation
	contact_container:                         Contact_Container,
	convex_container:                          Convex_Container,

	// NOTE: kinda overkill, but it simplifies copying sim states around a lot
	// Engine array data
	rpm_torque_curves_pool:                    spanpool.Span_Pool([2]f32),
	gear_ratios_pool:                          spanpool.Span_Pool(f32),

	// Level geometry
	geometry_vertices_pool:                    spanpool.Span_Pool(Vec3),
	geometry_indices_pool:                     spanpool.Span_Pool(u16),

	// BLAS for level geom
	blas_nodes_pool:                           spanpool.Span_Pool(bvh.Node),
	blas_primitives_pool:                      spanpool.Span_Pool(u16),

	// Temp stuff, kept for one frame, allocated from temp_allocator
	static_tlas:                               Static_TLAS,
	dynamic_tlas:                              Dynamic_TLAS,
}

Scene :: struct {
	assetman:               ^assets.Asset_Manager,
	simulation_states:      [2]Sim_State,
	simulation_state_index: i32,
	solver_state:           Solver_State,
}

// Copy current state to next
copy_sim_state :: proc(dst: ^Sim_State, src: ^Sim_State) {
	tracy.Zone()
	convex_container_reconcile(&src.convex_container)

	dst.scene = src.scene
	dst.num_bodies = src.num_bodies
	dst.first_free_body_plus_one = src.first_free_body_plus_one
	dst.first_free_suspension_constraint_plus_one = src.first_free_suspension_constraint_plus_one
	dst.first_free_engine_plus_one = src.first_free_engine_plus_one
	dst.first_free_shape_plus_one = src.first_free_shape_plus_one

	resize(&dst.bodies, len(src.bodies))
	resize(&dst.suspension_constraints, len(src.suspension_constraints))
	resize(&dst.engines, len(src.engines))
	resize(&dst.level_geoms, len(src.level_geoms))
	resize(&dst.shapes, len(src.shapes))

	dst.bodies_slice = dst.bodies[:]
	dst.suspension_constraints_slice = dst.suspension_constraints[:]

	for i in 0 ..< len(dst.bodies) {
		dst.bodies[i] = src.bodies[i]
	}
	for i in 0 ..< len(dst.suspension_constraints) {
		dst.suspension_constraints[i] = src.suspension_constraints[i]
	}
	copy(dst.engines[:], src.engines[:])
	copy(dst.level_geoms[:], src.level_geoms[:])
	copy(dst.shapes[:], src.shapes[:])

	contact_container_copy(&dst.contact_container, src.contact_container)
	convex_container_copy(&dst.convex_container, src.convex_container)
	spanpool.copy(&dst.rpm_torque_curves_pool, src.rpm_torque_curves_pool)
	spanpool.copy(&dst.gear_ratios_pool, src.gear_ratios_pool)
	spanpool.copy(&dst.geometry_vertices_pool, src.geometry_vertices_pool)
	spanpool.copy(&dst.geometry_indices_pool, src.geometry_indices_pool)
	spanpool.copy(&dst.blas_nodes_pool, src.blas_nodes_pool)
	spanpool.copy(&dst.blas_primitives_pool, src.blas_primitives_pool)
}

copy_physics_scene :: proc(dst, src: ^Scene) {
	tracy.Zone()

	dst.assetman = src.assetman
	dst.simulation_state_index = src.simulation_state_index
	src_sim_state := get_sim_state(src)
	dst_sim_state := get_sim_state(dst)

	copy_sim_state(dst_sim_state, src_sim_state)
	copy_solver_state(&dst.solver_state, &src.solver_state)
}

Body :: struct {
	// Is this body alive (if not it doesn't exist)
	alive:              bool,
	// Pos
	x:                  Vec3,
	// Linear vel
	v:                  Vec3,
	// Orientation
	q:                  Quat,
	// Angular vel (omega)
	w:                  Vec3,
	prev_x:             Vec3,
	prev_v:             Vec3,
	prev_q:             Quat,
	prev_w:             Vec3,
	// Mass
	inv_mass:           f32,
	// Moment of inertia
	inv_inertia_tensor: Matrix3,
	shape:              i32,
	// Collision shape offset = -combined_center_of_mass
	shape_offset:       Vec3,
	shape_aabb:         AABB,
	name:               name.Name,
	// 
	next_plus_one:      i32,
}

Shape_Sphere :: struct {
	radius: f32,
}

Shape_Box :: struct {
	size: Vec3,
}

Internal_Shape_Convex :: struct {
	mesh:           Convex_Handle,
	center_of_mass: Vec3,
	inertia_tensor: Matrix3,
	center:         Vec3,
	extent:         Vec3,
}

Shape_Convex :: struct {
	mesh:           collision.Convex,
	center_of_mass: Vec3,
	inertia_tensor: Matrix3,
	total_volume:   f32,
}

Collision_Shape_Internal :: union {
	Shape_Box,
	Internal_Shape_Convex,
}

Shape_Compound :: struct {
	rel_x:       Vec3,
	rel_q:       Quat,
	inner_shape: Collision_Shape_Internal,
	next_index:  i32,
	prev_index:  i32,
}

Collision_Shape :: struct {
	rel_x:       Vec3,
	rel_q:       Quat,
	inner_shape: Collision_Shape_Internal,
	next_index:  i32,
	prev_index:  i32,
}

Suspension_Constraint :: struct {
	alive:                  bool,
	// Pos relative to the body
	rel_pos:                Vec3,
	// Dir relative to the body
	rel_dir:                Vec3,
	// Handle of the rigid body
	body:                   Body_Handle,
	// Wheel radius
	radius:                 f32,
	// Wheel mass
	mass:                   f32,
	// Rest distance
	rest:                   f32,
	// Frequency of the spring, affects stiffness
	natural_frequency:      f32,
	// How much to damp velocity of the spring
	damping:                f32,
	pacejka_long:           Pacejka94_Longitudinal_Params,
	pacejka_lat:            Pacejka94_Lateral_Params,

	// Runtime state

	// Accumulated impulse
	spring_impulse:         f32,
	lateral_impulse:        f32,
	longitudinal_impulse:   f32,
	brake_friction_impulse: f32,

	// Inverse inertia along wheel rotation axis
	inv_inertia:            f32,

	// Rotation
	q:                      f32,
	// Angular velocity
	w:                      f32,
	hit:                    bool,
	hit_normal:             Vec3,
	hit_point:              Vec3,
	// rel_hit_point = rel_pos + rel_dir * hit_t
	hit_t:                  f32,
	turn_angle:             f32,
	drive_impulse:          f32,
	// Impulse of brake pad on brake disc, set outsie
	brake_impulse:          f32,
	applied_impulse:        Vec3,

	// Convenience for debug visualization to avoid recomputing
	slip_angle:             f32,
	slip_ratio:             f32,
	// Multipliers for combined friction
	slip_vec:               Vec2,

	// Free list
	next_plus_one:          i32,
}

Diff_Type :: enum {
	Open,
	Fixed,
	// TODO: LSD
}

Drive_Wheel :: struct {
	wheel:   Suspension_Constraint_Handle,
	impulse: f32,
}

Drive_Axle :: struct {
	// Params
	wheels:            [2]Drive_Wheel,
	wheel_count:       i32,
	diff_type:         Diff_Type,
	final_drive_ratio: f32,

	// State
	// Diff angular vel
	w:                 f32,
	// Impulse that constrains diff and engine motion
	engine_impulse:    f32,
	// Impulse that constrains wheels motion relative to each other
	diff_impulse:      f32,
}

Engine_Curve_Handle :: distinct spanpool.Handle
Gear_Ratios_Handle :: distinct spanpool.Handle

// This actually handles everything, engine, transmission, differential, etc. It's easier to keep it in one place
Engine :: struct {
	alive:              bool,
	// Engine Params
	rpm_torque_curve:   Engine_Curve_Handle,
	lowest_rpm:         f32,
	// Rpm when rev limiter activates
	rev_limit_rpm:      f32,
	// Time in seconds for how long rev limiter cuts the throttle
	rev_limit_interval: f32,
	inertia:            f32,
	internal_friction:  f32,

	// Transmission Params
	// 0 - reverse, 1 - first, etc.
	gear_ratios:        Gear_Ratios_Handle,
	axle:               Drive_Axle,

	// Engine State
	// Angular velocity, omega
	q:                  f32,
	w:                  f32,

	// Reset to -rev_limit_interval when rpm exceeds rev_limit_rpm, has to be >= 0 for throttle to work
	rev_limit_time:     f32,

	// Impulse applied when engine is stalling (rpm < lowest_rpm)
	unstall_impulse:    f32,
	// Friction that makes rpm go down when you're not accelerating
	friction_impulse:   f32,
	// Impulse applied from releasing throttle
	throttle_impulse:   f32,

	// Transmission State
	// -1 - reeverse, 0 - neutral, 1 - first, etc.
	gear:               i32,

	// Controls
	throttle:           f32,
	clutch:             f32,

	// Free list
	next_plus_one:      i32,
}

Geometry_Handle :: struct {
	vertices: spanpool.Handle,
	indices:  spanpool.Handle,
}

BLAS_Handle :: struct {
	nodes:      spanpool.Handle,
	primitives: spanpool.Handle,
}

Level_Geom_Source_Local :: struct {
	geometry: Geometry_Handle,
	blas:     BLAS_Handle,
}

Level_Geom_Source_Asset :: struct {
	assetpath: name.Name,
}

Level_Geom_Source :: union #no_nil {
	Level_Geom_Source_Local,
	Level_Geom_Source_Asset,
}

Level_Geom :: struct {
	alive:         bool,
	aabb:          AABB,
	source:        Level_Geom_Source,
	x:             Vec3,
	q:             Quat,
	next_plus_one: i32,
}

// Index plus one, so handle 0 maps to invalid body
Body_Handle :: distinct i32
Suspension_Constraint_Handle :: distinct i32
Engine_Handle :: distinct i32
// Handle for static geometry
Level_Geom_Handle :: distinct i32

INVALID_BODY :: Body_Handle(0)
INVALID_SUSPENSION_CONSTRAINT :: Suspension_Constraint_Handle(0)
INVALID_ENGINE :: Engine_Handle(0)
INVALID_LEVEL_GEOM :: Level_Geom_Handle(0)

is_body_handle_valid :: proc(handle: Body_Handle) -> bool {
	return i32(handle) > 0
}
is_suspension_constraint_handle_valid :: proc(handle: Suspension_Constraint_Handle) -> bool {
	return i32(handle) > 0
}
is_engine_handle_valid :: proc(handle: Engine_Handle) -> bool {
	return i32(handle) > 0
}
is_level_geom_handle_valid :: proc(handle: Level_Geom_Handle) -> bool {
	return i32(handle) > 0
}
is_handle_valid :: proc {
	is_body_handle_valid,
	is_suspension_constraint_handle_valid,
	is_engine_handle_valid,
	is_level_geom_handle_valid,
}

index_to_body_handle :: #force_inline proc(idx: int) -> Body_Handle {
	return Body_Handle(idx + 1)
}

index_to_level_geom :: #force_inline proc(idx: int) -> Level_Geom_Handle {
	return Level_Geom_Handle(idx + 1)
}

body_handle_to_index :: #force_inline proc(handle: Body_Handle) -> int {
	return int(handle) - 1
}

level_geom_handle_to_index :: #force_inline proc(handle: Level_Geom_Handle) -> int {
	return int(handle) - 1
}

handle_to_index :: proc {
	body_handle_to_index,
	level_geom_handle_to_index,
}

Body_Ptr :: #soa^#soa[]Body
Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint
Engine_Ptr :: ^Engine
Level_Geom_Ptr :: ^Level_Geom

_invalid_body: #soa[1]Body
_invalid_body_slice := _invalid_body[:]

_invalid_suspension_constraint: #soa[1]Suspension_Constraint
_invalid_suspension_constraint_slice := _invalid_suspension_constraint[:]

get_prev_sim_state_index :: proc(scene: ^Scene) -> i32 {
	return (scene.simulation_state_index - 1) %% i32(len(scene.simulation_states))
}

get_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
	return &scene.simulation_states[scene.simulation_state_index]
}

get_prev_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
	return &scene.simulation_states[get_prev_sim_state_index(scene)]
}

get_next_sim_state :: proc(scene: ^Scene) -> ^Sim_State {
	return(
		&scene.simulation_states[(scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))] \
	)
}

flip_sim_state :: proc(scene: ^Scene) {
	scene.simulation_state_index =
		(scene.simulation_state_index + 1) %% i32(len(scene.simulation_states))
}

/// Returns pointer to soa slice. NEVER STORE IT
get_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) -> Body_Ptr {
	index := int(handle) - 1
	if index < 0 || index >= len(sim_state.bodies_slice) {
		_invalid_body_slice[0] = {
			alive  = true,
			q      = lg.QUATERNIONF32_IDENTITY,
			prev_q = lg.QUATERNIONF32_IDENTITY,
		}
		return &_invalid_body_slice[0]
	}

	return &sim_state.bodies_slice[index]
}

remove_shape :: proc(sim_state: ^Sim_State, idx: i32) {
	cur_idx := idx
	for {
		shape := &sim_state.shapes[cur_idx]

		switch &s in shape.inner_shape {
		case Shape_Box:
		case Internal_Shape_Convex:
			convex_container_remove(&sim_state.convex_container, s.mesh)
		}

		prev_idx := cur_idx
		cur_idx = shape.next_index
		free_shape(sim_state, prev_idx)
		if cur_idx == idx {
			break
		}
	}
}

Input_Shape_Internal :: union {
	Shape_Box,
	Shape_Convex,
}

Input_Shape :: struct {
	rel_x:             Vec3,
	rel_q:             Quat,
	density_minus_one: f32,
	inner_shape:       Input_Shape_Internal,
}

Body_Config :: struct {
	name:            name.Name,
	initial_pos:     Vec3,
	initial_rot:     Quat,
	initial_vel:     Vec3,
	initial_ang_vel: Vec3,
	shapes:          []Input_Shape,
	mass:            f32,
	inertia_mode:    Body_Config_Inertia_Mode,
	// Unit inertia tensor
	inertia_tensor:  Matrix3,
	// Allows shifting the autocalculated center of mass
	com_shift:       Vec3,
}

// TODO: rename to wheel
Suspension_Constraint_Config :: struct {
	rel_pos:           Vec3,
	rel_dir:           Vec3,
	body:              Body_Handle,
	rest:              f32,
	natural_frequency: f32,
	damping:           f32,
	radius:            f32,
	mass:              f32,
	pacejka_long:      Pacejka94_Longitudinal_Params,
	pacejka_lat:       Pacejka94_Lateral_Params,
}

Drive_Axle_Config :: struct {
	wheels:            [2]Suspension_Constraint_Handle,
	wheel_count:       i32,
	diff_type:         Diff_Type,
	final_drive_ratio: f32,
}

Engine_Config :: struct {
	rpm_torque_curve:   [][2]f32,
	lowest_rpm:         f32,
	rev_limit_rpm:      f32,
	rev_limit_interval: f32,
	inertia:            f32,
	internal_friction:  f32,

	// Transmission Params
	// 0 - reverse, 1 - first, etc.
	gear_ratios:        []f32,
	axle:               Drive_Axle_Config,
}

Level_Geometry_Asset :: distinct string

Level_Geometry_Mesh :: struct {
	vertices: []Vec3,
	indices:  []u16,
}

// Either runtime mesh or asset reference which will be fetched from assetmanager
Level_Geometry_Source :: union #no_nil {
	Level_Geometry_Mesh,
	Level_Geometry_Asset,
}

Level_Geom_Config :: struct {
	position: Vec3,
	rotation: Quat,
	source:   Level_Geometry_Source,
}

calculate_body_params_from_config :: proc(
	config: Body_Config,
) -> (
	inv_mass: f32,
	inv_inertia_tensor: Matrix3,
) {
	inv_mass = config.mass == 0 ? 0 : 1.0 / config.mass

	inertia_tensor: Matrix3
	if config.inertia_mode == .Explicit {
		inertia_tensor = config.inertia_tensor
	} else {
		inertia_tensor = combined_inertia_tensor(config.shapes)
	}
	inertia_tensor = inertia_tensor * Matrix3(config.mass)

	inv_inertia_tensor = lg.determinant(inertia_tensor) == 0 ? 0 : lg.inverse(inertia_tensor)

	return
}

alloc_shape :: proc(sim_state: ^Sim_State) -> (idx: i32) {
	if sim_state.first_free_shape_plus_one != 0 {
		idx = sim_state.first_free_shape_plus_one - 1
		new_shape := &sim_state.shapes[idx]
		sim_state.first_free_shape_plus_one = new_shape.next_index + 1

		new_shape.prev_index = idx
		new_shape.next_index = idx

		return idx
	} else {
		idx = i32(len(sim_state.shapes))
		append(&sim_state.shapes, Collision_Shape{})
		new_shape := &sim_state.shapes[idx]

		new_shape.prev_index = idx
		new_shape.next_index = idx

		return idx
	}
}

free_shape :: proc(sim_state: ^Sim_State, idx: i32) {
	sim_state.shapes[idx].next_index = sim_state.first_free_shape_plus_one - 1
	sim_state.first_free_shape_plus_one = idx + 1
}

add_shape :: proc(sim_state: ^Sim_State, shapes: []Input_Shape) -> (first_idx: i32) {
	first_idx = -1
	last_idx := i32(-1)
	for shape in shapes {
		new_idx := alloc_shape(sim_state)
		new_shape := &sim_state.shapes[new_idx]
		if first_idx == -1 {
			first_idx = new_idx
		} else {
			sim_state.shapes[last_idx].next_index = new_idx
			sim_state.shapes[first_idx].prev_index = new_idx
			new_shape.prev_index = last_idx
			new_shape.next_index = first_idx
		}
		last_idx = new_idx

		new_shape.rel_x = shape.rel_x
		new_shape.rel_q = shape.rel_q

		switch s in shape.inner_shape {
		case Shape_Box:
			new_shape.inner_shape = s
		case Shape_Convex:
			convex: Internal_Shape_Convex
			convex.mesh = convex_container_add(&sim_state.convex_container, s.mesh)
			convex.center = s.mesh.center
			convex.extent = s.mesh.extent
			convex.center_of_mass = s.center_of_mass
			convex.inertia_tensor = s.inertia_tensor
			new_shape.inner_shape = convex
		}
	}

	return
}

initialize_body_from_config :: proc(sim_state: ^Sim_State, body: ^Body, config: Body_Config) {
	body.x = config.initial_pos
	body.q = config.initial_rot
	body.v = config.initial_vel
	body.w = config.initial_ang_vel

	body.shape = add_shape(sim_state, config.shapes)
	body.shape_offset = -combined_center_of_mass(config.shapes) + config.com_shift
	body.shape_aabb = input_shape_get_aabb(config.shapes)
	body.name = config.name

	body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
}

update_body_from_config :: proc(sim_state: ^Sim_State, body: Body_Ptr, config: Body_Config) {
	// Inefficient, but eh
	remove_shape(sim_state, body.shape)
	body.shape = add_shape(sim_state, config.shapes)
	body.shape_offset = -combined_center_of_mass(config.shapes) + config.com_shift
	body.shape_aabb = input_shape_get_aabb(config.shapes)
	body.name = config.name

	body.inv_mass, body.inv_inertia_tensor = calculate_body_params_from_config(config)
}

update_suspension_constraint_from_config :: proc(
	constraint: Suspension_Constraint_Ptr,
	config: Suspension_Constraint_Config,
) {
	constraint.rel_pos = config.rel_pos
	constraint.rel_dir = config.rel_dir
	constraint.body = config.body
	constraint.rest = config.rest
	constraint.natural_frequency = config.natural_frequency
	constraint.damping = config.damping
	constraint.radius = config.radius
	constraint.inv_inertia = 1.0 / (0.5 * config.mass * config.radius * config.radius)
	constraint.pacejka_long = config.pacejka_long
	constraint.pacejka_lat = config.pacejka_lat
}

add_engine_curve :: proc(sim_state: ^Sim_State, curve: [][2]f32) -> Engine_Curve_Handle {
	handle := spanpool.allocate_elems(&sim_state.rpm_torque_curves_pool, ..curve)
	return Engine_Curve_Handle(handle)
}

remove_engine_curve :: proc(sim_state: ^Sim_State, handle: Engine_Curve_Handle) {
	spanpool.free(&sim_state.rpm_torque_curves_pool, spanpool.Handle(handle))
}

get_engine_curve :: proc(sim_state: ^Sim_State, handle: Engine_Curve_Handle) -> [][2]f32 {
	return spanpool.resolve_slice(&sim_state.rpm_torque_curves_pool, spanpool.Handle(handle))
}

add_gear_ratios :: proc(sim_state: ^Sim_State, gear_ratios: []f32) -> Gear_Ratios_Handle {
	handle := spanpool.allocate_elems(&sim_state.gear_ratios_pool, ..gear_ratios)
	return Gear_Ratios_Handle(handle)
}

remove_gear_ratios :: proc(sim_state: ^Sim_State, handle: Gear_Ratios_Handle) {
	spanpool.free(&sim_state.gear_ratios_pool, spanpool.Handle(handle))
}

get_gear_ratios :: proc(
	sim_state: ^Sim_State,
	handle: Gear_Ratios_Handle,
	loc := #caller_location,
) -> []f32 {
	return spanpool.resolve_slice(&sim_state.gear_ratios_pool, spanpool.Handle(handle), loc)
}

update_engine_from_config :: proc(
	sim_state: ^Sim_State,
	engine: Engine_Ptr,
	config: Engine_Config,
) {
	remove_engine_curve(sim_state, engine.rpm_torque_curve)
	engine.rpm_torque_curve = add_engine_curve(sim_state, config.rpm_torque_curve)

	remove_gear_ratios(sim_state, engine.gear_ratios)
	engine.gear_ratios = add_gear_ratios(sim_state, config.gear_ratios)

	engine.lowest_rpm = config.lowest_rpm
	engine.rev_limit_rpm = config.rev_limit_rpm
	engine.rev_limit_interval = config.rev_limit_interval
	engine.inertia = config.inertia
	engine.internal_friction = config.internal_friction

	engine.axle.final_drive_ratio = config.axle.final_drive_ratio
	engine.axle.wheels[0].wheel = config.axle.wheels[0]
	engine.axle.wheels[1].wheel = config.axle.wheels[1]
	engine.axle.wheel_count = config.axle.wheel_count
	engine.axle.diff_type = config.axle.diff_type
}

add_body :: proc(sim_state: ^Sim_State, config: Body_Config) -> Body_Handle {
	body: Body

	sim_state.num_bodies += 1

	initialize_body_from_config(sim_state, &body, config)

	body.alive = true

	if sim_state.first_free_body_plus_one > 0 {
		index := sim_state.first_free_body_plus_one
		new_body := get_body(sim_state, Body_Handle(index))
		next_plus_one := new_body.next_plus_one
		new_body^ = body
		sim_state.first_free_body_plus_one = next_plus_one

		return Body_Handle(index)
	}

	append_soa(&sim_state.bodies, body)
	index := len(sim_state.bodies)

	sim_state.bodies_slice = sim_state.bodies[:]

	return Body_Handle(index)
}

remove_body :: proc(sim_state: ^Sim_State, handle: Body_Handle) {
	if int(handle) > 1 {
		body := get_body(sim_state, handle)

		body.alive = false

		remove_shape(sim_state, body.shape)

		body.next_plus_one = sim_state.first_free_body_plus_one
		sim_state.first_free_body_plus_one = i32(handle)

		sim_state.num_bodies -= 1
	}
}

/// Returns pointer to soa slice. NEVER STORE IT
get_suspension_constraint :: proc(
	sim_state: ^Sim_State,
	handle: Suspension_Constraint_Handle,
) -> Suspension_Constraint_Ptr {
	if !is_handle_valid(handle) {
		return &_invalid_suspension_constraint_slice[0]
	}

	index := int(handle) - 1
	return &sim_state.suspension_constraints_slice[index]
}

add_suspension_constraint :: proc(
	sim_state: ^Sim_State,
	constraint: Suspension_Constraint,
) -> Suspension_Constraint_Handle {
	copy := constraint

	copy.alive = true
	copy.next_plus_one = 0

	if sim_state.first_free_suspension_constraint_plus_one > 0 {
		index := sim_state.first_free_suspension_constraint_plus_one
		new_constraint := get_suspension_constraint(sim_state, Suspension_Constraint_Handle(index))
		next_plus_one := new_constraint.next_plus_one
		new_constraint^ = copy
		sim_state.first_free_suspension_constraint_plus_one = next_plus_one
		return Suspension_Constraint_Handle(index)
	}

	append_soa(&sim_state.suspension_constraints, copy)
	sim_state.suspension_constraints_slice = sim_state.suspension_constraints[:]
	index := len(sim_state.suspension_constraints)
	return Suspension_Constraint_Handle(index)
}

remove_suspension_constraint :: proc(sim_state: ^Sim_State, handle: Suspension_Constraint_Handle) {
	if is_handle_valid(handle) {
		constraint := get_suspension_constraint(sim_state, handle)

		constraint.alive = false
		constraint.next_plus_one = sim_state.first_free_suspension_constraint_plus_one
		sim_state.first_free_suspension_constraint_plus_one = i32(handle)
	}
}

invalid_engine: Engine

get_engine :: proc(sim_state: ^Sim_State, handle: Engine_Handle) -> Engine_Ptr {
	index := int(handle) - 1
	if index < 0 || index >= len(sim_state.bodies_slice) {
		return &invalid_engine
	}

	return &sim_state.engines[index]
}

add_engine :: proc(sim_state: ^Sim_State, config: Engine_Config) -> Engine_Handle {
	sim_state.num_engines += 1

	engine: Engine
	update_engine_from_config(sim_state, &engine, config)

	engine.alive = true

	if sim_state.first_free_engine_plus_one > 0 {
		index := sim_state.first_free_engine_plus_one
		new_engine := get_engine(sim_state, Engine_Handle(index))
		next_plus_one := new_engine.next_plus_one
		new_engine^ = engine
		sim_state.first_free_engine_plus_one = next_plus_one
		return Engine_Handle(index)
	}

	append(&sim_state.engines, engine)
	index := len(sim_state.engines)
	return Engine_Handle(index)
}

remove_engine :: proc(sim_state: ^Sim_State, handle: Engine_Handle) {
	if is_handle_valid(handle) {
		engine := get_engine(sim_state, handle)

		remove_engine_curve(sim_state, engine.rpm_torque_curve)
		remove_gear_ratios(sim_state, engine.gear_ratios)

		engine.alive = false
		engine.next_plus_one = sim_state.first_free_engine_plus_one
		sim_state.first_free_engine_plus_one = i32(handle)
		sim_state.num_engines -= 1
	}
}

invalid_level_geom: Level_Geom

get_level_geom :: proc(sim_state: ^Sim_State, handle: Level_Geom_Handle) -> Level_Geom_Ptr {
	index := int(handle) - 1
	if index < 0 || index >= len(sim_state.bodies_slice) {
		return &invalid_level_geom
	}

	return &sim_state.level_geoms[index]
}

get_level_geom_data :: proc(
	sim_state: ^Sim_State,
	handle: Level_Geom_Handle,
) -> (
	vertices: []Vec3,
	indices: []u16,
) {
	level_geom := get_level_geom(sim_state, handle)

	switch s in level_geom.source {
	case Level_Geom_Source_Local:
		vertices = spanpool.resolve_slice(&sim_state.geometry_vertices_pool, s.geometry.vertices)
		indices = spanpool.resolve_slice(&sim_state.geometry_indices_pool, s.geometry.indices)
	case Level_Geom_Source_Asset:
		loaded_bvh, reloaded := assets.get_bvh(
			sim_state.scene.assetman,
			strings.unsafe_string_to_cstring(name.to_string(s.assetpath)),
		)
		if reloaded {
			level_geom.aabb = bvh_aabb_to_phys_aabb(loaded_bvh.aabb)
		}
		vertices = loaded_bvh.vertices
		indices = loaded_bvh.indices
	}

	return
}

get_level_geom_blas :: proc(sim_state: ^Sim_State, handle: Level_Geom_Handle) -> (bvh: bvh.BVH) {
	level_geom := get_level_geom(sim_state, handle)
	switch s in level_geom.source {
	case Level_Geom_Source_Local:
		bvh.nodes = spanpool.resolve_slice(&sim_state.blas_nodes_pool, s.blas.nodes)
		bvh.primitives = spanpool.resolve_slice(&sim_state.blas_primitives_pool, s.blas.primitives)
		bvh.nodes_used = i32(len(bvh.nodes))
	case Level_Geom_Source_Asset:
		loaded_bvh, reloaded := assets.get_bvh(
			sim_state.scene.assetman,
			strings.unsafe_string_to_cstring(name.to_string(s.assetpath)),
		)
		if reloaded {
			level_geom.aabb = bvh_aabb_to_phys_aabb(loaded_bvh.aabb)
		}

		bvh = loaded_bvh.bvh
	}
	return
}

update_level_geom_from_config :: proc(
	sim_state: ^Sim_State,
	level_geom: Level_Geom_Ptr,
	config: Level_Geom_Config,
) {
	level_geom.x = config.position
	level_geom.q = config.rotation

	// TODO: figure out if asset changed and rebuild only then


}

add_level_geom :: proc(sim_state: ^Sim_State, config: Level_Geom_Config) -> Level_Geom_Handle {
	sim_state.num_level_geoms += 1

	level_geom: Level_Geom
	level_geom.alive = true
	update_level_geom_from_config(sim_state, &level_geom, config)

	switch s in config.source {
	case Level_Geometry_Mesh:
		assert(len(s.vertices) > 0)
		assert(len(s.vertices) <= int(max(u16)))
		blas :=
			bvh.build_bvh_from_mesh(bvh.Mesh{vertices = s.vertices, indices = s.indices}, context.temp_allocator).bvh

		source: Level_Geom_Source_Local
		source.blas.nodes = spanpool.allocate_elems(
			&sim_state.blas_nodes_pool,
			..blas.nodes[:blas.nodes_used],
		)
		source.blas.primitives = spanpool.allocate_elems(
			&sim_state.blas_primitives_pool,
			..blas.primitives,
		)

		aabb_min, aabb_max: Vec3 = max(f32), min(f32)
		for v in s.vertices {
			aabb_min = lg.min(aabb_min, v)
			aabb_max = lg.max(aabb_max, v)
		}

		level_geom.aabb.center = (aabb_max + aabb_min) * 0.5
		level_geom.aabb.extent = (aabb_max - aabb_min) * 0.5

		// if spanpool.is_handle_valid(
		// 	&sim_state.geometry_vertices_pool,
		// 	level_geom.geometry.vertices,
		// ) {
		// 	spanpool.free(&sim_state.geometry_vertices_pool, level_geom.geometry.vertices)
		// 	spanpool.free(&sim_state.geometry_indices_pool, level_geom.geometry.indices)
		// }

		source.geometry.vertices = spanpool.allocate_elems(
			&sim_state.geometry_vertices_pool,
			..s.vertices,
		)
		source.geometry.indices = spanpool.allocate_elems(
			&sim_state.geometry_indices_pool,
			..s.indices,
		)
		level_geom.source = source
	case Level_Geometry_Asset:
		level_geom.source = Level_Geom_Source_Asset {
			assetpath = name.from_string(string(s)),
		}

		bvh, _ := assets.get_bvh(
			sim_state.scene.assetman,
			strings.unsafe_string_to_cstring(string(s)),
		)
		level_geom.aabb = AABB {
			center = (bvh.aabb.max + bvh.aabb.min) * 0.5,
			extent = (bvh.aabb.max - bvh.aabb.min) * 0.5,
		}
	}

	if sim_state.first_free_level_geom_plus_one > 0 {
		index := sim_state.first_free_level_geom_plus_one
		new_level_geom := get_level_geom(sim_state, Level_Geom_Handle(index))
		next_plus_one := new_level_geom.next_plus_one
		new_level_geom^ = level_geom
		sim_state.first_free_level_geom_plus_one = next_plus_one
		return Level_Geom_Handle(index)
	}

	append(&sim_state.level_geoms, level_geom)
	index := len(sim_state.level_geoms)
	return Level_Geom_Handle(index)
}

remove_level_geom :: proc(sim_state: ^Sim_State, handle: Level_Geom_Handle) {
	level_geom := get_level_geom(sim_state, handle)

	switch s in level_geom.source {
	case Level_Geom_Source_Local:
		spanpool.free(&sim_state.geometry_vertices_pool, s.geometry.vertices)
		spanpool.free(&sim_state.geometry_indices_pool, s.geometry.indices)

		spanpool.free(&sim_state.blas_nodes_pool, s.blas.nodes)
		spanpool.free(&sim_state.blas_primitives_pool, s.blas.primitives)
	case Level_Geom_Source_Asset:
	}
}

_get_first_free_body :: proc(sim_state: ^Sim_State) -> i32 {
	return sim_state.first_free_body_plus_one - 1
}

destry_sim_state :: proc(sim_state: ^Sim_State) {
	delete_soa(sim_state.bodies)
	delete_soa(sim_state.suspension_constraints)
	delete(sim_state.engines)
	delete(sim_state.level_geoms)
	delete(sim_state.shapes)

	delete_soa(sim_state.contact_container.contacts)
	delete_map(sim_state.contact_container.lookup)
	convex_container_destroy(&sim_state.convex_container)
	spanpool.destroy_spanpool(&sim_state.rpm_torque_curves_pool)
	spanpool.destroy_spanpool(&sim_state.gear_ratios_pool)
	spanpool.destroy_spanpool(&sim_state.geometry_vertices_pool)
	spanpool.destroy_spanpool(&sim_state.geometry_indices_pool)

	static_tlas_destroy(&sim_state.static_tlas)
	dynamic_tlas_destroy(&sim_state.dynamic_tlas)
}

scene_init :: proc(scene: ^Scene, assetman: ^assets.Asset_Manager) {
	scene.assetman = assetman
	for &sim_state in scene.simulation_states {
		sim_state.scene = scene
	}
}

scene_destroy :: proc(scene: ^Scene) {
	for &sim_state in scene.simulation_states {
		destry_sim_state(&sim_state)
	}
	destroy_solver_state(&scene.solver_state)
}