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
}