class_name Player extends PhysicsBody3D

enum MovementType { VERTICAL, LATERAL }

#region Exports
@export var mouse_sensitivity := 0.002  # radians/pixel

@export_group("Movement")
@export var slope_limit := 45.0
@export var step_height := 0.2
@export var snap_to_ground_distance := 0.2

@export_subgroup("Crouching")
@export var crouch_height_decrease := 1.0
@export var crouch_speed := 2.0
@export var crouch_acceleration_time := 0.5
@export var crouch_deceleration_time := 0.2

@export_subgroup("Running")
@export var run_speed := 8.0
@export var run_acceleration_time := 1.5
@export var run_deceleration_time := 0.5

# TODO Change jumping to take a height, time-to-peak, and time-to-ground. Gravity can be worked out from that
@export_subgroup("Jumping")
@export var jump_speed := 4.5
@export var jump_pre_grace_time := 0.1
@export var jump_post_grace_time := 0.1

@export_group("Connector Nodes")
@export var head: Node3D
@export var body: Node3D
@export var camera: DampenedCamera3D
@export var collision_shape: CollisionShape3D
@export var state_chart: StateChart

@export_group("Advanced")
## Stop movement under this distance, but only if the movement touches at least 2 steep slopes
## The slope movement code in this class does not handle all edge cases; this is a hack to eliminate
## jitter movement
@export var steep_slope_jitter_reduce := 0.03

## The godot move_and_collide method has built in depenetration
## Higher values can eliminate jittery movement against obscure geometry, but in my experience
## this comes at the cost of making movement across flush collision planes a bit unreliable
@export var depenetration_margin := 0.001

## The distance under the player to check for ground at the start of movement
## This is in addition to the usual method of setting grounded state by collision
@export var ground_cast_distance := 0.004

## If a collision happens within this distance of the bottom of the collider
## it's considered the "bottom"
## This value is used to determine if slopes should actually make the player
## rise, or if they should be considered a wall, in the case where the slope
## is above the players feet
@export var bottom_height := 0.05

## The movement code in this class tries to adjust translation to confirm to the collision plane
## This means the same plane should never be hit more than once within 1 frame
## This sometimes happens anyway, typically when there is a small safe margin
## If it happens, the movement will be blocked and the rest of the movement iterations will be
## consumed
## This is a little hack to slightly adjust the translation to break out of this infinite loop
@export var same_surface_adjust_distance := 0.001

## How many times to move_and_collide. The algorithm early exits anyway
## The value of turning this up is to make movement in very complicated terrain more
## accurate. 4 is a decent number for low poly terrain!
@export var max_iteration_count := 4

#endregion

var target_speed := 0.0
var state_enter_speed := 0.0

var gravity := 9.8
var _velocity: Vector3 = Vector3()
var grounded_prev := false
var grounded := true
var ground_normal: Vector3
var steep_slope_normals: Array[Vector3] = []
var total_stepped_height: float = 0

# TODO Should these be class variables? Could get away with just locals
var vertical_collisions: Array[KinematicCollision3D]
var lateral_collisions: Array[KinematicCollision3D]
var snap_collisions: Array[KinematicCollision3D]

#region Godot Functions


func _ready() -> void:
	_lock_mouse()


# TODO should this have an action associated?
# TODO should it be in unhandled?
func _input(event: InputEvent) -> void:
	# Rotate body/camera
	if event is InputEventMouseMotion and Input.get_mouse_mode() == Input.MOUSE_MODE_CAPTURED:
		var motion := event as InputEventMouseMotion
		body.rotate_y(-motion.relative.x * mouse_sensitivity)
		head.rotate_x(-motion.relative.y * mouse_sensitivity)
		head.rotation.x = clamp(head.rotation.x, -1.4, 1.4)

	# Toggle mouse capture mode
	if event is InputEventKey && event.is_pressed():
		var key_event := event as InputEventKey
		if key_event.keycode == KEY_ESCAPE:
			if Input.get_mouse_mode() == Input.MOUSE_MODE_CAPTURED:
				_unlock_mouse()
			else:
				_lock_mouse()


#endregion
#region Core Movement


# Entry point to moving
func move(intended_velocity: Vector3, delta: float) -> void:
	var start_position := position

	var lateral_translation := _horz(intended_velocity * delta)
	var initial_lateral_translation := lateral_translation
	var vertical_translation := _vert(intended_velocity * delta)
	var initial_vertical_translation := vertical_translation

	grounded = false
	steep_slope_normals = []
	total_stepped_height = 0

	vertical_collisions.clear()
	lateral_collisions.clear()
	snap_collisions.clear()

	# An initial grounded check is important because ground normal is used
	# to detect seams with steep slopes; which often are collided with before the ground
	if vertical_translation.y <= 0:
		var collision := move_and_collide(
			Vector3.DOWN * ground_cast_distance, true, depenetration_margin
		)
		if collision:
			var normal := collision.get_normal(0)
			if _under_slope_limit(normal):
				grounded = true
				ground_normal = normal

	# Lateral movement
	for i in max_iteration_count:
		if lateral_translation.length() <= 0:
			break
		lateral_translation = move_iteration(
			MovementType.LATERAL,
			lateral_collisions,
			initial_lateral_translation,
			lateral_translation
		)

		# De-jitter by just ignoring lateral movement
		# (multiple steep slopes have been collided, but movement is very small)
		if (
			steep_slope_normals.size() > 1
			and _horz(position - start_position).length() < steep_slope_jitter_reduce
		):
			position = start_position

	# Vertical movement
	for i in max_iteration_count:
		if vertical_translation.length() <= 0:
			break
		vertical_translation = move_iteration(
			MovementType.VERTICAL,
			vertical_collisions,
			initial_vertical_translation,
			vertical_translation
		)

	# Don't include step height in actual velocity
	var actual_translation := position - start_position
	var actual_translation_no_step := actual_translation - Vector3.UP * total_stepped_height
	var actual_velocity := actual_translation_no_step / delta

	# HACK!
	# For some reason it's difficult to accumulate velocity when sliding down steep slopes
	# Here I just ignore the actual velocity in favour of:
	# "If intended travel was down, and actual travel was down, just keep the intended velocity"
	# This means the user is responsible for resetting vertical velocity when grounded
	if intended_velocity.y < 0 and actual_velocity.y < 0:
		_velocity = Vector3(actual_velocity.x, intended_velocity.y, actual_velocity.z)
	else:
		_velocity = actual_velocity

	# Snap Down
	# Happens last so it doesn't affect velocity
	# Keeps the character on slopes and on steps when travelling down
	if grounded:
		camera.damp()
		snap_down()
	else:
		camera.donmp()


# Moves are composed of multiple iterates
# In each iteration, move until collision, then calculate and return the next movement
func move_iteration(
	movement_type: MovementType,
	collision_array: Array,
	initial_direction: Vector3,
	translation: Vector3
) -> Vector3:
	var collisions: KinematicCollision3D

	# If Lateral movement, try stepping
	if movement_type == MovementType.LATERAL:
		var temp_position := position
		var walk_test_collision := move_and_collide(translation, true, 0)

		# To correctly step we need to cast up, then forwards, then down
		# This makes sure that the full step motion is actually clear, and ensures
		# that the step action is reversible
		var current_step_height := step_height
		var step_up_collisions := move_and_collide(Vector3.UP * step_height, false, 0)
		if step_up_collisions:
			current_step_height = step_up_collisions.get_travel().length()
		var raised_forward_collisions := move_and_collide(translation, false, 0)
		var down_collision := move_and_collide(Vector3.DOWN * current_step_height, false, 0)

		# Only step if the step algorithm landed on a walkable surface
		# AND the walk lands on a non-walkable surface
		# This stops stepping up ramps
		if (
			down_collision
			and _under_slope_limit(down_collision.get_normal(0))
			and walk_test_collision
			and !_under_slope_limit(walk_test_collision.get_normal(0))
		):  # Keep track of stepepd distance to cancel it out later
			total_stepped_height += position.y - temp_position.y
			collisions = raised_forward_collisions
			camera.damp()
		else:  # Reset and move normally
			position = temp_position
			collisions = move_and_collide(translation, false, depenetration_margin)

	# If Vertical movement, just move; no need to step
	else:
		collisions = move_and_collide(translation, false, depenetration_margin)

	# Moved all remaining distance
	if !collisions:
		return Vector3.ZERO

	collision_array.append(collisions)

	# If any ground collisions happen during movement, the character is grounded
	# Imporant to keep this up-to-date rather than just rely on the initial grounded state
	var normal := collisions.get_normal(0)
	if _under_slope_limit(normal):
		grounded = true
		ground_normal = normal

	# This algorithm for determining where to move on a collisions uses "projection plane"
	# Whatever surface the character hits, we generate a blocking "plane" that we will slide along
	#
	# We calculate the normal of the plane we want to use, projection_normal, then
	# transform into a plane at the end
	#
	# By default, projection normal is just the normal of the surface
	# This may be unnecessary after we account for all edge cases
	#
	# We also want to "block" movement in some directions based on a surface angle
	# For Vertical, blocking angle is between 0 - slopeLimit
	# For Lateral, blocking angle is slopeLimit - 360 (grounded) or 90 (not grounded)
	# The latter allows players to slide down ceilings while in the air
	# TODO These can be precalculated on slope limit change
	var projection_normal := normal
	var surface_angle := normal.angle_to(Vector3.UP)
	match movement_type:
		MovementType.LATERAL:
			var min_block_angle := deg_to_rad(slope_limit)
			var max_block_angle := 2 * PI if grounded else PI / 2

			# If collision happens on the "side" of the cylinder, treat it as a vertical
			# wall in all cases (we use the tangent of the cylinder)
			var cylinder := collision_shape.shape as CylinderShape3D
			var collision_point := collisions.get_position(0)
			var side_y := (collision_shape.global_position.y - cylinder.height / 2) + bottom_height
			if collision_point.y > side_y:
				projection_normal = collision_shape.global_position - collision_point
				projection_normal.y = 0
				projection_normal = projection_normal.normalized()
			elif surface_angle >= min_block_angle and surface_angle <= max_block_angle:
				# "Wall off" the slope
				projection_normal = _horz(normal).normalized()

				# Or, "Wall off" the slope by figuring out the seam with the ground
				if grounded and surface_angle < PI / 2:
					if !already_touched_slope_close_match(normal):
						steep_slope_normals.append(normal)

					var seam := normal.cross(ground_normal)
					var temp_projection_plane := Plane(Vector3.ZERO, seam, seam + Vector3.UP)
					projection_normal = temp_projection_plane.normal
			# Otherwise force the direction to align with input direction
			# (projecting translation over the normal of a slope does not align with input direction)
			elif surface_angle < (PI / 2):
				projection_normal = relative_slope_normal(normal, translation)
		MovementType.VERTICAL:
			# If vertical is blocked, you're on solid ground - just stop moving
			var min_block_angle := 0
			var max_block_angle := deg_to_rad(slope_limit)
			if surface_angle >= min_block_angle and surface_angle <= max_block_angle:
				return Vector3.ZERO

	# Don't let one move call ping pong around
	var projection_plane := Plane(projection_normal)
	var continued_translation := projection_plane.project(collisions.get_remainder())
	var initial_influenced_translation := projection_plane.project(initial_direction)
	var next_translation: Vector3
	if initial_influenced_translation.dot(continued_translation) >= 0:
		next_translation = continued_translation
	else:
		next_translation = (
			initial_influenced_translation.normalized() * continued_translation.length()
		)

	# See same_surface_adjust_distance
	if next_translation.normalized() == translation.normalized():
		next_translation += normal * same_surface_adjust_distance

	return next_translation


# TODO Should this use same_surface_adjust_distance
func already_touched_slope_close_match(normal: Vector3) -> bool:
	return steep_slope_normals.any(
		func(n: Vector3) -> bool: return n.distance_squared_to(normal) < 0.001
	)


# I wrote this a while ago in Unity
# I ported it here but I only have a vague grasp of how it works
func relative_slope_normal(slope_normal: Vector3, lateral_desired_direction: Vector3) -> Vector3:
	var surface_angle := slope_normal.angle_to(Vector3.UP)
	var inverse_surface_angle := PI / 2 - surface_angle
	if inverse_surface_angle <= 0:
		push_error("Trying to calculate relative slope normal for a ceiling")

	# Geometry!
	# This is the component of the desired travel that points straight into the slope
	var angle_to_straight := _horz(slope_normal).angle_to(-lateral_desired_direction)
	var straight_length := cos(angle_to_straight) * lateral_desired_direction.length()

	# Which helps us calculate the height on the slope at the end of the desired travel
	var height := straight_length / tan(inverse_surface_angle)

	# Which gives us the actual desired movement
	var vector_up_slope := Vector3(lateral_desired_direction.x, height, lateral_desired_direction.z)

	# Due to the way the movement algorithm works we need to figure out the normal that defines
	# the plane that will give this result
	var rotation_axis := vector_up_slope.cross(Vector3.UP).normalized()
	var emulated_normal := vector_up_slope.rotated(rotation_axis, PI / 2)
	return emulated_normal.normalized()


# TODO this doesn't always work. Why?
func snap_down() -> void:
	# We allow snap to slide down slopes
	# It really helps reduce jitter on steep slopes
	var before_snap_pos := position
	var ground_snap_translation := Vector3.DOWN * snap_to_ground_distance
	for i in max_iteration_count:
		if ground_snap_translation.length() <= 0:
			break
		ground_snap_translation = move_iteration(
			MovementType.VERTICAL, snap_collisions, Vector3.DOWN, ground_snap_translation
		)

	# Decide whether to keep the snap or not
	if snap_collisions.is_empty():
		var ground_collision := move_and_collide(
			Vector3.DOWN * ground_cast_distance, true, depenetration_margin
		)
		if ground_collision && _under_slope_limit(ground_collision.get_normal(0)):
			# There was no snap collisions, but there is ground underneath
			# This can be due to an edge case where the snap movement falls through the ground
			# Why does this check not fall through the ground? I don't know
			# In any case, manually set the y
			position.y = ground_collision.get_position(0).y
		else:
			# No snap collisions and no floor, reset
			position = before_snap_pos
	elif !_under_slope_limit(snap_collisions[snap_collisions.size() - 1].get_normal(0)):
		# Collided with steep ground, reset
		position = before_snap_pos


#endregion
#region Utilities


func _under_slope_limit(normal: Vector3) -> bool:
	return normal.angle_to(Vector3.UP) < deg_to_rad(slope_limit)


func _horz(value: Vector3) -> Vector3:
	return Vector3(value.x, 0, value.z)


func _vert(value: Vector3) -> Vector3:
	return Vector3(0, value.y, 0)


func _lock_mouse() -> void:
	Input.mouse_mode = Input.MOUSE_MODE_CAPTURED


func _unlock_mouse() -> void:
	Input.mouse_mode = Input.MOUSE_MODE_VISIBLE


func _get_move_dir() -> Vector2:
	# We don't want to be moving if the mouse isn't captured!
	if Input.get_mouse_mode() != Input.MOUSE_MODE_CAPTURED:
		return Vector2.ZERO

	var input_dir := Vector2.ZERO
	if Input.is_action_pressed("cc_forward"):
		input_dir += Vector2.UP
	if Input.is_action_pressed("cc_backward"):
		input_dir -= Vector2.UP
	if Input.is_action_pressed("cc_left"):
		input_dir += Vector2.LEFT
	if Input.is_action_pressed("cc_right"):
		input_dir -= Vector2.LEFT
	input_dir = input_dir.normalized()

	# Local rotation is fine given the parent isn't rotating ever
	return input_dir.rotated(-body.rotation.y)


#endregion
#region States


func _on_root_state_processing(_delta: float) -> void:
	# !HACK - Printing this here for tracking
	print(target_speed)

	if _get_move_dir() == Vector2.ZERO:
		state_chart.send_event("Idle")
	else:
		state_chart.send_event("Move")
	if Input.is_action_just_pressed("cc_sprint"):
		state_chart.send_event("Crouch")
	if Input.is_action_just_pressed("cc_jump"):
		state_chart.send_event("Jump")

	if grounded != grounded_prev:
		var event := "Grounded" if grounded else "Airborne"
		state_chart.send_event(event)
	grounded_prev = grounded


# TODO Rename sprint to crouch
func _on_idle_state_processing(_delta: float) -> void:
	# !HACK - No deceleration. Just reset the target speed for now
	target_speed = 0.0


func _on_running_state_physics_processing(delta: float) -> void:
	if state_enter_speed < run_speed:
		target_speed += (run_speed / run_acceleration_time) * delta
		target_speed = minf(target_speed, run_speed)
	else:
		var decel_rate := (state_enter_speed - run_speed) / run_deceleration_time
		target_speed -= decel_rate * delta
		target_speed = maxf(target_speed, run_speed)

	var target_velocity_h := _get_move_dir() * target_speed
	var target_velocity_v := -gravity * delta
	var target_velocity := Vector3(target_velocity_h.x, target_velocity_v, target_velocity_h.y)
	move(target_velocity, delta)


func _on_falling_state_physics_processing(delta: float) -> void:
	var move_speed := run_speed
	var target_velocity_h := _get_move_dir() * move_speed
	var target_velocity_v := _velocity.y - gravity * delta
	var target_velocity := Vector3(target_velocity_h.x, target_velocity_v, target_velocity_h.y)
	move(target_velocity, delta)


func _on_jumping_state_physics_processing(delta: float) -> void:
	# !HACK - We're setting this to test run deceleration
	target_speed = run_speed + 2.0
	var target_velocity_h := _get_move_dir() * target_speed
	var target_velocity_v := jump_speed
	var target_velocity := Vector3(target_velocity_h.x, target_velocity_v, target_velocity_h.y)
	move(target_velocity, delta)


func _on_root_child_state_entered() -> void:
	state_enter_speed = target_speed

#endregion