package physics

import rl "vendor:raylib"

Scene :: struct {
	bodies:                                    #soa[dynamic]Body,
	suspension_constraints:                    #soa[dynamic]Suspension_Constraint,
	first_free_body_plus_one:                  i32,
	first_free_suspension_constraint_plus_one: i32,
}

Body :: struct {
	// Is this body alive (if not it doesn't exist)
	alive:               bool,
	// Pos
	x:                   rl.Vector3,
	// Linear vel
	v:                   rl.Vector3,
	// Orientation
	q:                   rl.Quaternion,
	// Angular vel (omega)
	w:                   rl.Vector3,
	// Mass
	inv_mass:            f32,
	// Moment of inertia
	inv_intertia_tensor: rl.Vector3,
	// 
	next_plus_one:       i32,
}

Suspension_Constraint :: struct {
	alive:         bool,
	// Pos relative to the body
	rel_pos:       rl.Vector3,
	// Dir relative to the body
	rel_dir:       rl.Vector3,
	// Handle of the rigid body
	body:          Body_Handle,
	// Rest distance
	rest:          f32,
	// Inverse stiffness
	compliance:    f32,

	// Runtime state
	hit:           bool,
	hit_point:     rl.Vector3,
	// rel_hit_point = rel_pos + rel_dir * hit_t
	hit_t:         f32,

	// Free list
	next_plus_one: i32,
}

// Index plus one, so handle 0 maps to invalid body
Body_Handle :: distinct i32
Suspension_Constraint_Handle :: distinct i32

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_handle_valid :: proc {
	is_body_handle_valid,
	is_suspension_constraint_handle_valid,
}

Body_Ptr :: #soa^#soa[]Body
Suspension_Constraint_Ptr :: #soa^#soa[]Suspension_Constraint

_invalid_body: #soa[1]Body
_invalid_suspension_constraint: #soa[1]Suspension_Constraint

/// Returns pointer to soa slice. NEVER STORE IT
get_body :: proc(scene: ^Scene, handle: Body_Handle) -> Body_Ptr {
	index := int(handle) - 1
	if index < 0 {
		slice := _invalid_body[:]
		return &slice[0]
	}

	bodies_slice := scene.bodies[:]
	return &bodies_slice[index]
}

add_body :: proc(scene: ^Scene, body: Body) -> Body_Handle {
	body_copy := body

	body_copy.alive = true
	body_copy.next_plus_one = 0

	if scene.first_free_body_plus_one > 1 {
		index := scene.first_free_body_plus_one
		new_body := get_body(scene, Body_Handle(index))
		next_plus_one := new_body.next_plus_one
		new_body^ = body_copy
		scene.first_free_body_plus_one = next_plus_one
		return Body_Handle(index)
	}

	append_soa(&scene.bodies, body_copy)
	index := len(scene.bodies)
	return Body_Handle(index)
}

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

		body.alive = false
		body.next_plus_one = scene.first_free_body_plus_one
		scene.first_free_body_plus_one = i32(handle)
	}
}

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

	index := int(handle) - 1
	slice := scene.suspension_constraints[:]
	return &slice[index]
}

add_suspension_constraint :: proc(
	scene: ^Scene,
	constraint: Suspension_Constraint,
) -> Suspension_Constraint_Handle {
	copy := constraint

	copy.alive = true
	copy.next_plus_one = 0

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

	append_soa(&scene.suspension_constraints, copy)
	index := len(scene.suspension_constraints)
	return Suspension_Constraint_Handle(index)
}

remove_suspension_constraint :: proc(scene: ^Scene, handle: Suspension_Constraint_Handle) {
	if is_handle_valid(handle) {
		constraint := get_suspension_constraint(scene, handle)

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

_get_first_free_body :: proc(scene: ^Scene) -> i32 {
	return scene.first_free_body_plus_one - 1
}

destroy_physics_scene :: proc(scene: ^Scene) {
	delete_soa(scene.bodies)
	delete_soa(scene.suspension_constraints)
}