package numerics

import "math"

// GeoVec is a geographic state vector: latitude and longitude in degrees and
// altitude in metres. The same struct represents a per-second derivative,
// in which case the fields are deg/s and m/s.
//
// GeoVec is the hot-path state type for the integrator. It is a small value
// type (three float64) and is passed by value to stay allocation-free; a
// future SIMD/SoA batch integrator can lift these fields into parallel
// slices (see Path).
type GeoVec struct {
	Lat      float64 `json:"lat"`
	Lng      float64 `json:"lng"`
	Altitude float64 `json:"altitude"`
}

// PyMod returns a mod b with Python semantics: the result carries the sign of
// b, so for b > 0 it always lies in [0, b).
func PyMod(a, b float64) float64 {
	r := math.Mod(a, b)
	if r < 0 {
		r += b
	}
	return r
}

// GeoAdd returns y + k*dy with longitude wrapped to [0, 360). Latitude and
// altitude accumulate linearly. This is the integrator's state-update step.
func GeoAdd(y GeoVec, k float64, dy GeoVec) GeoVec {
	return GeoVec{
		Lat:      y.Lat + k*dy.Lat,
		Lng:      PyMod(y.Lng+k*dy.Lng, 360),
		Altitude: y.Altitude + k*dy.Altitude,
	}
}

// GeoLerp linearly interpolates two geographic states by parameter l in
// [0, 1]. Longitude takes the shorter great-circle arc.
func GeoLerp(a, b GeoVec, l float64) GeoVec {
	return GeoVec{
		Lat:      (1-l)*a.Lat + l*b.Lat,
		Lng:      LngLerp(a.Lng, b.Lng, l),
		Altitude: (1-l)*a.Altitude + l*b.Altitude,
	}
}

// LngLerp interpolates between two longitudes in [0, 360), choosing the
// shorter arc and wrapping the result back into range.
func LngLerp(a, b, l float64) float64 {
	l2 := 1 - l
	if a > b {
		a, b = b, a
		l, l2 = l2, l
	}
	if b-a < 180 {
		return l2*a + l*b
	}
	return PyMod(l2*(a+360)+l*b, 360)
}

// Lerp returns (1-l)*a + l*b.
func Lerp(a, b, l float64) float64 {
	return (1-l)*a + l*b
}

// AddGeo returns the component-wise sum a+b without longitude wrapping. Use it
// to combine derivative (rate) vectors — rates accumulate linearly, unlike
// positions, which wrap via GeoAdd.
func AddGeo(a, b GeoVec) GeoVec {
	return GeoVec{Lat: a.Lat + b.Lat, Lng: a.Lng + b.Lng, Altitude: a.Altitude + b.Altitude}
}

// EarthRadius is the spherical Earth radius (metres) used for horizontal
// motion, matching the reference Tawhiri implementation.
const EarthRadius = 6371009.0

// WindToGeoRate converts eastward (u) and northward (v) wind in m/s at the
// given latitude (deg) and altitude (m) into the geographic rate in deg/s on a
// spherical Earth. The returned dLng diverges near the poles as cos(lat) → 0.
func WindToGeoRate(u, v, lat, alt float64) (dLat, dLng float64) {
	const degPerRad = 180.0 / math.Pi
	const piOver180 = math.Pi / 180.0
	r := EarthRadius + alt
	dLat = degPerRad * v / r
	dLng = degPerRad * u / (r * math.Cos(lat*piOver180))
	return dLat, dLng
}