"""
Newton's Method Example: Stopping Distance
-------------------------------------------

Engineering problem:
    A vehicle has:
        reaction time = 1.0 s
        braking deceleration = 7.0 m/s^2

    What initial speed v will produce a total stopping
    distance of exactly 50 m?

Stopping-distance model:

    d_stop = d_reaction + d_braking

    d_reaction = t_r * v
    d_braking  = v^2 / (2a)

Therefore:

    d_stop(v) = t_r*v + v^2/(2a)

We want to find the value of v for which:

    d_stop(v) = 50 m

Instead of solving the equation algebraically, we will use
Newton's Method and observe how the solution converges.

This example is also designed for learning how to DEBUG
a numerical algorithm in VS Code.
"""


# ============================================================
# ENGINEERING MODEL
# ============================================================

def stopping_distance(v, reaction_time, deceleration):
    """
    Calculate the total stopping distance of a vehicle.

    Parameters
    ----------
    v : float
        Initial vehicle speed [m/s]

    reaction_time : float
        Driver reaction time [s]

    deceleration : float
        Magnitude of braking deceleration [m/s^2]

    Returns
    -------
    float
        Total stopping distance [m]
    """

    # Distance traveled before the driver begins braking
    reaction_distance = reaction_time * v

    # Distance traveled while braking
    braking_distance = v**2 / (2 * deceleration)

    # Total distance from detecting the hazard to stopping
    return reaction_distance + braking_distance


# ============================================================
# ROOT-FINDING EQUATION
# ============================================================

def f(v):
    """
    Define the root-finding problem.

    We want:
        stopping_distance = 50 m

    Therefore:
        f(v) = stopping_distance(v) - 50

    The desired vehicle speed occurs when:
        f(v) = 0
    """

    return stopping_distance(v, 1.0, 7.0) - 50.0


def fprime(v):
    """
    Derivative (slope) of f(v).

    For:
        f(v) = v + v^2/14 - 50

    the derivative is:
        f'(v) = 1 + v/7
    """

    return 1.0 + v / 7.0


# ============================================================
# NEWTON'S METHOD
# ============================================================

# Starting estimate for the vehicle speed.
# Newton's Method needs an initial guess.
v = 20.0                 # [m/s]

# Stop iterating when the Newton correction becomes this small.
tolerance = 0.01         # [m/s]

# Keep track of how many Newton iterations are required.
iteration = 0


while True:

    # --------------------------------------------------------
    # DEBUGGING DEMONSTRATION:
    #
    # Put a BREAKPOINT on the next line.
    #
    # Then use STEP OVER to watch:
    #
    #   function_value  -> How far are we from the target?
    #   slope           -> What is the local slope?
    #   correction      -> How much should v change?
    #   v               -> What is our new estimate?
    # --------------------------------------------------------

    function_value = f(v)

    # Evaluate the slope at the current estimate
    slope = fprime(v)

    # Newton correction:
    #
    #       Δv = -f(v) / f'(v)
    #
    # This tells us how far to move from the current estimate.
    correction = -function_value / slope

    # Update the estimate
    v = v + correction

    # One Newton iteration has been completed
    iteration = iteration + 1

    # Display the progress of the numerical solution
    print(
        f"Iteration {iteration}: "
        f"v = {v:.6f} m/s, "
        f"correction = {correction:.6f} m/s"
    )

    # --------------------------------------------------------
    # CONVERGENCE CHECK
    # --------------------------------------------------------
    #
    # If Newton's correction is sufficiently small,
    # the estimate is no longer changing significantly.
    #
    if abs(correction) <= tolerance:
        break

    # --------------------------------------------------------
    # SAFETY GUARD
    # --------------------------------------------------------
    #
    # Newton's Method is fast when it works, but convergence
    # is NOT guaranteed. Never allow an uncontrolled loop.
    #
    assert iteration < 50, "Newton's Method did not converge"


# ============================================================
# REPORT AND VALIDATE THE SOLUTION
# ============================================================

print("\nSolution")
print(f"v = {v:.6f} m/s")

# IMPORTANT ENGINEERING PRACTICE:
# Substitute the computed speed back into the ORIGINAL
# engineering model. A numerical answer should be validated.
distance_check = stopping_distance(v, 1.0, 7.0)

print(f"Stopping distance = {distance_check:.6f} m")