Untitled

using System;
using System.Collections;
using UnityEngine;

public class PlayerPhysics : MonoBehaviour
{
    public Rigidbody RB;
    public LayerMask layermask;
    public Vector3 horizontalVelocity => Vector3.ProjectOnPlane(RB.velocity, RB.transform.up);
    public Vector3 verticalVelocity => Vector3.Project(RB.velocity, RB.transform.up);
    public float verticalSpeed => Vector3.Dot(RB.velocity, RB.transform.up);
    public float speed => horizontalVelocity.magnitude;

    [SerializeField] float gravity;
    [SerializeField] float smoothRotationSpeed = 5f;  // Speed for smoothing rotation
    [SerializeField] float smoothPositionSpeed = 5f;  // Speed for smoothing position

    public Action onPlayerPhysicsUpdate;

    void FixedUpdate() 
    { 
        onPlayerPhysicsUpdate?.Invoke();

        if(!groundInfo.ground)
        {
            Gravity();
        }

        if(groundInfo.ground && verticalSpeed < RB.sleepThreshold)
            RB.velocity = horizontalVelocity;

        StartCoroutine(LateFixedUpdateRoutine());

        IEnumerator LateFixedUpdateRoutine()
        {
            yield return new WaitForFixedUpdate();
            LateFixedUpdate();
        }
    }

    void Gravity()
    {
        RB.velocity -= Vector3.up * gravity * Time.deltaTime;
    }

    void LateFixedUpdate()
    {
        Ground();
        Snap();

        if (groundInfo.ground)
            RB.velocity = horizontalVelocity;
    }

    [SerializeField] float groundDistance;
    
    public struct GroundInfo
    {
        public Vector3 point;
        public bool ground;
        public Vector3 normal;
    }

    [HideInInspector] public GroundInfo groundInfo;

    public Action onGroundEnter;
    public Action onGroundExit;

    void Ground()
    {
        float maxDistance = Mathf.Max(RB.centerOfMass.y, 0) + (RB.sleepThreshold * Time.fixedDeltaTime);

        if (groundInfo.ground && verticalSpeed < RB.sleepThreshold)
            maxDistance += groundDistance;

        bool ground = Physics.Raycast(RB.worldCenterOfMass, -RB.transform.up, out RaycastHit hit, maxDistance, layermask, QueryTriggerInteraction.Ignore);

        Vector3 point = ground ? hit.point : RB.transform.position;
        Vector3 normal = ground ? hit.normal : Vector3.up;

        if (ground != groundInfo.ground)
        {
            if (ground)
                onGroundEnter?.Invoke();
            else
                onGroundExit?.Invoke();
        }

        groundInfo = new()
        {
            point = point,
            normal = normal,
            ground = ground
        };
    }

    void Snap()
    {
        // Current ground normal
        Vector3 targetUp = groundInfo.normal;

        // Smoothly interpolate the player's up direction
        if (IsOnSlope())
        {
            // Slerp the player's up vector to smoothly match the ground's normal
            RB.transform.up = Vector3.Slerp(RB.transform.up, targetUp, Time.deltaTime * smoothRotationSpeed);
        }
        else
        {
            // Instantly snap the player's up vector to match the ground's normal
            RB.transform.up = targetUp;
        }

        // Determine target position
        Vector3 goal = groundInfo.point;
        Vector3 difference = goal - RB.transform.position;

        // If there's no obstacle in the way, smoothly move the player to the target ground position
        if (!RB.SweepTest(difference, out _, difference.magnitude, QueryTriggerInteraction.Ignore))
        {
            if (IsOnSlope())
            {
                // Smoothly interpolate the player's position to the ground position
                RB.transform.position = Vector3.Lerp(RB.transform.position, goal, Time.deltaTime * smoothPositionSpeed);
            }
            else
            {
                // Instantly snap the player's position to the ground position
                RB.transform.position = goal;
            }
        }
    }

    bool IsOnSlope()
    {
        // Calculate the up/down angle
        Vector3 forward = RB.transform.forward;
        Vector3 projectedForward = Vector3.ProjectOnPlane(forward, Vector3.up).normalized;
        float cosAngle = Vector3.Dot(projectedForward, forward.normalized);
        float angleInDegrees = Mathf.Acos(cosAngle) * Mathf.Rad2Deg;

        // Return true if the angle is significant (i.e., the player is on a slope)
        return Mathf.Abs(angleInDegrees) > 0.1f; // Threshold can be adjusted
    }
}