extends RigidBody3D

# Local coords to axis
# Yaw = Y
# Roll = Z
# Pitch = X

@export_category("Parameters")
@export var flight_modes: Array = ["normal", "acro"]
@export var armed: bool = false
@export var can_flip: bool = false
@export var throttle_speed: float = 0.0
@export var rotation_speed: float = 0.5
@export var max_speed: float = 100.0
@export var max_rotation_speed: float = 15.0
@export var camera_tilt_angle: float = 0.0
@export_category("Node References")
@export var camera: Camera3D
@export var anim_player: AnimationPlayer
@export var flip_over_detection_ray: RayCast3D
@export var flip_over_timer: Timer
@export var input_suggestion_label: Label
@export var reset_point: Marker3D

var animation_initalized: bool = false
var using_joy_controller: bool = true


func _ready():
	flip_over_timer.timeout.connect(_on_flip_over_timeout)


func _process(_delta):
	if Input.is_action_just_pressed("arm"):
		handle_arming()


func handle_throttle(throttle_input: float):
	if using_joy_controller:
		if throttle_input > 0:
			throttle_speed = throttle_input * 100
			apply_central_force(global_transform.basis.y * throttle_speed)

		# Speed scale adjusted for animation
		anim_player.speed_scale = throttle_input * 4
		anim_player.speed_scale = clampf(anim_player.speed_scale, 1.0, 4.0)

	linear_velocity.x = clampf(linear_velocity.x, -max_speed, max_speed)
	linear_velocity.y = clampf(linear_velocity.y, -max_speed, max_speed)
	linear_velocity.z = clampf(linear_velocity.z, -max_speed, max_speed)



func handle_arming():
	if !armed:
		if !animation_initalized:
			anim_player.play("spin_props")
			animation_initalized = true
		elif animation_initalized:
			anim_player.play()
		armed = !armed
		print("Drone armed")
	elif armed:
		anim_player.pause()
		armed = !armed
		print("Drone disarmed")


func _start_flip_over_timer():
	if flip_over_timer.time_left > 0:
		pass
	elif !can_flip:
		flip_over_timer.start()


func _on_flip_over_timeout():
	if flip_over_detection_ray.is_colliding() and flip_over_detection_ray.get_collider().is_in_group("Ground"):
		can_flip = true
		input_suggestion_label.text = "press A to flip drone"
		input_suggestion_label.show()


func _handle_drone_flipping():
	if flip_over_detection_ray.is_colliding() and flip_over_detection_ray.get_collider().is_in_group("Ground"):
		global_rotation = Vector3.ZERO
		input_suggestion_label.hide()
		can_flip = false


func _physics_process(_delta): # Still needed for other physics
	if flip_over_detection_ray.is_colliding() and flip_over_detection_ray.get_collider().is_in_group("Ground"):
		_start_flip_over_timer()

	if Input.is_action_just_pressed("accept") and can_flip:
		_handle_drone_flipping()


func _input(event):
	if Input.is_action_just_pressed("reset_drone"):
		global_position = reset_point.global_position
		global_rotation = Vector3.ZERO
		linear_velocity = Vector3.ZERO
		angular_velocity = Vector3.ZERO


func _integrate_forces(state):  # Use _integrate_forces for RigidBody3D control
	if armed and GameManager.game_started:  # Only apply controls when armed and when the game has started
		var yaw_input = Input.get_axis("yaw_right", "yaw_left")
		var roll_input = Input.get_axis("roll_right", "roll_left")
		var pitch_input = Input.get_axis("pitch_backward", "pitch_forward")
		var throttle_input = Input.get_axis("throttle_down", "throttle_up")

		handle_throttle(throttle_input) # Throttle handled here

		if yaw_input != 0:
			rotate_object_local(Vector3.UP, yaw_input * rotation_speed)  # Y axis rotation
		if roll_input != 0:
			rotate_object_local(Vector3.FORWARD, roll_input * rotation_speed)  # Z axis rotation
		if pitch_input != 0:
			rotate_object_local(Vector3.RIGHT, pitch_input * rotation_speed)  # X axis rotation

		# Damping
		if yaw_input == 0:
			state.angular_velocity.y = lerp(state.angular_velocity.y, 0.0, 0.05)
		if roll_input == 0:
			state.angular_velocity.z = lerp(state.angular_velocity.z, 0.0, 0.05)
		if pitch_input == 0:
			state.angular_velocity.x = lerp(state.angular_velocity.x, 0.0, 0.05)

		# Clamp the angular velocity
		state.angular_velocity.y = clampf(state.angular_velocity.y, -max_rotation_speed, max_rotation_speed)
		state.angular_velocity.z = clampf(state.angular_velocity.z, -max_rotation_speed, max_rotation_speed)
		state.angular_velocity.x = clampf(state.angular_velocity.x, -max_rotation_speed, max_rotation_speed)