class_name Wheel
extends RayCast3D

@export var wheel_radius: float = 0.316
@export var flipped: bool = false
@export_group("Suspension")
@export var suspension_rest_length: float = 0.1
@export var suspension_stiffness: float = 30_000
@export var suspension_damping: float = 2000

func _ready() -> void:
	target_position *= suspension_rest_length + wheel_radius
	$"RX-Wheel".rotate_y(PI if flipped else 0.0)

func apply_forces(car: Car) -> void:
	force_raycast_update()
	
	var force: Vector3 = get_suspension_force(car)
	car.apply_force(force, car.to_local(get_collision_point()))

func get_suspension_force(car: Car) -> Vector3:
	var up: Vector3 = global_basis.y
	if not is_colliding():
		$"RX-Wheel".position = -up * suspension_rest_length
		return Vector3.ZERO
	
	var collision_point: Vector3 = to_local(get_collision_point())
	var wheel_center: Vector3 = collision_point + (up * wheel_radius)
	$"RX-Wheel".position = wheel_center
	var suspension_length: float = -up.dot(wheel_center)
	var suspension_displacement: float = suspension_rest_length - suspension_length
	#https://en.wikipedia.org/wiki/Hookes_law
	var hookes_force_scalar: float = suspension_stiffness * suspension_displacement
	
	var wheel_velocity: Vector3 = car.get_velocity_at_point(car.to_local(to_global(wheel_center)))
	var wheel_up_velocity: float = up.dot(wheel_velocity)
	var damping_force_scalar: float = suspension_damping * wheel_up_velocity
	
	var y_force_scalar: float = hookes_force_scalar - damping_force_scalar
	var force: Vector3 = Vector3.UP * y_force_scalar
	
	return force