class_name Biplan
extends SeatedVehicle

const PLANE_FORWARD := Vector3.BACK # z increases when moving forward

@export var thrust_power: float = 3500.0
@export var turn_to_torque: float = 10000.0
@export var move_to_pitch: float = 20000.0
@export var wing_resistance: float = 2500.0
@export var foil_resistance: float = 500.0
@export var lift: float = 25.0

var target_torque: float = 0.0
var target_pitch: float = 0.0
var target_rotation := Vector3.ZERO
var target_thrust := Vector3.ZERO

# Physics logs
var vertical_speed: float = 0.0
var horizontal_speed: float = 0.0
var local_wing_force := Vector3.ZERO
var local_foil_force := Vector3.ZERO
var local_lift_force := Vector3.ZERO

@onready var VerticalFoil : Node3D = $VerticalFoil


func _register_custom_monitors() -> void:
	return
	Performance.add_custom_monitor("plane/torque", func(): return abs(target_torque))
	Performance.add_custom_monitor("plane/pitch", func(): return abs(target_pitch))
	Performance.add_custom_monitor("plane/rotation", func(): return target_rotation.length())
	Performance.add_custom_monitor("plane/thrust", func(): return target_thrust.length())

	Performance.add_custom_monitor("plane/vertical_speed", func(): return abs(vertical_speed))
	Performance.add_custom_monitor("plane/horizontal_speed", func(): return abs(horizontal_speed))
	Performance.add_custom_monitor("plane/wing_force", func(): return local_wing_force.length())
	Performance.add_custom_monitor("plane/foil_force", func(): return local_foil_force.length())
	Performance.add_custom_monitor("plane/lift_force", func(): return local_lift_force.length())


func _ready() -> void:
	_register_custom_monitors()


func trigger_thrust(activate: bool) -> void:
	if activate:
		#target_thrust = PLANE_FORWARD * thrust_power
		target_thrust = basis.x * thrust_power
	else:
		target_thrust = Vector3.ZERO


func trigger_direction(dir: Vector2) -> void:
	target_torque = dir.x * turn_to_torque
	target_pitch = dir.y * move_to_pitch


func get_free_seat() -> Node3D:
	return $DrivingSeat


func _physics_process(_delta) -> void:
	# Command part
	_apply_plane_rotation()
	_apply_plane_thrust()

	# Air simulation part
	if is_sleeping():
		return

	_apply_wing_resistance()
	_apply_foil_resistance()
	_apply_lift()
	transform = transform.orthonormalized()


func _apply_plane_rotation() -> void:
	var torque: Vector3 = transform.basis * Vector3(target_pitch, 0.0, target_torque)
	apply_torque(torque)


func _apply_plane_thrust() -> void:
	var force: Vector3 = transform.basis * target_thrust
	#apply_central_force(force)
	#debug_central_force(force, $ArrowForceThrust)


func _apply_wing_resistance() -> void:
	vertical_speed = linear_velocity.dot(transform.basis * Vector3.UP)
	local_wing_force = Vector3.UP * -wing_resistance * vertical_speed
	var wing_force = transform.basis * local_wing_force
	apply_central_force(wing_force)
	#debug_central_force(wing_force, $ArrowForceWing)


func _apply_foil_resistance() -> void:
	#var foil_position = 3 * Vector3.BACK
	#var foil_position : Vector3 = get_global_position() - VerticalFoil.get_global_position()
	var foil_position: Vector3 = VerticalFoil.get_position()
	var foil_offset: Vector3 = transform.basis * foil_position
	horizontal_speed = linear_velocity.dot(transform.basis * Vector3.RIGHT)
	local_foil_force = Vector3.RIGHT * -foil_resistance * horizontal_speed
	local_foil_force = target_thrust
	apply_force(local_foil_force, foil_offset)
	debug_force(local_foil_force, foil_position, $ArrowForceFoil)


func _apply_lift() -> void:
	var forward_speed = linear_velocity.dot(transform.basis * PLANE_FORWARD)
	if forward_speed < 0.0 :
		forward_speed = 0.0
	local_lift_force = Vector3.UP * lift * forward_speed
	var lift_force = transform.basis * local_lift_force
	apply_central_force(lift_force)
	debug_central_force(lift_force, $ArrowForceLift)


func _on_dir_changed(dir: Vector2) -> void:
	trigger_direction(dir)


func _on_main_action(pressed: bool) -> void:
	trigger_thrust(pressed)


func debug_central_force(force: Vector3, arrow: Node3D) -> void:
	if force.is_zero_approx():
		arrow.hide()
		return
	else:
		arrow.show()

	arrow.look_at(force)
	arrow.set_scale(Vector3.ONE * force.length() / 1000.0)


func debug_force(force: Vector3, pos: Vector3, arrow: Node3D) -> void:
	if force.is_zero_approx():
		arrow.hide()
		return
	else:
		arrow.show()

	arrow.set_position(pos)
	arrow.look_at(force)
	arrow.set_scale(Vector3.ONE * force.length() / 1000.0)