extends RigidBody3D

@export var joy_device_id: int = -1

@export var pitch_axis: int = 1
@export var pitch_invert: bool = true
@export var pitch_device_id: int = -1
@export var roll_axis: int = 0
@export var roll_invert: bool = false
@export var roll_device_id: int = -1
@export var yaw_axis: int = 2
@export var yaw_invert: bool = false
@export var yaw_device_id: int = -1
@export var throttle_axis: int = 3
@export var throttle_invert: bool = true
@export var throttle_signed: bool = true
@export var throttle_device_id: int = -1
@export var strafe_axis: int = 4
@export var strafe_invert: bool = false
@export var strafe_device_id: int = -1
@export var lift_axis: int = 5
@export var lift_invert: bool = false
@export var lift_device_id: int = -1

@export var stick_deadzone: float = 0.08
@export var throttle_deadzone: float = 0.02

@export var max_thrust: float = 7500.0
@export var strafe_thrust: float = 3500.0
@export var lift_thrust: float = 3500.0
@export var torque_strength: float = 2000.0

@export var throttle_response: float = 3.0
@export var stick_response: float = 6.0
@export var stick_expo: float = 1.6
@export var max_speed: float = 180.0
@export var max_angular_speed: float = 2.6

@export var vector_line_path: NodePath = NodePath("DisplacementVector")
@export var vector_scale: float = 0.05
@export var max_vector_length: float = 20.0

var _device_id: int = -1
var _current_throttle: float = 0.0
var _vector_line: Node
var _axis_state := {
	"roll": 0.0,
	"pitch": 0.0,
	"yaw": 0.0,
	"throttle": 0.0,
	"strafe": 0.0,
	"lift": 0.0,
}
var _axis_smoothed := {
	"roll": 0.0,
	"pitch": 0.0,
	"yaw": 0.0,
	"strafe": 0.0,
	"lift": 0.0,
}

func _ready() -> void:
	_vector_line = get_node_or_null(vector_line_path)
	_device_id = _resolve_joypad()
	if _device_id == -1:
		push_warning("No HOTAS detected. Connect the X56 or set joy_device_id.")

func _physics_process(delta: float) -> void:
	_device_id = _resolve_joypad()
	if _device_id == -1:
		_update_vector_line()
		return

	var roll = _shape_axis(_read_axis(_resolve_axis_device(roll_device_id), roll_axis, stick_deadzone, roll_invert))
	var pitch = _shape_axis(_read_axis(_resolve_axis_device(pitch_device_id), pitch_axis, stick_deadzone, pitch_invert))
	var yaw = _shape_axis(_read_axis(_resolve_axis_device(yaw_device_id), yaw_axis, stick_deadzone, yaw_invert))
	var strafe = _shape_axis(_read_axis(_resolve_axis_device(strafe_device_id), strafe_axis, stick_deadzone, strafe_invert))
	var lift = _shape_axis(_read_axis(_resolve_axis_device(lift_device_id), lift_axis, stick_deadzone, lift_invert))
	var raw_throttle = _read_axis(_resolve_axis_device(throttle_device_id), throttle_axis, throttle_deadzone, throttle_invert)

	var target_throttle: float
	if throttle_signed:
		target_throttle = clamp(raw_throttle, -1.0, 1.0)
	else:
		target_throttle = clamp((raw_throttle + 1.0) * 0.5, 0.0, 1.0)

	_current_throttle = lerp(
		_current_throttle,
		target_throttle,
		clamp(throttle_response * delta, 0.0, 1.0)
	)

	roll = _smooth_axis("roll", roll, delta)
	pitch = _smooth_axis("pitch", pitch, delta)
	yaw = _smooth_axis("yaw", yaw, delta)
	strafe = _smooth_axis("strafe", strafe, delta)
	lift = _smooth_axis("lift", lift, delta)

	_axis_state["roll"] = roll
	_axis_state["pitch"] = pitch
	_axis_state["yaw"] = yaw
	_axis_state["throttle"] = _current_throttle
	_axis_state["strafe"] = strafe
	_axis_state["lift"] = lift

	var basis = global_transform.basis
	var forward = -basis.z
	var right = basis.x
	var up = basis.y

	var thrust_force = forward * (_current_throttle * max_thrust)
	var strafe_force = right * (strafe * strafe_thrust)
	var lift_force = up * (lift * lift_thrust)
	apply_central_force(thrust_force + strafe_force + lift_force)

	var local_angular = basis.inverse() * angular_velocity
	var desired_angular = Vector3(pitch, yaw, -roll) * max_angular_speed
	var torque_local = (desired_angular - local_angular) * torque_strength
	apply_torque(basis * torque_local)

	if max_speed > 0.0 and linear_velocity.length() > max_speed:
		linear_velocity = linear_velocity.limit_length(max_speed)
	if max_angular_speed > 0.0 and angular_velocity.length() > max_angular_speed:
		angular_velocity = angular_velocity.limit_length(max_angular_speed)

	_update_vector_line()

func get_axis_debug_text() -> String:
	if _device_id == -1:
		return "No HOTAS detected"
	return "Roll: %.2f (d%d)  Pitch: %.2f (d%d)  Yaw: %.2f (d%d)\nThrottle: %.2f (d%d)  Strafe: %.2f (d%d)  Lift: %.2f (d%d)" % [
		_axis_state["roll"],
		_resolve_axis_device(roll_device_id),
		_axis_state["pitch"],
		_resolve_axis_device(pitch_device_id),
		_axis_state["yaw"],
		_resolve_axis_device(yaw_device_id),
		_axis_state["throttle"],
		_resolve_axis_device(throttle_device_id),
		_axis_state["strafe"],
		_resolve_axis_device(strafe_device_id),
		_axis_state["lift"],
		_resolve_axis_device(lift_device_id),
	]

func get_joypad_id() -> int:
	return _device_id

func _resolve_joypad() -> int:
	if joy_device_id >= 0:
		return joy_device_id
	var pads = Input.get_connected_joypads()
	if pads.is_empty():
		return -1
	return pads[0]

func _resolve_axis_device(axis_device_id: int) -> int:
	if axis_device_id >= 0:
		return axis_device_id
	return _device_id

func _read_axis(device_id: int, axis: int, deadzone: float, invert: bool) -> float:
	if axis < 0 or device_id < 0:
		return 0.0
	var value = Input.get_joy_axis(device_id, axis)
	if invert:
		value = -value
	var magnitude = abs(value)
	if magnitude <= deadzone:
		return 0.0
	var scaled = (magnitude - deadzone) / (1.0 - deadzone)
	return scaled * sign(value)

func _shape_axis(value: float) -> float:
	if stick_expo <= 1.0:
		return value
	return sign(value) * pow(abs(value), stick_expo)

func _smooth_axis(name: String, value: float, delta: float) -> float:
	if stick_response <= 0.0:
		_axis_smoothed[name] = value
		return value
	var t = clamp(stick_response * delta, 0.0, 1.0)
	_axis_smoothed[name] = lerp(_axis_smoothed[name], value, t)
	return _axis_smoothed[name]

func _update_vector_line() -> void:
	if _vector_line == null:
		return
	if not _vector_line.has_method("set_vector"):
		return
	var local_velocity = global_transform.basis.inverse() * linear_velocity
	var clamped = local_velocity.limit_length(max_vector_length)
	_vector_line.set_vector(clamped * vector_scale)