Godot 3D Jetpack + First Person Movement + Head bobbing

extends CharacterBody3D

enum WalkState{
    NORMAL,
    SPRINT,
    CROUCH,
    PRONE,
    SLIDE,
    JETPACK
}

#movement attribute values
const TARGET_LERP = .8
const JETPACK_TARGER_LERP = .6

const SPRINT_SPEED = 10.0
const SPRINT_LERP_ACC = 2.0
const SPRINT_LERP_DEC = 8.0

const WALK_SPEED = 6.0
const WALK_LERP_ACC = 3.5
const WALK_LERP_DEC = 10.0

const CROUCH_SPEED = 3.0
const CROUCH_LERP_ACC = 8.0
const CROUCH_LERP_DEC = 14.0

const PRONE_SPEED = 1.5
const PRONE_LERP_ACC = 12.0
const PRONE_LERP_DEC = 22.0

const JETPACK_SPEED = 6.0
const JETPACK_LERP_ACC = 6.0
const JETPACK_LERP_DEC = 12.0
const JETPACK_VERTICAL_FORCE = 5.0
const JETPACK_VERTICAL_VELOCITY_MAX = 2.5
const JETPACK_VERTICAL_DIFFERENCE_MAX = 5.0
var current_jetpack_vertical = 0.0
const JETPACK_VELOCITY_THRESHOLD = .5
const ENERGY_MAX = 100.0
const ENERGY_DRAIN_JETPACK_START = 10
const ENERGY_DRAIN_JETPACK_MULT = 10.0
const ENERGY_DRAIN_SPRINT_MULT = 5.0
const ENERGY_REGEN_MULT = 30.0
const ENERGY_REGEN_COOLDOWN = 2.5
var current_energy_regen_cooldown = 0.0
var current_energy = ENERGY_MAX
const JETPACK_START_TIMER = 0.65
var current_jetpack_start_timer = 0

const SLIDE_SPEED = 30
const SLIDE_TIME_MAX = .7
const SLIDE_DAMPEN_RATE = .05
const SLIDE_FLAT_DAMPEN_RATE = .001
const SLOPE_SLIDE_THRESHOLD = .1
var current_slide_time = 0
var current_slide_vector : Vector3 = Vector3.ZERO

var SPRINT_CD_MAX = .2
var current_sprint_cd = 0

const FALL_SPEED_MAX = 15
const JUMP_VELOCITY = 6
const JUMP_HANG_TIME_THRESHOLD = .3
const JUMP_HANG_TIME_SPEED_MULT = 1.05
const JUMP_HANG_TIME_ACC_MULT = 3

const COYOTE_TIME_MAX = .1
var current_coyote_time = 0
const JUMP_CD_MAX = .25
var current_jump_cd = 0



# Get the gravity from the project settings to be synced with RigidBody nodes.
var gravity_default = ProjectSettings.get_setting("physics/3d/default_gravity")
var gravity_falling = 2 * gravity_default
var gravity_hang_time = .5 * gravity_default
var current_gravity = gravity_default

@export var camera_sensitivity = .25

var input_dir = Vector2.ZERO
var direction = Vector3.ZERO

@onready var collider = $Collider
const COLLIDER_HEIGHT_NORMAL = 2
const COLLIDER_HEIGHT_CROUCH = 1.1
const COLLIDER_HEIGHT_PRONE = .5

const CAMERA_HEIGHT_NORMAL = 1.8
const CAMERA_HEIGHT_CROUCH = .9
const CAMERA_HEIGHT_PRONE = .3

const HEAD_BOB_INTENSITY_SPRINT = .15
const HEAD_BOB_INTENSITY_NORMAL = .12
const HEAD_BOB_INTENSITY_CROUCH = .08
const HEAD_BOB_INTENSITY_PRONE = .04

const HEAD_BOB_FREQUENCY_SPRINT = 18
const HEAD_BOB_FREQUENCY_NORMAL = 14
const HEAD_BOB_FREQUENCY_CROUCH = 10
const HEAD_BOB_FREQUENCY_PRONE = 8

const HEAD_BOB_LERP_RATE = 50

var head_bob_var = 0
var is_head_bob_active = true
var current_head_bob_intensity = 0
var current_head_bob_frequency = 0

const CAMERA_LERP = 10
const CAMERA_FOV_NORMAL = 70.0
const CAMERA_FOV_MAX_SPEED = 100.0

@onready var camera_pivot = $CameraPivot
@onready var debug_label = $DebugLabel
@onready var energy_bar = $EnergyBar
@onready var energy_bar_bar = $EnergyBar/Bar
@onready var energy_bar_label = $EnergyBar/Bar/Label
@onready var height_raycast = $HeightRaycast
@onready var camera_3d = $CameraPivot/HeadBobPivot/Camera3D
@onready var head_bob_pivot = $CameraPivot/HeadBobPivot
@onready var jetpack_stream = $jetpackStream

var current_max_speed : float = 0
var current_lerp_acc : float = 0
var current_lerp_dec : float = 0
var current_camera_height : float = 0


const JETPACK_VOLUME_LERP_MULT = 5
const JETPACK_STREAM_STOP_THRESHOLD = .01

var current_walk_state : WalkState = WalkState.NORMAL


var floor_angle

var debug = null
var debug1 = null
var debug2 = null
var debug3 = null

func _ready():
    Input.set_mouse_mode(Input.MOUSE_MODE_CAPTURED)
    _UpdateCollider()
    debug =  get_tree().root.get_node("ROOT/raycasts")
    debug1 = debug.get_node("DebugRaycast")
    debug2 = debug.get_node("DebugRaycast2")
    debug3 = debug.get_node("DebugRaycast3")
    floor_angle = 0

    energy_bar_bar.min_value = 0
    energy_bar_bar.max_value = ENERGY_MAX
    _UpdateEnergyLabel()

func _input(event):
    if event is InputEventMouseMotion:
        rotate_y(deg_to_rad(camera_sensitivity * -event.relative.x))
        camera_pivot.rotate_x(deg_to_rad(camera_sensitivity * -event.relative.y))
        camera_pivot.rotation.x = clamp(camera_pivot.rotation.x, deg_to_rad(-89), deg_to_rad(89))


func _process(delta):
    input_dir = Input.get_vector("left", "right", "forward", "back")

    direction = transform.basis * Vector3(input_dir.x, 0, input_dir.y).normalized()

    if WalkState.SLIDE != current_walk_state && WalkState.JETPACK != current_walk_state:
        if Input.is_action_pressed("prone") && is_on_floor():
            if current_walk_state != WalkState.PRONE:
                current_walk_state = WalkState.PRONE
                _UpdateCollider()
        elif Input.is_action_pressed("crouch") and !height_raycast.is_colliding() && is_on_floor():
            if current_walk_state != WalkState.CROUCH:
                if current_sprint_cd > 0:
                    current_walk_state = WalkState.SLIDE
                    current_slide_time = SLIDE_TIME_MAX
                    current_slide_vector = abs(velocity) * direction
                    current_slide_vector.y = 0
                    _UpdateCollider()
                else:
                    current_walk_state = WalkState.CROUCH
                    _UpdateCollider()
        elif !height_raycast.is_colliding():
            if current_walk_state == WalkState.PRONE:
                current_walk_state = WalkState.CROUCH
                _UpdateCollider()
            elif Input.is_action_pressed("sprint"):
                current_sprint_cd = SPRINT_CD_MAX
                if current_walk_state != WalkState.SPRINT:
                    current_walk_state = WalkState.SPRINT
                    _UpdateCollider()
            elif current_walk_state != WalkState.NORMAL:
                current_walk_state = WalkState.NORMAL
                _UpdateCollider()

    floor_angle = get_floor_angle()

    _UpdateEnergyLabel()


func _physics_process(delta):
    if WalkState.SLIDE == current_walk_state:
        if current_slide_time > 0:
            if floor_angle < SLOPE_SLIDE_THRESHOLD || velocity.y > 0:
                current_slide_time -= delta
                current_slide_time = clamp(current_slide_time, 0, SLIDE_TIME_MAX)

        else:
            current_walk_state = WalkState.CROUCH
            _UpdateCollider()

    if current_sprint_cd > 0:
        current_sprint_cd -= delta

    #calc target speed based on current input
    var target_speed : Vector3 = direction * current_max_speed

    var current_acc_rate : Vector3 = Vector3.ZERO
    if input_dir:
        current_acc_rate = Vector3(current_lerp_acc,
                0,
                current_lerp_acc)
    else:
        current_acc_rate = Vector3(current_lerp_dec,
            0,
            current_lerp_dec)


    #lerp the target speed for smoother change
    #if the movement is in the same direction make the target closer to the current velocity
    #otherwise, since direction shift is required make the target closer to actual target
    #if player is faster than top speed don't slow down on X and Z axis
    if (target_speed.x != 0 &&
        abs(velocity.x) >= abs(target_speed.x) &&
        sign(velocity.x) == sign(target_speed.x)):

        target_speed.x = lerp(velocity.x, target_speed.x, 1-TARGET_LERP)
    else:
        target_speed.x = lerp(velocity.x, target_speed.x, TARGET_LERP)

    if (target_speed.z != 0 &&
        abs(velocity.z) >= abs(target_speed.z) &&
        sign(velocity.z) == sign(target_speed.z)):

        target_speed.z = lerp(velocity.z, target_speed.z, 1-TARGET_LERP)
    else:
        target_speed.z = lerp(velocity.z, target_speed.z, TARGET_LERP)

    #Handle jetpack
    if Input.is_action_pressed("jump"):
        current_jetpack_start_timer += delta
        current_jetpack_start_timer = min(current_jetpack_start_timer, JETPACK_START_TIMER)
    elif Input.is_action_just_released("jump"):
        current_jetpack_start_timer = 0

    if current_walk_state != WalkState.JETPACK:
        if current_energy_regen_cooldown >= ENERGY_REGEN_COOLDOWN:
            current_energy += delta * ENERGY_REGEN_MULT
            current_energy = min(current_energy, ENERGY_MAX)
        else:
            current_energy_regen_cooldown += delta

        if current_energy > ENERGY_DRAIN_JETPACK_START && (
            current_jetpack_start_timer >= JETPACK_START_TIMER ||
            (Input.is_action_just_pressed("jump") &&
            abs(velocity.y) > JETPACK_VELOCITY_THRESHOLD)):
            current_walk_state = WalkState.JETPACK
            _UpdateCollider()
            jetpack_stream.volume_db = linear_to_db(0.00)
            jetpack_stream.play()
            current_energy -= ENERGY_DRAIN_JETPACK_START

    if current_walk_state != WalkState.JETPACK:
        if jetpack_stream.playing:
            jetpack_stream.volume_db = linear_to_db(lerp(
                db_to_linear(jetpack_stream.volume_db),
                0.0,
                JETPACK_VOLUME_LERP_MULT * delta
            ))
            if linear_to_db(JETPACK_STREAM_STOP_THRESHOLD) >= jetpack_stream.volume_db:
                jetpack_stream.volume_db = linear_to_db(0)
                jetpack_stream.stop()

        # Handle Jump.
        if Input.is_action_just_released("jump") and velocity.y > 0:
            current_jump_cd = 0
            velocity.y = velocity.y / 2.0
#           current_gravity = gravity_falling
        elif current_coyote_time > 0 && (
            Input.is_action_just_pressed("jump") || current_jump_cd > 0):
            current_jump_cd = JUMP_CD_MAX
            current_coyote_time = 0
            velocity.y = JUMP_VELOCITY
            if Input.is_action_pressed("sprint"): current_walk_state = WalkState.SPRINT
            else: current_walk_state = WalkState.NORMAL
            _UpdateCollider()

        if abs(velocity.y) < JUMP_HANG_TIME_THRESHOLD && !is_on_floor():
            #make the gravity weaker around apex of jump
            current_gravity = gravity_hang_time

            #increase responsiveness
            target_speed *= JUMP_HANG_TIME_SPEED_MULT
            current_acc_rate *= JUMP_HANG_TIME_ACC_MULT
        else:
            #if falling make gravity stronger
            if velocity.y < 0:
                current_gravity = gravity_falling
            else:
                current_gravity = gravity_default
    else:
        if jetpack_stream.volume_db < linear_to_db(1):
            jetpack_stream.volume_db = linear_to_db(lerp(
                db_to_linear(jetpack_stream.volume_db),
                1.0,
                JETPACK_VOLUME_LERP_MULT * delta
            ))
            jetpack_stream.volume_db = min(linear_to_db(1),
            jetpack_stream.volume_db)

        current_gravity = 0
        current_energy -= delta * ENERGY_DRAIN_JETPACK_MULT
        current_energy_regen_cooldown = 0

        var vertical_target = JETPACK_VERTICAL_VELOCITY_MAX
        vertical_target = lerp(velocity.y, vertical_target, JETPACK_TARGER_LERP)
        var vertical_difference : float = vertical_target - velocity.y

        #final force that will be applied to character
        var jetpack_movement = vertical_difference * JETPACK_VERTICAL_FORCE

        if position.y <= current_jetpack_vertical + JETPACK_VERTICAL_DIFFERENCE_MAX:
            velocity.y += jetpack_movement * delta
            velocity.y = min(velocity.y, JETPACK_VERTICAL_VELOCITY_MAX)
        else:
            velocity.y = lerp(velocity.y, 0.0, 15 * delta)


        if Input.is_action_just_released("jump") || current_energy <= 0:
            current_jetpack_start_timer = 0
            if Input.is_action_pressed("sprint"): current_walk_state = WalkState.SPRINT
            else: current_walk_state = WalkState.NORMAL
            _UpdateCollider()

    # Add the gravity.
    if not is_on_floor():
        current_coyote_time -= delta
        if current_coyote_time < 0: current_coyote_time = 0
        velocity.y -= current_gravity * delta
        current_jetpack_vertical = min(position.y, current_jetpack_vertical)
    else:
        current_jetpack_vertical = position.y
        current_coyote_time = COYOTE_TIME_MAX


    #calculate dif between max and current speed
    #ignore y axis
    var speed_difference : Vector3 = target_speed - velocity
    speed_difference.y = 0

    #final force that will be applied to character
    var movement = speed_difference * current_acc_rate

    if is_on_floor() && floor_angle > SLOPE_SLIDE_THRESHOLD:
        var plane = Plane(get_floor_normal())
        movement = plane.project(movement)
        current_slide_vector = plane.project(current_slide_vector)

    if WalkState.SLIDE == current_walk_state:
        velocity = velocity + (movement) * delta * SLIDE_DAMPEN_RATE
        velocity = velocity + current_slide_vector * delta * (current_slide_time) * (-(current_slide_vector.y) + .01)
    else:
        velocity = velocity + (movement) * delta

    if velocity.y < -FALL_SPEED_MAX:
        velocity.y = -FALL_SPEED_MAX

    current_jump_cd -= delta
    if current_jump_cd < 0: current_jump_cd = 0

    debug_label.text = "State: " + WalkState.keys()[current_walk_state] + "\n" + str(velocity.length()) + "\n" + (
        "Vertical:" + str(velocity.y)) + "\nJetpack: " + str(current_energy) + (
        "\nGravity: " + str(current_gravity)) + "\nVolume: " + str(jetpack_stream.volume_db)

    _UpdateCameraPosition(delta, inverse_lerp(0, abs(SPRINT_SPEED), velocity.length()))
    move_and_slide()

func _UpdateCameraPosition(delta, speed_t):
    var t = CAMERA_LERP * delta

    if WalkState.SLIDE == current_walk_state:
        camera_3d.rotation.z = lerp(camera_3d.rotation.z, deg_to_rad(15.0), t)
    else:
        camera_3d.rotation.z = lerp(camera_3d.rotation.z, 0.0, t)

    if is_head_bob_active && WalkState.SLIDE != current_walk_state && is_on_floor() && direction != Vector3.ZERO:
        head_bob_var += current_head_bob_frequency * delta
        head_bob_pivot.position.y = lerp(
            head_bob_pivot.position.y,
            sin(head_bob_var) * current_head_bob_intensity / 2.0,
            t)
        head_bob_pivot.position.x = lerp(
            head_bob_pivot.position.x,
            cos(head_bob_var / 2.0) * current_head_bob_intensity,
            t)
    else:
        head_bob_pivot.position.y = lerp(
            head_bob_pivot.position.y,
            0.0,
            t)
        head_bob_pivot.position.x = lerp(
            head_bob_pivot.position.x,
            0.0,
            t)

    var tmp = lerp(CAMERA_FOV_NORMAL, CAMERA_FOV_MAX_SPEED, speed_t)
    camera_3d.fov = lerp(camera_3d.fov, min(tmp, CAMERA_FOV_MAX_SPEED), t)
    camera_pivot.position.y = lerp(camera_pivot.position.y, current_camera_height, t)

func _UpdateCollider():
    match current_walk_state:
        WalkState.NORMAL:
            collider.shape.height = COLLIDER_HEIGHT_NORMAL
            collider.position.y = COLLIDER_HEIGHT_NORMAL / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_NORMAL
            current_camera_height = CAMERA_HEIGHT_NORMAL
            current_max_speed = WALK_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_NORMAL
            current_head_bob_intensity = HEAD_BOB_INTENSITY_NORMAL
            current_lerp_acc = WALK_LERP_ACC
            current_lerp_dec = WALK_LERP_DEC
        WalkState.CROUCH:
            collider.shape.height = COLLIDER_HEIGHT_CROUCH
            collider.position.y = COLLIDER_HEIGHT_CROUCH / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_NORMAL
            current_camera_height = CAMERA_HEIGHT_CROUCH
            current_max_speed = CROUCH_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_CROUCH
            current_head_bob_intensity = HEAD_BOB_INTENSITY_CROUCH
            current_lerp_acc = CROUCH_LERP_ACC
            current_lerp_dec = CROUCH_LERP_DEC
        WalkState.SLIDE:
            collider.shape.height = COLLIDER_HEIGHT_CROUCH
            collider.position.y = COLLIDER_HEIGHT_CROUCH / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_NORMAL
            current_camera_height = CAMERA_HEIGHT_CROUCH
            current_max_speed = WALK_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_CROUCH
            current_head_bob_intensity = HEAD_BOB_INTENSITY_CROUCH
            current_lerp_acc = CROUCH_LERP_ACC
            current_lerp_dec = CROUCH_LERP_DEC
        WalkState.PRONE:
            collider.shape.height = COLLIDER_HEIGHT_PRONE
            collider.position.y = COLLIDER_HEIGHT_PRONE / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_CROUCH
            current_camera_height = CAMERA_HEIGHT_PRONE
            current_max_speed = PRONE_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_PRONE
            current_head_bob_intensity = HEAD_BOB_INTENSITY_PRONE
            current_lerp_acc = PRONE_LERP_ACC
            current_lerp_dec = PRONE_LERP_DEC
        WalkState.SPRINT:
            collider.shape.height = COLLIDER_HEIGHT_NORMAL
            collider.position.y = COLLIDER_HEIGHT_NORMAL / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_NORMAL
            current_camera_height = CAMERA_HEIGHT_NORMAL
            current_max_speed = SPRINT_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_SPRINT
            current_head_bob_intensity = HEAD_BOB_INTENSITY_SPRINT
            current_lerp_acc = SPRINT_LERP_ACC
            current_lerp_dec = SPRINT_LERP_DEC
        WalkState.JETPACK:
            collider.shape.height = COLLIDER_HEIGHT_NORMAL
            collider.position.y = COLLIDER_HEIGHT_NORMAL / 2.0
            height_raycast.target_position.y = COLLIDER_HEIGHT_NORMAL
            current_camera_height = CAMERA_HEIGHT_NORMAL
            current_max_speed = JETPACK_SPEED
            current_head_bob_frequency = HEAD_BOB_FREQUENCY_PRONE
            current_head_bob_intensity = HEAD_BOB_INTENSITY_PRONE
            current_lerp_acc = JETPACK_LERP_ACC
            current_lerp_dec = JETPACK_LERP_DEC

func _UpdateEnergyLabel():
    energy_bar_label.text = str(floor(current_energy))
    energy_bar_bar.value = current_energy