class_name Car
extends RigidBody3D

## X axis is RPM, Y axis is torque in NM
@export var torque_curve: Curve
@export var air_resistance: float = .3

@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
var rpm: float = 0

func _ready() -> void:
	for wheel: Wheel in wheels:
		wheel.set_new_car(self)

func _physics_process(delta: float) -> void:
	steer(delta)
	apply_air_resistance()
	get_tree().get_first_node_in_group("spedometer").text = str(linear_velocity.length())

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 = outer_angle
		front_left.rotation.y = inner_angle
	else:
		front_right.rotation.y = -inner_angle
		front_left.rotation.y = -outer_angle

func apply_air_resistance() -> void:
	var force: Vector3 = -air_resistance * linear_velocity * linear_velocity.length()
	apply_force(force, center_of_mass)
	DebugDraw3D.draw_arrow(
		to_global(center_of_mass),
		to_global(center_of_mass) + force,
		Color.YELLOW,
		.2
	)

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

## Returns torque in NM, per wheel on the drive train
func get_torque_per_wheel() -> float:
	var is_powered: Callable = func (wheel: Wheel) -> bool: return wheel.on_drivetrain
	var powered_wheel_count: int = wheels.filter(is_powered).size()
	if 0 < powered_wheel_count:
		var engine_torque: float = Input.get_action_strength("Accelerate") * torque_curve.sample_baked(rpm)
		return engine_torque / powered_wheel_count
	return 0.0