class_name Wheel
extends RayCast3D

@export var wheel_radius: float = 0.316
@export var flipped: bool = false
@export var on_drivetrain: bool = false
@export var rolling_resistance: float = 2
@export_group("Suspension")
@export var suspension_rest_length: float = 0.1
@export var suspension_stiffness: float = 30_000
@export var suspension_damping: float = 2000
@export_group("Friction")
@export var static_friction: float = 1.1

var car: Car
var acceleration: Vector3
var _last_velocity: Vector3

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

func set_new_car(new_car: Car) -> void:
	car = new_car
	# Little bit freaky but we put up with it
	_last_velocity = car.get_velocity_at_point(get_collision_point())

func _physics_process(delta: float) -> void:
	var velocity: Vector3 = car.get_velocity_at_point(get_collision_point())
	acceleration = velocity - _last_velocity / delta
	force_raycast_update()
	
	apply_suspension_force()
	apply_rolling_resistance()
	apply_friction_force()
	
	_last_velocity = car.get_velocity_at_point(get_collision_point())

func apply_suspension_force() -> void:
	var up: Vector3 = global_basis.y
	if not is_colliding():
		$"RX-Wheel".position = -up * suspension_rest_length
		return
	
	var wheel_center: Vector3 = get_wheel_center()
	$"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(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 = up * y_force_scalar
	
	DebugDraw3D.draw_arrow(
		to_global(wheel_center),
		to_global(wheel_center + force / 1000),
		Color.RED,
		.2
	)
	car.apply_force(force, car.to_local(to_global(wheel_center)))

func apply_rolling_resistance() -> void:
	if not is_colliding(): return
	
	var normal: Vector3 = get_collision_normal()
	
	var velocity: Vector3 = car.get_velocity_at_point(get_collision_point())
	velocity -= normal * normal.dot(velocity)
	var force: Vector3 = -rolling_resistance * velocity
	var arrow_tail: Vector3 = to_global(get_wheel_center())
	
	car.apply_force(force, car.to_local(get_collision_point()))
	DebugDraw3D.draw_arrow(
		arrow_tail,
		arrow_tail + force / 250,
		Color.ORANGE,
		.2
	)

func apply_friction_force() -> void:
	if not on_drivetrain: return
	if not is_colliding(): return
	
	var forwards: Vector3 = -global_basis.z
	var backwards: Vector3 = global_basis.z
	var normal: Vector3 = get_collision_normal()
	
	var total_weight: Vector3 = car.mass * car.get_gravity()
	var wheel_to_com: float = abs(
		forwards.dot(car.center_of_mass) - \
		forwards.dot(car.to_local(get_collision_point()))
	)
	var weight: Vector3 = wheel_to_com / car.wheelbase * total_weight
	var normal_force: Vector3 = -weight
	
	# Intern please fix
	var surface_static_friction: float = 1
	var static_friction_coeff: float = static_friction * surface_static_friction
	var maximum_static_friction: float = static_friction_coeff *  normal_force.length()
	
	var torque: float = car.get_torque_per_wheel()
	var applied_force: Vector3 = backwards * torque / wheel_radius
	#Restricts applied force to plane parralel to surface
	applied_force -= normal * applied_force.dot(normal)
	
	var friction_force: Vector3 = Vector3.ZERO
	if applied_force.length() <= maximum_static_friction:
		friction_force = -applied_force
	
	var arrow_tail: Vector3 = to_global(get_wheel_center())
	car.apply_force(friction_force, car.to_local(get_collision_point()))
	DebugDraw3D.draw_arrow(
		arrow_tail,
		arrow_tail + friction_force / 250,
		Color.BLUE,
		.2
	)

func get_wheel_center() -> Vector3:
	var up: Vector3 = global_basis.y
	var collision_point: Vector3 = to_local(get_collision_point())
	return collision_point + (up * wheel_radius)