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.15
@export var max_speed: float = 20.0
@export var max_rotation_speed: float = 15.0
@export var thrust_factor: float = 2.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
@export var thrust_point_fr: Marker3D
@export var thrust_point_fl: Marker3D
@export var thrust_point_rl: Marker3D
@export var thrust_point_rr: 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_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()
	else:
		input_suggestion_label.hide()


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
	else:
		input_suggestion_label.hide()


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):
	if armed and GameManager.game_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")
		
		if using_joy_controller:
			if throttle_input > 0:
				throttle_speed = throttle_input * 100
				state.apply_force(global_transform.basis.y * throttle_input * thrust_factor, thrust_point_fr.global_position)
				state.apply_force(global_transform.basis.y * throttle_input * thrust_factor, thrust_point_fl.global_position)
				state.apply_force(global_transform.basis.y * throttle_input * thrust_factor, thrust_point_rr.global_position)
				state.apply_force(global_transform.basis.y * throttle_input * thrust_factor, thrust_point_rl.global_position)
				
				#linear_velocity.x = clampf(linear_velocity.x, -max_speed, max_speed)
				#linear_velocity.y = clampf(linear_velocity.y, -100.0, max_speed)
				#linear_velocity.z = clampf(linear_velocity.z, -max_speed, max_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)

		# Rotation