using System;
using UnityEngine;

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;

        var divisor = Mathf.Pow(g * gradient * systemMass, 3f);
        var dividend = 27f * Mathf.Pow(A * D * K_A, 3f);
        Console.WriteLine($"Divisor: {divisor}");
        Console.WriteLine($"Dividend: {dividend}");

        var divisor2 = Mathf.Pow(speedFlat, 6f);
        var dividend2 = 4f;
        Console.WriteLine($"Divisor2: {divisor2}");
        Console.WriteLine($"Dividend2: {dividend2}");
        
        var sqrtTerm = Mathf.Sqrt(
            divisor
            /
            dividend
            +
            divisor2
            /
            dividend2
        );
        
        Console.WriteLine($"Sqrt: {sqrtTerm}");

        var divisor3 = Mathf.Pow(speedFlat, 3f);
        var dividend3 = 2f;
        Console.WriteLine($"Divisor3: {divisor3}");
        Console.WriteLine($"Dividend3: {dividend3}");
        
        var secondTerm = divisor3 / dividend3;
        
        Console.WriteLine($"Second term: {secondTerm}");
        Console.WriteLine($"SqrtTerm + secondTerm: {sqrtTerm+secondTerm}");

        var firstqbrt = Qbrt(sqrtTerm + secondTerm);
        var secondqbrt = Qbrt(-sqrtTerm + secondTerm);
        
        Console.WriteLine($"firstqbrt: {firstqbrt}");
        Console.WriteLine($"secondqbrt: {secondqbrt}");
        
        return firstqbrt + secondqbrt;
    }

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