package numerics

// Crossed reports whether a termination condition holds at (t, y).
type Crossed func(t float64, y GeoVec) bool

// RefineCrossing locates a crossing between (t1, y1) (not crossed) and
// (t2, y2) (crossed) by binary search in the linear-interpolation parameter
// space, stopping when the parameter interval is narrower than tol.
//
// It returns the final midpoint sampled, matching Tawhiri's solver.pyx: the
// returned point is not guaranteed to satisfy the predicate, but for tol << 1
// it is within one tolerance-width of the true crossing.
func RefineCrossing(t1 float64, y1 GeoVec, t2 float64, y2 GeoVec, crossed Crossed, tol float64) (float64, GeoVec) {
	left, right := 0.0, 1.0
	t3, y3 := t2, y2
	for right-left > tol {
		mid := (left + right) / 2
		t3 = Lerp(t1, t2, mid)
		y3 = GeoLerp(y1, y2, mid)
		if crossed(t3, y3) {
			right = mid
		} else {
			left = mid
		}
	}
	return t3, y3
}

// Path is a struct-of-arrays trajectory: parallel slices of time and the
// three state components. SoA layout keeps each component contiguous, which
// is friendlier to cache and to vectorised post-processing than a slice of
// point structs, and lets the integrator append with a single bounds check
// per component.
type Path struct {
	T        []float64
	Lat      []float64
	Lng      []float64
	Altitude []float64
}

// NewPath returns a Path with capacity reserved for n points.
func NewPath(n int) Path {
	return Path{
		T:        make([]float64, 0, n),
		Lat:      make([]float64, 0, n),
		Lng:      make([]float64, 0, n),
		Altitude: make([]float64, 0, n),
	}
}

// Len returns the number of points in the path.
func (p *Path) Len() int { return len(p.T) }

// Append adds one point to the path.
func (p *Path) Append(t float64, y GeoVec) {
	p.T = append(p.T, t)
	p.Lat = append(p.Lat, y.Lat)
	p.Lng = append(p.Lng, y.Lng)
	p.Altitude = append(p.Altitude, y.Altitude)
}

// Last returns the final (t, state) of the path. It panics on an empty path.
func (p *Path) Last() (float64, GeoVec) {
	i := len(p.T) - 1
	return p.T[i], GeoVec{Lat: p.Lat[i], Lng: p.Lng[i], Altitude: p.Altitude[i]}
}

// At returns the point at index i.
func (p *Path) At(i int) (float64, GeoVec) {
	return p.T[i], GeoVec{Lat: p.Lat[i], Lng: p.Lng[i], Altitude: p.Altitude[i]}
}