step one

internal/engine/propagator.go

@@ -0,0 +1,156 @@
+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
+}