machineGroupControl/src/movement/movementScheduler.js

'use strict';

// Pure movement planner. Given a set of machine profile snapshots and the
// optimizer's chosen flow combination, returns a tick-indexed schedule of
// commands that minimises flow disruption during the transition.
//
// Algorithm — rendezvous-on-demand-at-current-pressure:
//
//   1. For each machine, classify the move it needs (startup, flow-move
//      up, flow-move down, shutdown, no-op) based on its current FSM state
//      and the optimizer's target flow for it.
//   2. Compute eta_i (seconds-to-target-flow) per machine via
//      MoveTrajectory. Machines that can't contribute on this dispatch
//      (stopping / coolingdown / unknown) are skipped.
//   3. Rendezvous time t* = max(eta_i over ALL non-noop moves). The
//      slowest move (typically a startup ladder + ramp) sets the deadline.
//   4. Every command — including a startup's `execsequence` — is delayed by
//      (t* − eta_j) so its move FINISHES at t*. A startup is delayed as a
//      whole: its ladder begins at (t* − eta) and completes at (t* − rampS),
//      then the queued flowmovement (held in the pump's delayedMove) ramps to
//      finish at t*. The slowest mover (t* − eta == 0) fires immediately.
//      Delaying the ladder — rather than firing it at tick 0 — is what keeps a
//      faster-than-slowest startup from reaching `operational` early and
//      sitting at its MINIMUM flow before t* (calcFlow at min position is not
//      zero), which otherwise leaks ~min-flow into the group total ahead of
//      the rendezvous (the staging bump).
//
//   Net effect: ALL pumps reach their per-pump flow target at the same
//   wall-clock instant t*. Sum-of-flows is monotonic during the transition
//   (no overshoot from a fast in-flight retarget arriving before the
//   startup pumps catch up).
//
// The pump's flow→position conversion (via predictCtrl.y) lives in the
// profile so this module is pure: no Node-RED calls, no live child reads.

const MoveTrajectory = require('./moveTrajectory');

const ACTIVE_STATES = new Set(['operational', 'accelerating', 'decelerating']);
const STARTUP_LADDER = new Set(['starting', 'warmingup']);
const SHUTDOWN_LADDER = new Set(['stopping', 'coolingdown']);

// Tick cadence — MGC main loop is 1 Hz per .claude/rules tick convention.
const DEFAULT_TICK_S = 1;

function isOn(state) {
    return ACTIVE_STATES.has(state) || STARTUP_LADDER.has(state);
}

// Classify the action a machine needs. The optimizer's combination is a
// canonical statement of "what flow should this machine deliver now."
// `targetFlow == 0` (or absence from combination) means "this machine is
// not part of the new combination."
function classify(profile, targetFlow) {
    const isOff = !isOn(profile.state) && !SHUTDOWN_LADDER.has(profile.state);
    if (targetFlow > 0) {
        if (isOff) return 'startup';
        return 'flowmove'; // up or down depending on current vs target
    }
    // targetFlow <= 0
    if (ACTIVE_STATES.has(profile.state) || STARTUP_LADDER.has(profile.state)) {
        return 'shutdown';
    }
    return 'noop';
}

// Direction in flow-space: increasing, decreasing, or unchanged. Drives
// rendezvous: t* is the max eta over INCREASING moves; DECREASING moves
// get delayed to land at t*.
function directionOf(profile, targetFlow) {
    if (!isOn(profile.state)) return targetFlow > 0 ? 'increasing' : 'unchanged';
    const currentFlow = Number.isFinite(profile.flowAt?.(profile.position, profile._pressureForClassification))
        ? profile.flowAt(profile.position, profile._pressureForClassification)
        : null;
    if (currentFlow == null) {
        // Without a current-flow read, assume increasing iff target > 0.
        return targetFlow > 0 ? 'increasing' : 'decreasing';
    }
    if (targetFlow > currentFlow) return 'increasing';
    if (targetFlow < currentFlow) return 'decreasing';
    return 'unchanged';
}

// Plan the schedule.
//
//   profiles        — array from buildProfile(child)
//   combination     — array of {machineId, flow} from optimizer
//   currentPressure — Pa, for flow→flow and flow→position conversions
//   options         — { tickS?: 1, useRendezvous?: true }
//
//   useRendezvous=false collapses the schedule to "all commands fire at
//   tick 0" — every pump moves at its own speed and lands at its own eta.
//   Used when the operator explicitly opts out of same-time landing.
function plan(profiles, combination, currentPressure, options = {}) {
    const tickS = Number.isFinite(options.tickS) && options.tickS > 0 ? options.tickS : DEFAULT_TICK_S;
    const useRendezvous = options.useRendezvous !== false;
    const targets = new Map();
    for (const item of combination || []) {
        if (item && item.machineId != null) targets.set(String(item.machineId), Number(item.flow) || 0);
    }

    // First pass: classify + compute eta per machine.
    const plans = [];
    for (const p of profiles) {
        const id = String(p.id);
        const targetFlow = targets.get(id) ?? 0;

        // Stash pressure on a copy of the profile so directionOf can read it
        // without changing the public profile shape. Non-mutating: classify
        // only needs the value during this pass.
        const probeProfile = Object.assign({}, p, { _pressureForClassification: currentPressure });
        const action = classify(p, targetFlow);
        const direction = directionOf(probeProfile, targetFlow);

        if (action === 'noop') {
            plans.push({ machineId: id, action, direction, eta: 0, targetFlow, skip: true });
            continue;
        }

        // Convert target flow to target position using the pump's inverse
        // curve (lives on the profile). Fallback: linear interpolation
        // across [min,max] using the curve domain we know.
        let targetPosition = null;
        if (action !== 'shutdown' && typeof p.positionForFlow === 'function') {
            targetPosition = p.positionForFlow(targetFlow);
        }
        if (targetPosition == null) {
            // Shutdown: target is the minimum position.
            targetPosition = action === 'shutdown' ? (Number.isFinite(p.minPosition) ? p.minPosition : 0) : p.position;
        }

        let eta;
        // Per-pump ladder duration; used to gate the flowmovement so it
        // can't fire before warmup completes (the pump won't accept it).
        const ladderS = action === 'startup'
            ? ((Number(p.timings?.startingS) || 0) + (Number(p.timings?.warmingupS) || 0))
            : 0;
        // Ramp-only portion of the eta. For startup this is eta − ladder.
        // For flow-move or shutdown the entire eta IS the ramp.
        let rampS = 0;

        if (action === 'shutdown') {
            // Time for flow to reach zero = position ramp from current
            // position to minPosition. stoppingS / coolingdownS happen
            // AFTER flow is zero; they don't affect rendezvous.
            const v = Number(p.velocityPctPerS) > 0 ? p.velocityPctPerS : Infinity;
            const dist = Math.max(0, p.position - (p.minPosition ?? 0));
            eta = v === Infinity ? 0 : dist / v;
            rampS = eta;
        } else {
            const traj = new MoveTrajectory(p, { targetPosition });
            eta = traj.etaToTargetS();
            if (eta == null) eta = Infinity; // shouldn't happen for non-shutdown actions, but defensive
            rampS = Math.max(0, Number.isFinite(eta) ? eta - ladderS : 0);
        }

        plans.push({ machineId: id, action, direction, eta, ladderS, rampS, targetFlow, targetPosition, skip: false });
    }

    // Rendezvous: t* = max eta over ALL non-noop moves. Includes
    // increasing AND decreasing flow-moves so the slowest mover sets the
    // deadline for everyone. When useRendezvous=false, tStar is forced
    // to 0 so every command's delay collapses to 0 (legacy behaviour).
    const allEtas = plans
        .filter((q) => !q.skip && Number.isFinite(q.eta))
        .map((q) => q.eta);
    const tStar = useRendezvous && allEtas.length > 0 ? Math.max(...allEtas) : 0;

    // Second pass: assign fireAtTickN. Every command is delayed so its
    // move finishes at t*; the lone exception is the startup ladder's
    // execsequence (the ladder must begin now because eta == ladder + ramp).
    const commands = [];
    for (const q of plans) {
        if (q.skip) continue;

        // Delay-to-rendezvous: fire (t* − eta) seconds from now so the
        // move FINISHES at t*. Clamped to >= 0 (the eta == t* mover fires
        // immediately).
        const fireAtSDelayed = Math.max(0, tStar - q.eta);
        const fireAtTickNDelayed = Math.round(fireAtSDelayed / tickS);
        // Unchanged moves are no-ops; fire at 0 for simplicity (the pump
        // ignores them and we don't pollute the schedule with delays).
        const isUnchanged = q.direction === 'unchanged';

        if (q.action === 'startup') {
            // Just-in-time start. Delay the ENTIRE startup — ladder AND ramp —
            // by (t* − eta), so the warmup ladder finishes (and the ramp
            // begins) at (t* − rampS) and the flow lands exactly at t*.
            //
            // The ladder duration can't be compressed, but it CAN be delayed.
            // Firing the execsequence at tick 0 (the old behaviour) made a
            // faster-than-slowest startup reach `operational` early and sit at
            // its minimum flow from warmup-end until its delayed ramp — leaking
            // ~min-flow into the group total before t* (the staging bump). For
            // the slowest pump (eta == t*) fireAtTickNDelayed is 0, so it still
            // fires immediately. The flowmovement fires on the same tick; the
            // pump holds it in delayedMove through the ladder, then ramps over
            // rampS to finish at t*.
            commands.push({
                machineId: q.machineId,
                action: 'execsequence',
                sequence: 'startup',
                fireAtTickN: fireAtTickNDelayed,
                eta: q.eta,
            });
            commands.push({
                machineId: q.machineId,
                action: 'flowmovement',
                flow: q.targetFlow,
                fireAtTickN: fireAtTickNDelayed,
                eta: q.eta,
            });
        } else if (q.action === 'flowmove') {
            commands.push({
                machineId: q.machineId,
                action: 'flowmovement',
                flow: q.targetFlow,
                // Unchanged moves are no-ops; fire immediately so we
                // don't park them behind a long startup ladder for no
                // reason. Up/down moves both delay so they land at t*.
                fireAtTickN: isUnchanged ? 0 : fireAtTickNDelayed,
                eta: q.eta,
            });
        } else if (q.action === 'shutdown') {
            commands.push({
                machineId: q.machineId,
                action: 'execsequence',
                sequence: 'shutdown',
                fireAtTickN: fireAtTickNDelayed,
                eta: q.eta,
            });
        }
    }

    return {
        tStarS: tStar,
        tickS,
        commands,
        // Debugging telemetry — kept in the output so tests can introspect.
        _plans: plans,
    };
}

module.exports = { plan, DEFAULT_TICK_S };