HoHCI-BikeSteering/Assets/Scripts/Phscs/RbBicycleSlopeSpeedManager.cs

using System;
using Controller.Bicycle;
using UnityEngine;
using Wheels;

namespace Phscs
{
    [RequireComponent(typeof(RbBicycleController))]
    public class RbBicycleSlopeSpeedManager : MonoBehaviour
    {
        [Header("Game Objects")] public LerpSlopeCollider slopeCollider;
        private RbBicycleController bicycleController;

        [Header("Linear Adjustment")] [Range(0, 3)]
        public float speedFactorUphill;

        [Range(0, 3)] public float speedFactorDownhill;

        [Header("Lerp time at slope changes")] public float lerpTimeUphillStart; //TODO: maybe as factor of speed
        public float lerpTimeUphillEnd;
        public float lerpTimeDownhillStart;
        public float lerpTimeDownhillEnd;

        private const float THRESHOLD = 0.05f;
        private const float SLOPE_CHANGE_THRESHOLD = 2f;

        private float t0;
        private float v0;
        private readonly float g = UnityEngine.Physics.gravity.y;
        private float alpha;
        private float alphaBefore;
        private int direction;
        private int directionBefore;

        public float? AdjustedSpeed { private set; get; }


        private void Start()
        {
            bicycleController = GetComponent<RbBicycleController>();
            slopeCollider.OnSlopeChanged += delegate(float timestamp, float slopeDeg)
            {
                alphaBefore = alpha;
                alpha = slopeDeg;
                if (Mathf.Abs(alphaBefore - alpha) < SLOPE_CHANGE_THRESHOLD) return;
                t0 = timestamp;
                v0 = bicycleController.CurrentSpeed;
                directionBefore = direction;
            };
        }

        private void OnGUI()
        {
            GUI.TextField(new Rect(150, 10, 200, 20), $"direction: {direction}, directionBefore: {directionBefore}");
        }

        private void FixedUpdate()
        {
            direction = Math.Abs(alpha) > THRESHOLD ? slopeCollider.SlopeDirection : 0;
            if (direction == 0 && directionBefore == 0)
            {
                AdjustedSpeed = null;
                return;
            }
            var t = Time.fixedTime - t0;
            var gradient = Mathf.Tan(direction * alpha * Mathf.Deg2Rad);
            var gradientBefore = Mathf.Tan(directionBefore * alphaBefore * Mathf.Deg2Rad);
            var mass = bicycleController.rigidBody.mass;
            float calculatedSpeed;
            var sensedSpeed = bicycleController.CurrentSpeedSensed;

            if (directionBefore == 0 && direction == 1) //flat to uphill
            {
                calculatedSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(sensedSpeed, mass, gradient) * speedFactorUphill;
                AdjustedSpeed = Mathf.Lerp(sensedSpeed, calculatedSpeed, t / lerpTimeUphillStart);
            }
            else if (directionBefore == 1 && direction == 0) //uphill to flat
            {
                calculatedSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(sensedSpeed, mass, gradientBefore) * speedFactorUphill;
                AdjustedSpeed = Mathf.Lerp(calculatedSpeed, sensedSpeed , t / lerpTimeUphillEnd);
            }
            else if (directionBefore == 0 && direction == -1) //flat to downhill
            {
                calculatedSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(sensedSpeed, mass, gradient) * speedFactorDownhill;
                AdjustedSpeed = Mathf.Lerp(sensedSpeed, calculatedSpeed, t / lerpTimeDownhillStart);
            }
            else if (directionBefore == -1 && direction == 0) //downhill to flat
            {
                calculatedSpeed = BicyclePhysics.SpeedAtGradientForSpeedAtFlat(sensedSpeed, mass, gradientBefore) * speedFactorDownhill;
                AdjustedSpeed = Mathf.Lerp(calculatedSpeed, sensedSpeed, t / lerpTimeDownhillEnd);
            }
        }
    }
}