package numerics

import (
	"math"
	"testing"
)

// Closed-form checks for great-circle stepping. Each expectation is an exact
// analytic result, not a golden value copied from a previous run.

const tolDeg = 1e-9

func TestGeoStep(t *testing.T) {
	t.Parallel()

	// Angular distance covered by `speed` for `dt` at sea level, in degrees.
	arcDeg := func(speed, dt float64) float64 {
		return speed * dt / EarthRadius * 180 / math.Pi
	}

	tests := []struct {
		name             string
		start            GeoVec
		rate             Rate
		dt               float64
		wantLat, wantLng float64
		wantAlt          float64
	}{
		{
			name:    "eastward at the equator advances longitude only",
			start:   GeoVec{Lat: 0, Lng: 0},
			rate:    Rate{East: 10},
			dt:      100,
			wantLat: 0,
			wantLng: arcDeg(10, 100),
		},
		{
			name:    "northward at the equator advances latitude only",
			start:   GeoVec{Lat: 0, Lng: 0},
			rate:    Rate{North: 10},
			dt:      100,
			wantLat: arcDeg(10, 100),
			wantLng: 0,
		},
		{
			name:    "zero horizontal rate leaves the position alone",
			start:   GeoVec{Lat: 51.5, Lng: 359.9, Altitude: 1000},
			rate:    Rate{Vertical: 5},
			dt:      10,
			wantLat: 51.5,
			wantLng: 359.9,
			wantAlt: 1050,
		},
		{
			// The whole point of the change: 111 m from the pole, a due-north
			// step must pass over the pole and come down the far meridian.
			// Start 0.001 deg from the pole, travel 600 m (0.005396 deg), so it
			// overshoots by 0.004396 deg on longitude 30+180.
			name:    "due north over the pole flips longitude by 180",
			start:   GeoVec{Lat: 89.999, Lng: 30},
			rate:    Rate{North: 10},
			dt:      60,
			wantLat: 90 - (arcDeg(10, 60) - 0.001),
			wantLng: 210,
		},
		{
			name:    "due south over the south pole flips longitude by 180",
			start:   GeoVec{Lat: -89.999, Lng: 200},
			rate:    Rate{North: -10},
			dt:      60,
			wantLat: -90 + (arcDeg(10, 60) - 0.001),
			wantLng: 20,
		},
		{
			name:    "longitude stays wrapped into [0,360)",
			start:   GeoVec{Lat: 0, Lng: 359.999},
			rate:    Rate{East: 100},
			dt:      100,
			wantLat: 0,
			wantLng: math.Mod(359.999+arcDeg(100, 100), 360),
		},
	}

	for _, tt := range tests {
		t.Run(tt.name, func(t *testing.T) {
			t.Parallel()
			got := GeoStep(tt.start, tt.rate, tt.dt)
			if math.Abs(got.Lat-tt.wantLat) > tolDeg {
				t.Errorf("Lat = %.12f, want %.12f", got.Lat, tt.wantLat)
			}
			if math.Abs(got.Lng-tt.wantLng) > tolDeg {
				t.Errorf("Lng = %.12f, want %.12f", got.Lng, tt.wantLng)
			}
			if math.Abs(got.Altitude-tt.wantAlt) > 1e-9 {
				t.Errorf("Altitude = %v, want %v", got.Altitude, tt.wantAlt)
			}
		})
	}
}

// The defect this replaces: dLng went as 1/cos(lat), reaching 1.46e12 deg/s at
// the pole. Ground displacement must instead stay equal to speed*dt at every
// latitude, because that is what physically happens.
func TestGeoStepGroundDistanceIndependentOfLatitude(t *testing.T) {
	t.Parallel()

	const speed, dt = 10.0, 60.0
	want := speed * dt // 600 m

	for _, lat := range []float64{0, 45, 60, 85.051129, 89, 89.9, 89.99, 89.999, 90} {
		start := GeoVec{Lat: lat, Lng: 17}
		got := GeoStep(start, Rate{East: speed}, dt)
		d := GreatCircleMetres(start, got)
		// 0.1 mm. The point is to catch a divergence — the old code was wrong by
		// a factor of 1e12 here — not to police the last bit: near the pole
		// cos(lat) is ~1e-5, so double precision limits this to a few microns.
		if math.Abs(d-want) > 1e-4 {
			t.Errorf("lat %g: ground distance = %.9f m, want %.9f m", lat, d, want)
		}
	}
}

func TestGeoStepReverseGoesBackAlongTheSameCircle(t *testing.T) {
	t.Parallel()

	// A negative dt must travel the same arc in the opposite direction. Note it
	// is NOT true that GeoStep(GeoStep(y, r, dt), r, -dt) == y: the local frame
	// rotates during the step, so the same east/north pair means a different
	// physical direction at the arrival point. The invariant that does hold is
	// that the two endpoints straddle the start on one great circle.
	const speed, dt = 12.0, 60.0
	for _, lat := range []float64{0, 60, 89.99} {
		y := GeoVec{Lat: lat, Lng: 100, Altitude: 5000}
		r := Rate{East: speed, Vertical: 3}
		fwd := GeoStep(y, r, dt)
		back := GeoStep(y, r, -dt)

		// The arc is flown at altitude, so its projection onto the surface —
		// which is what GreatCircleMetres reports — is shorter by R/(R+alt).
		wantGround := speed * dt * EarthRadius / (EarthRadius + y.Altitude)
		if d := GreatCircleMetres(y, back); math.Abs(d-wantGround) > 1e-4 {
			t.Errorf("lat %g: reverse arc = %.9f m, want %.9f m", lat, d, wantGround)
		}
		if d := GreatCircleMetres(fwd, back); math.Abs(d-2*wantGround) > 1e-4 {
			t.Errorf("lat %g: forward/reverse separation = %.9f m, want %.9f m",
				lat, d, 2*wantGround)
		}
		if math.Abs(back.Altitude-(y.Altitude-3*dt)) > 1e-9 {
			t.Errorf("lat %g: reverse altitude = %v, want %v", lat, back.Altitude, y.Altitude-3*dt)
		}
	}
}

// Great-circle motion is rotation about a fixed axis, so a field returning the
// local east/north components of `omega x r` has an exact solution: a single
// rotation. RK4 must reproduce it, including next to the pole where the local
// frame spins fastest between stages.
//
// A field with *constant* east/north would be a rhumb line, not a great circle,
// so it cannot be used for this comparison.
func TestRK4StepMatchesGreatCircle(t *testing.T) {
	t.Parallel()

	for _, lat := range []float64{0, 60, 89.9, 89.999} {
		start := GeoVec{Lat: lat, Lng: 40, Altitude: 20000}
		const speed, dt, n = 35.0, 60.0, 10

		// Rate at the start point defines the great circle; GeoStep over the
		// whole interval is then the exact answer.
		initial := Rate{East: speed * 0.8, North: speed * 0.6}
		exact := GeoStep(start, initial, dt*n)

		// Pointwise field for that same great circle: rotation about the axis
		// r0 x t0 at constant angular rate.
		field := greatCircleField(start, initial)

		stepped := start
		for range n {
			stepped = RK4Step(0, stepped, dt, field)
		}

		if d := GreatCircleMetres(stepped, exact); d > 0.01 {
			t.Errorf("lat %g: RK4 drifted %.6f m from the exact great circle", lat, d)
		}
	}
}

func TestAddRate(t *testing.T) {
	t.Parallel()

	got := AddRate(Rate{East: 1, North: 2, Vertical: 3}, Rate{East: 10, North: 20, Vertical: 30})
	want := Rate{East: 11, North: 22, Vertical: 33}
	if got != want {
		t.Errorf("AddRate = %+v, want %+v", got, want)
	}
}

// greatCircleField builds a rate field whose exact solution is the great circle
// through `start` with initial rate `initial`: rotation about the fixed axis
// r0 x t0. At any point it returns the local east/north components of that
// rotation's velocity, so the field is defined pointwise without needing to
// know how far along the arc we are.
func greatCircleField(start GeoVec, initial Rate) RateField {
	speed := math.Hypot(initial.East, initial.North)
	r0, e0, n0 := basis(start.Lat, start.Lng)
	var t0 [3]float64
	for i := range t0 {
		t0[i] = (initial.East*e0[i] + initial.North*n0[i]) / speed
	}
	axis := cross(r0, t0)

	return func(_ float64, y GeoVec) Rate {
		r, e, n := basis(y.Lat, y.Lng)
		v := cross(axis, r)
		return Rate{
			East:  speed * dot(v, e),
			North: speed * dot(v, n),
		}
	}
}

func cross(a, b [3]float64) [3]float64 {
	return [3]float64{
		a[1]*b[2] - a[2]*b[1],
		a[2]*b[0] - a[0]*b[2],
		a[0]*b[1] - a[1]*b[0],
	}
}

func dot(a, b [3]float64) float64 { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] }