using UnityEngine;

namespace Phscs
{
    public class BicyclePhysics
    {
        private const float K_A = 0.5f; //wind resistance coefficient
        private const float A = 0.6f; //frontal area bike + rider; m2
        private const float D = 1.226f; //air density; kg/m3

        /// <summary>
        /// Returns a speed in m/s for a bike riding a gradient with the same power it would be ridden in the flat with the input speed.
        /// Using formula from http://www.sportsci.org/jour/9804/dps.html. Assuming v=s and neglecting rolling resistance
        /// The speed is calculated by solving k_aAs_1^3d = k_aAs_2^3d + giMs_2 for s_2
        /// </summary>
        /// <param name="speedFlat">Speed the bike would have in flat terrain</param>
        /// <param name="systemMass">Mass of bike + rider</param>
        /// <param name="gradient">gradient in vertical meters gained per meter travelled forward</param>
        /// <returns></returns>
        public static float SpeedAtGradientForSpeedAtFlat(float speedFlat, float systemMass, float gradient)
        {
            var g = -Physics.gravity.y;
            const float k = K_A * A * D;
            var b = g * gradient * systemMass;

            float result;

            if (gradient < -0.01f) // 1 percent
            {
                result = 2 * Mathf.Sqrt(-b / (3 * k)) *
                         Mathf.Cos(
                             Mathf.Acos(3 * Mathf.Sqrt(-3 * k / b) * k * speedFlat
                                        / (2 * b))
                             / 3);
            }
            else
            {
                var sqrtTerm = Mathf.Sqrt(
                    Mathf.Pow(b, 3f)
                    /
                    (27f * Mathf.Pow(k, 3f))
                    +
                    Mathf.Pow(speedFlat, 6f)
                    /
                    4f
                );
                var secondTerm = Mathf.Pow(speedFlat, 3f) / 2f;
                result = Qbrt(sqrtTerm + secondTerm) + Qbrt(-sqrtTerm + secondTerm);
            }

            if (float.IsNaN(result))
            {
                Debug.LogWarning(
                    $"BicyclePhysics: result is NaN for k = {k} and b = {b} (gradient = {gradient}, mass = {systemMass})");
                return speedFlat;
            }

            return result;
        }

        private static float Qbrt(float x) => x < 0 ? -Mathf.Pow(-x, 1f / 3f) : Mathf.Pow(x, 1f / 3f);
    }
}