import numpy as np
import pytest

from defense_sim.guidance.ascent import altitude_reference, ascent_blend, automatic_ascent_length


@pytest.mark.parametrize('curve', ['cubic', 'quintic'])
def test_smoothstep_endpoints_monotonicity_and_airborne_start(curve):
    def height(x):
        return altitude_reference(x, 20, 100, curve=curve, length_m=500)
    assert height(-10) == height(0) == 20
    assert height(500) == height(700) == 100
    assert height(250) == pytest.approx(60)
    samples = np.array([height(x) for x in np.linspace(0, 500, 101)])
    assert np.all(np.diff(samples) >= 0)
    assert abs((height(.01)-height(0))/.01) < .001
    assert abs((height(500)-height(499.99))/.01) < .001


def test_quintic_second_derivative_vanishes_at_endpoints():
    def height(x):
        return altitude_reference(x, 0, 100, curve='quintic', length_m=500)
    dx = .1
    assert abs((height(2*dx)-2*height(dx)+height(0))/dx**2) < .0001
    assert abs((height(500)-2*height(500-dx)+height(500-2*dx))/dx**2) < .0001


def test_automatic_length_respects_nominal_peak_climb_rate():
    length = automatic_ascent_length(1500, 20, 5)
    assert 1.875*1500*20/length == pytest.approx(4.)
    assert automatic_ascent_length(10, 5, 5) == 500


def test_exponential_retains_asymptote_and_initial_altitude():
    assert altitude_reference(0, 20, 100, curve='exponential') == 20
    at_scale = altitude_reference(500, 20, 100, curve='exponential', k_per_m=.002)
    assert at_scale == pytest.approx(20+80*(1-np.exp(-1)))
    assert at_scale < 100


def test_blend_slope_matches_the_altitude_curve_and_is_flat_at_the_ends():
    for curve in ("cubic", "quintic"):
        for x in (0., 100., 250., 400.):
            weight, slope = ascent_blend(x, 500., curve=curve)
            climbed = altitude_reference(x, 0., 100., curve=curve, length_m=500.)
            assert climbed == pytest.approx(100. * weight)
            dx = 0.05
            right = altitude_reference(x + dx, 0., 100., curve=curve, length_m=500.)
            left = altitude_reference(max(0., x - dx), 0., 100., curve=curve, length_m=500.)
            numeric = (right - left) / (dx if x == 0. else 2 * dx)
            assert slope * 100. == pytest.approx(numeric, abs=0.05)
        assert ascent_blend(0., 500., curve=curve) == (0., 0.)
        assert ascent_blend(500., 500., curve=curve) == (1., 0.)
        assert ascent_blend(700., 500., curve=curve) == (1., 0.)
    weight, slope = ascent_blend(0., 500., curve="exponential", k_per_m=.002)
    assert weight == 0. and slope == pytest.approx(.002)


def test_invalid_ascent_configuration_fails():
    with pytest.raises(ValueError):
        altitude_reference(0, 0, 100, length_m=0)