class_name Car
extends RigidBody3D

@export_group("Steering")
## Maximum angle of imaginary wheel at center front of the car
@export var max_steer_angle: float = 30
@export var steer_speed: float = 120

@export_group("Wheels")
@export var front_right: Wheel
@export var front_left: Wheel
@export var back_right: Wheel
@export var back_left: Wheel

@onready var wheels: Array[Wheel] = [front_right, front_left, back_right, back_left]
#kinda cheeks but ignorable
@onready var wheelbase: float = front_right.position.distance_to(back_right.position)
@onready var track: float = front_right.position.distance_to(front_left.position)

var current_steer_angle: float = 0

func _physics_process(delta: float) -> void:
	steer(delta)
	for wheel: Wheel in wheels:
		wheel.apply_forces(self)

func steer(delta: float) -> void:
	var steer_input = Input.get_axis("Steer Right", "Steer Left")
	if steer_input != 0:
		current_steer_angle += steer_input * steer_speed * delta
		current_steer_angle = clampf(current_steer_angle, -max_steer_angle, max_steer_angle)
	else:
		current_steer_angle = move_toward(current_steer_angle, 0, steer_speed * delta)
	
	var current_steer_angle_rads: float = abs(deg_to_rad(current_steer_angle))
	var numerator: float = 2 * wheelbase * sin(current_steer_angle_rads)
	var steer_radius: float = 2 * wheelbase * cos(current_steer_angle_rads)
	var inner_angle: float = atan(numerator / (steer_radius - track * sin(current_steer_angle_rads)))
	var outer_angle: float = atan(numerator / (steer_radius + track * sin(current_steer_angle_rads)))
	if 0 < current_steer_angle:
		front_right.rotation.y = inner_angle
		front_left.rotation.y = outer_angle
	else:
		front_right.rotation.y = -inner_angle
		front_left.rotation.y = -outer_angle

##Point argument is in local space
func get_velocity_at_point(point: Vector3) -> Vector3:
	return linear_velocity + angular_velocity.cross(point - center_of_mass)