predictor/internal/engine/propagator.go

package engine

import (
	"predictor-refactored/internal/numerics"
)

// Propagator advances state under one Model, checking a set of Constraints
// after every integration step.
//
// When a constraint fires, the propagator binary-search refines the violation
// point and emits it as its final trajectory point. The Action of the
// triggering constraint controls what the surrounding Profile does next:
// stop the profile, transfer to Fallback, or clip and continue.
type Propagator struct {
	// Name identifies the propagator in trajectory metadata.
	Name string

	// Step is the magnitude of the integration step in seconds (always positive).
	// The Profile flips its sign for Reverse direction.
	Step float64

	// Model produces the per-second time derivative of state.
	Model Model

	// Constraints are evaluated after each step. Any fired constraint stops
	// the propagator at the refined point; the first one in this slice wins
	// on ties.
	Constraints []Constraint

	// Fallback is the propagator to switch to when a constraint with
	// ActionFallback fires. Optional.
	Fallback *Propagator

	// Tolerance is the binary-search refinement tolerance in parameter space
	// (default 0.01, matching Tawhiri).
	Tolerance float64
}

// Outcome describes how a propagator's run ended.
type Outcome int

const (
	// OutcomeStopped means a Constraint with ActionStop fired and the profile
	// should end here.
	OutcomeStopped Outcome = iota
	// OutcomeFallback means a Constraint with ActionFallback fired and the
	// profile should transfer to the propagator's Fallback chain.
	OutcomeFallback
	// OutcomeContinued means no constraint fired before the time horizon was
	// reached. In practice this is only seen when a propagator runs unbounded,
	// which means the profile is misconfigured.
	OutcomeContinued
)

// Result is the output of running one propagator.
type Result struct {
	Propagator string
	Points     []TrajectoryPoint
	Outcome    Outcome
	// Constraint is the constraint that fired, or nil if Outcome == OutcomeContinued.
	Constraint Constraint
}

// run integrates the model from (t0, s0) in direction dir, returning a Result.
// globals are constraints injected by the Profile and checked alongside the
// propagator's local Constraints.
func (p *Propagator) run(t0 float64, s0 State, dir Direction, globals []Constraint) Result {
	dt := p.Step * float64(dir)
	tol := p.Tolerance
	if tol == 0 {
		tol = 0.01
	}

	deriv := numerics.Deriv[State](func(t float64, s State) State { return p.Model(t, s) })
	add := numerics.VecAdd[State](stateAdd)
	lerp := numerics.VecLerp[State](stateLerp)

	out := Result{
		Propagator: p.Name,
		Outcome:    OutcomeContinued,
		Points: []TrajectoryPoint{{
			Time: t0, Lat: s0.Lat, Lng: s0.Lng, Altitude: s0.Altitude,
		}},
	}

	t := t0
	s := s0

	for {
		s2 := numerics.RK4Step(t, s, dt, deriv, add)
		t2 := t + dt

		if c, fired := firstFiring(p.Constraints, globals, t2, s2); fired {
			trig := numerics.Trigger[State](func(tt float64, ss State) bool { return c.Violated(tt, ss) })
			t3, s3 := numerics.RefineTrigger(t, s, t2, s2, trig, lerp, tol)

			switch c.Action() {
			case ActionClip:
				s3 = clipToConstraint(c, s3)
				out.Points = append(out.Points, TrajectoryPoint{
					Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
				})
				t, s = t3, s3
				continue
			case ActionFallback:
				out.Points = append(out.Points, TrajectoryPoint{
					Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
				})
				out.Outcome = OutcomeFallback
				out.Constraint = c
				return out
			default: // ActionStop
				out.Points = append(out.Points, TrajectoryPoint{
					Time: t3, Lat: s3.Lat, Lng: s3.Lng, Altitude: s3.Altitude,
				})
				out.Outcome = OutcomeStopped
				out.Constraint = c
				return out
			}
		}

		t, s = t2, s2
		out.Points = append(out.Points, TrajectoryPoint{
			Time: t, Lat: s.Lat, Lng: s.Lng, Altitude: s.Altitude,
		})
	}
}

// firstFiring scans local then global constraints for the first one whose
// Violated returns true at (t, s).
func firstFiring(local, globals []Constraint, t float64, s State) (Constraint, bool) {
	for _, c := range local {
		if c.Violated(t, s) {
			return c, true
		}
	}
	for _, c := range globals {
		if c.Violated(t, s) {
			return c, true
		}
	}
	return nil, false
}

// clipToConstraint adjusts s so that the given constraint is exactly satisfied
// (not violated). Implemented for constraints with a well-defined boundary;
// others fall through unchanged.
func clipToConstraint(c Constraint, s State) State {
	switch v := c.(type) {
	case MaxAltitude:
		s.Altitude = v.Limit
	case MinAltitude:
		s.Altitude = v.Limit
	}
	return s
}