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,

	// 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,
}

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_inertia_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,
	// Wheel radius
	radius:          f32,
	// Rest distance
	rest:            f32,
	// Inverse stiffness
	compliance:      f32,
	// How much to damp velocity of the spring
	damping:         f32,

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

	// 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

INVALID_BODY :: Body_Handle(0)
INVALID_SUSPENSION_CONSTRAINT :: Suspension_Constraint_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_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_body_slice := _invalid_body[:]

_invalid_suspension_constraint: #soa[1]Suspension_Constraint
_invalid_suspension_constraint_slice := _invalid_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 || index >= len(scene.bodies_slice) {
		return &_invalid_body_slice[0]
	}

	return &scene.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 > 0 {
		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)

	scene.bodies_slice = 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) {
		return &_invalid_suspension_constraint_slice[0]
	}

	index := int(handle) - 1
	return &scene.suspension_constraints_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)
	scene.suspension_constraints_slice = scene.suspension_constraints[:]
	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)
}