class_name SubmarineActor
extends CharacterBody3D

signal collision_sampled(sample: VehicleCollisionSample)

const VehicleCommand = preload("res://game/application/vehicle/vehicle_command.gd")
const VehicleCollisionSample = preload(
	"res://game/application/vehicle/vehicle_collision_sample.gd"
)

@export var cruise_speed_mps := 6.0
@export var high_thrust_speed_mps := 9.5
@export var acceleration_mps2 := 2.0
@export var braking_mps2 := 1.35
@export var vertical_speed_mps := 3.5
@export var vertical_acceleration_mps2 := 1.8
@export var lateral_grip_mps2 := 2.2
@export var maximum_yaw_speed_degrees := 45.0
@export var yaw_acceleration_degrees := 30.0
@export var yaw_braking_degrees := 24.0
@export var maximum_depth_m := 500.0

var _command: VehicleCommand = VehicleCommand.neutral(0)
var _yaw_velocity_rps := 0.0
var _collision_sequence := 0
var _last_longitudinal_effort := 0.0
var _last_steering_effort := 0.0
var _last_vertical_effort := 0.0
var _last_high_thrust_active := false


func apply_vehicle_command(command: VehicleCommand) -> void:
	_command = command


func current_depth_m() -> float:
	return maxf(-global_position.y, 0.0)


func current_yaw_velocity_rps() -> float:
	return _yaw_velocity_rps


func actual_longitudinal_effort() -> float:
	return _last_longitudinal_effort


func actual_steering_effort() -> float:
	return _last_steering_effort


func actual_vertical_effort() -> float:
	return _last_vertical_effort


func high_thrust_active() -> bool:
	return _last_high_thrust_active


func _physics_process(delta: float) -> void:
	_update_yaw(delta)
	_update_velocity(delta)
	var velocity_before_move := velocity
	move_and_slide()
	_keep_inside_vertical_bounds()
	_publish_collisions(velocity_before_move)


func _update_yaw(delta: float) -> void:
	var maximum_yaw_speed := deg_to_rad(maximum_yaw_speed_degrees)
	var target_yaw_velocity := -_command.steering * maximum_yaw_speed
	var yaw_change_degrees := (
		yaw_acceleration_degrees
		if not is_zero_approx(_command.steering)
		else yaw_braking_degrees
	)
	_yaw_velocity_rps = move_toward(
		_yaw_velocity_rps,
		target_yaw_velocity,
		deg_to_rad(yaw_change_degrees) * delta
	)
	rotation.y = wrapf(rotation.y + _yaw_velocity_rps * delta, -PI, PI)
	_last_steering_effort = _command.steering


func _update_velocity(delta: float) -> void:
	var local_velocity := global_basis.inverse() * velocity
	var requested_speed := (
		high_thrust_speed_mps if _command.high_thrust_active else cruise_speed_mps
	)
	var target_forward_speed := _command.longitudinal * requested_speed
	var current_forward_speed := -local_velocity.z
	var forward_change_rate := (
		acceleration_mps2
		if not is_zero_approx(target_forward_speed)
		else braking_mps2
	)
	current_forward_speed = move_toward(
		current_forward_speed,
		target_forward_speed,
		forward_change_rate * delta
	)
	local_velocity.z = -current_forward_speed

	var target_vertical_speed := _command.vertical * vertical_speed_mps
	local_velocity.y = move_toward(
		local_velocity.y,
		target_vertical_speed,
		vertical_acceleration_mps2 * delta
	)
	local_velocity.x = move_toward(local_velocity.x, 0.0, lateral_grip_mps2 * delta)
	velocity = global_basis * local_velocity
	_last_longitudinal_effort = _command.longitudinal
	_last_vertical_effort = _command.vertical
	_last_high_thrust_active = _command.high_thrust_active


func _keep_inside_vertical_bounds() -> void:
	if global_position.y > -2.0:
		global_position.y = -2.0
		velocity.y = minf(velocity.y, 0.0)
	if global_position.y < -maximum_depth_m:
		global_position.y = -maximum_depth_m
		velocity.y = maxf(velocity.y, 0.0)


func _publish_collisions(previous_velocity: Vector3) -> void:
	for collision_index in get_slide_collision_count():
		var collision := get_slide_collision(collision_index)
		var impact_speed := absf(previous_velocity.dot(collision.get_normal()))
		if impact_speed <= 0.01:
			continue
		var sample_id := StringName(
			"collision-%d-%d" % [_collision_sequence, collision_index]
		)
		collision_sampled.emit(
			VehicleCollisionSample.new(sample_id, _collision_sequence, impact_speed)
		)
	_collision_sequence += 1