Modbus-rtu/PLSR/Src/plsr_planner.c

#include "plsr_planner.h"

#include <stddef.h>
#include <string.h>

#define PLSR_PLANNER_Q32_ONE (4294967296ULL)

#if !defined(PLSR_HOST_TEST)
#define PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS  (0xE000EDFCUL)
#define PLSR_PLANNER_BENCHMARK_DWT_CTRL       (0xE0001000UL)
#define PLSR_PLANNER_BENCHMARK_DWT_CYCCNT     (0xE0001004UL)
#define PLSR_PLANNER_BENCHMARK_TRCENA         (1UL << 24U)
#define PLSR_PLANNER_BENCHMARK_CYCCNTENA      (1UL << 0U)
#define PLSR_PLANNER_BENCHMARK_PULSES         (100000UL)
#define PLSR_PLANNER_BENCHMARK_BATCH_CALLS    (128UL)
#define PLSR_PLANNER_BENCHMARK_TARGET_HZ      (100000UL)

volatile uint32_t PlsrPlannerBenchmarkRequest;
volatile uint32_t PlsrPlannerBenchmarkRunning;
volatile uint32_t PlsrPlannerBenchmarkRunCount;
volatile uint32_t PlsrPlannerBenchmarkCoreClockHz = 168000000UL;
volatile PLSR_PLANNER_BENCHMARK_RESULT PlsrPlannerBenchmarkResults[3];
#endif

#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
#define PLSR_PLANNER_CYCCNT_ADDRESS (0xE0001004UL)

volatile PLSR_PLANNER_TIMING PlsrPlannerTiming;

static uint32_t PlsrPlannerTimingNow(void)
{
    return *((volatile uint32_t *)PLSR_PLANNER_CYCCNT_ADDRESS);
}

static void PlsrPlannerTimingRecord(
    volatile PLSR_PLANNER_TIMING_SAMPLE *sample,
    uint32_t startedAt)
{
    uint32_t elapsed = PlsrPlannerTimingNow() - startedAt;
    uint32_t total = sample->totalCycles;

    sample->lastCycles = elapsed;
    if ((sample->callCount == 0UL) || (elapsed < sample->minCycles))
    {
        sample->minCycles = elapsed;
    }
    if (sample->callCount != 0xFFFFFFFFUL)
    {
        sample->callCount++;
    }
    sample->totalCycles = (elapsed > (0xFFFFFFFFUL - total))
                              ? 0xFFFFFFFFUL : total + elapsed;
    if (elapsed > sample->maxCycles)
    {
        sample->maxCycles = elapsed;
    }
}

void PlsrPlannerTimingReset(void)
{
    (void)memset((void *)&PlsrPlannerTiming, 0, sizeof(PlsrPlannerTiming));
}
#endif

static const uint32_t PlsrPlannerSmoothIntegralQ24[65] =
{
    0UL, 64UL, 504UL, 1688UL, 3968UL, 7688UL, 13176UL, 20752UL,
    30720UL, 43376UL, 59000UL, 77864UL, 100224UL, 126328UL, 156408UL,
    190688UL, 229376UL, 272672UL, 320760UL, 373816UL, 432000UL,
    495464UL, 564344UL, 638768UL, 718848UL, 804688UL, 896376UL,
    993992UL, 1097600UL, 1207256UL, 1323000UL, 1444864UL, 1572864UL,
    1707008UL, 1847288UL, 1993688UL, 2146176UL, 2304712UL, 2469240UL,
    2639696UL, 2816000UL, 2998064UL, 3185784UL, 3379048UL, 3577728UL,
    3781688UL, 3990776UL, 4204832UL, 4423680UL, 4647136UL, 4875000UL,
    5107064UL, 5343104UL, 5582888UL, 5826168UL, 6072688UL, 6322176UL,
    6574352UL, 6828920UL, 7085576UL, 7344000UL, 7603864UL, 7864824UL,
    8126528UL, 8388608UL
};

static const uint32_t PlsrPlannerSineIntegralQ24[65] =
{
    0UL, 53UL, 421UL, 1420UL, 3362UL, 6560UL, 11321UL, 17949UL,
    26744UL, 38000UL, 52007UL, 69047UL, 89393UL, 113314UL, 141066UL,
    172899UL, 209052UL, 249753UL, 295221UL, 345662UL, 401269UL,
    462225UL, 528698UL, 600845UL, 678806UL, 762711UL, 852672UL,
    948789UL, 1051146UL, 1159812UL, 1274841UL, 1396271UL, 1524127UL,
    1658415UL, 1799129UL, 1946244UL, 2099722UL, 2259509UL, 2425536UL,
    2597719UL, 2775958UL, 2960141UL, 3150138UL, 3345809UL, 3546997UL,
    3753534UL, 3965237UL, 4181913UL, 4403356UL, 4629347UL, 4859658UL,
    5094050UL, 5332273UL, 5574071UL, 5819175UL, 6067312UL, 6318200UL,
    6571549UL, 6827065UL, 7084448UL, 7343394UL, 7603596UL, 7864741UL,
    8126517UL, 8388608UL
};

static uint32_t PlsrPlannerAbsDifference(uint32_t first, uint32_t second)
{
    return (first > second) ? (first - second) : (second - first);
}

static uint32_t PlsrPlannerRampTime(const PLSR_MOTION_BLOCK *block,uint32_t fromHz, uint32_t toHz)
                                    
                                   
{
    uint32_t baseTimeMs;
    uint64_t durationMs;

    if (fromHz == toHz)
    {
        return 0UL;
    }
    baseTimeMs = (toHz > fromHz) ? block->accelerationTimeMs
                                 : block->decelerationTimeMs;
    if (baseTimeMs == 0UL)
    {
        return 0UL;
    }
    durationMs = ((uint64_t)PlsrPlannerAbsDifference(fromHz, toHz)
                  * baseTimeMs + block->referenceSpeedHz - 1UL)
                 / block->referenceSpeedHz;
    return (durationMs > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL
                                        : (uint32_t)durationMs;
}

static uint16_t PlsrPlannerBaseRampTime(const PLSR_MOTION_BLOCK *block,
                                        uint32_t fromHz,
                                        uint32_t toHz)
{
    if (toHz > fromHz)
    {
        return block->accelerationTimeMs;
    }
    if (toHz < fromHz)
    {
        return block->decelerationTimeMs;
    }
    return 0U;
}

static uint64_t PlsrPlannerRampWeight(uint32_t fromHz,
                                      uint32_t toHz,
                                      uint16_t baseTimeMs)
{
    uint64_t fromSquared = (uint64_t)fromHz * fromHz;
    uint64_t toSquared = (uint64_t)toHz * toHz;
    uint64_t difference = (fromSquared > toSquared)
                              ? (fromSquared - toSquared)
                              : (toSquared - fromSquared);

    return difference * baseTimeMs;
}

static uint64_t PlsrPlannerRequiredPulses(const PLSR_MOTION_BLOCK *block,
                                          uint64_t rampWeight)
{
    uint64_t denominator = (uint64_t)2U * block->referenceSpeedHz * 1000UL;

    return (rampWeight == 0ULL) ? 0ULL
                                : (rampWeight + denominator - 1ULL)
                                      / denominator;
}

static uint32_t PlsrPlannerIntegerSquareRoot(uint64_t value)
{
    uint64_t bit = (uint64_t)1U << 62U;
    uint64_t root = 0ULL;

    while (bit > value)
    {
        bit >>= 2U;
    }
    while (bit != 0ULL)
    {
        if (value >= root + bit)
        {
            value -= root + bit;
            root = (root >> 1U) + bit;
        }
        else
        {
            root >>= 1U;
        }
        bit >>= 2U;
    }
    return (uint32_t)root;
}

static uint32_t PlsrPlannerReachableFrequency(
    const PLSR_MOTION_BLOCK *block,
    uint32_t fromHz,
    uint32_t towardHz,
    uint32_t pulseCount)
{
    uint16_t baseTimeMs = PlsrPlannerBaseRampTime(block, fromHz, towardHz);
    uint64_t frequencySquared = (uint64_t)fromHz * fromHz;
    uint64_t changeSquared;
    uint32_t reachableHz;

    if ((baseTimeMs == 0U) || (fromHz == towardHz))
    {
        return towardHz;
    }
    changeSquared = (uint64_t)2U * pulseCount * block->referenceSpeedHz
                    * 1000UL / baseTimeMs;
    if (towardHz > fromHz)
    {
        reachableHz = PlsrPlannerIntegerSquareRoot(
            frequencySquared + changeSquared);
        return (reachableHz > towardHz) ? towardHz : reachableHz;
    }
    frequencySquared = (changeSquared >= frequencySquared)
                           ? 0ULL : (frequencySquared - changeSquared);
    reachableHz = PlsrPlannerIntegerSquareRoot(frequencySquared);
    if ((uint64_t)reachableHz * reachableHz < frequencySquared)
    {
        reachableHz++;
    }
    return (reachableHz < towardHz) ? towardHz : reachableHz;
}

static uint32_t PlsrPlannerPeak(const PLSR_MOTION_BLOCK *block,
                                uint32_t startHz,
                                uint32_t endHz)
{
    uint32_t targetHz = block->cruiseHz;
    uint32_t upperEndpoint = (startHz > endHz) ? startHz : endHz;
    uint32_t lowerEndpoint = (startHz < endHz) ? startHz : endHz;
    uint16_t entryTimeMs;
    uint16_t exitTimeMs;
    uint32_t timeSumMs;
    uint64_t weightedEndpoints;
    uint64_t availableArea;
    uint64_t peakSquared;
    uint32_t peakHz;

    if ((targetHz <= upperEndpoint) && (targetHz >= lowerEndpoint))
    {
        return targetHz;
    }
    entryTimeMs = PlsrPlannerBaseRampTime(block, startHz, targetHz);
    exitTimeMs = PlsrPlannerBaseRampTime(block, targetHz, endHz);
    timeSumMs = (uint32_t)entryTimeMs + exitTimeMs;
    if (timeSumMs == 0UL)
    {
        return targetHz;
    }
    weightedEndpoints = (uint64_t)startHz * startHz * entryTimeMs
                        + (uint64_t)endHz * endHz * exitTimeMs;
    availableArea = (uint64_t)2U * block->pulseBudget
                    * block->referenceSpeedHz * 1000UL;

    if (targetHz > upperEndpoint)
    {
        peakSquared = (availableArea + weightedEndpoints) / timeSumMs;
        peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);
        if (peakHz < upperEndpoint)
        {
            peakHz = upperEndpoint;
        }
        return (peakHz > targetHz) ? targetHz : peakHz;
    }

    if (availableArea >= weightedEndpoints)
    {
        return targetHz;
    }
    peakSquared = (weightedEndpoints - availableArea) / timeSumMs;
    peakHz = PlsrPlannerIntegerSquareRoot(peakSquared);
    if (peakHz < targetHz)
    {
        peakHz = targetHz;
    }
    return (peakHz > lowerEndpoint) ? lowerEndpoint : peakHz;
}
//C(x)函数
//瞬时速度=delta*C(x)
static uint64_t PlsrPlannerCurveIntegralQ32(uint64_t progressQ32,
                                            uint16_t curveMode)
{
    const uint32_t *table;
    uint64_t scaled;
    uint32_t index;
    uint32_t fraction;
    uint64_t first;
    uint64_t second;

    if (progressQ32 >= PLSR_PLANNER_Q32_ONE)
    {
        return PLSR_PLANNER_Q32_ONE / 2ULL;
    }
    if (curveMode == 0U)
    {
        return (progressQ32 * progressQ32) >> 33U;
    }
    table = (curveMode == 1U) ? PlsrPlannerSmoothIntegralQ24
                              : PlsrPlannerSineIntegralQ24;
    scaled = progressQ32 * 64ULL;
    index = (uint32_t)(scaled >> 32U);
    fraction = (uint32_t)scaled;
    first = (uint64_t)table[index] << 8U;
    second = (uint64_t)table[index + 1UL] << 8U;
    return first + (((second - first) * fraction) >> 32U);
}

static uint64_t PlsrPlannerRampAreaQ32(const PLSR_PLANNER_CONTEXT *context,
                                       uint32_t fromHz,
                                       uint32_t toHz,
                                       uint64_t progressQ32)
{
    int64_t delta = (int64_t)toHz - (int64_t)fromHz;
    int64_t area = (int64_t)((uint64_t)fromHz * progressQ32)
                   + delta * (int64_t)PlsrPlannerCurveIntegralQ32(
                       progressQ32, context->block.curveMode);

    return (uint64_t)area;
}

static uint64_t PlsrPlannerExactBoundaryQ32(
    const PLSR_PLANNER_CONTEXT *context,
    uint64_t previousBoundaryQ32,
    uint64_t targetAreaQ32)
{
    uint64_t lowerQ32 = previousBoundaryQ32;
    uint64_t upperQ32 = PLSR_PLANNER_Q32_ONE;
    uint64_t middleQ32;
    uint32_t iteration;

    for (iteration = 0UL; iteration < 32UL; iteration++)
    {
        middleQ32 = lowerQ32 + ((upperQ32 - lowerQ32) >> 1U);
        if (PlsrPlannerRampAreaQ32(context, context->rampFromHz,
                                   context->rampToHz, middleQ32)
            < targetAreaQ32)
        {
            lowerQ32 = middleQ32;
        }
        else
        {
            upperQ32 = middleQ32;
        }
    }
    return upperQ32;
}

static uint32_t PlsrPlannerInstantFrequency(
    const PLSR_PLANNER_CONTEXT *context,
    uint64_t progressQ32)
{
    const uint32_t *table;
    uint64_t scaled;
    uint64_t curveProgressQ32;
    uint32_t index;
    uint32_t gap;

    if (progressQ32 >= PLSR_PLANNER_Q32_ONE)
    {
        return context->rampToHz;
    }
    if (context->block.curveMode == 0U)
    {
        curveProgressQ32 = progressQ32;
    }
    else
    {
        table = (context->block.curveMode == 1U)
                    ? PlsrPlannerSmoothIntegralQ24
                    : PlsrPlannerSineIntegralQ24;
        scaled = progressQ32 * 64ULL;
        index = (uint32_t)(scaled >> 32U);
        curveProgressQ32 =
            (uint64_t)(table[index + 1UL] - table[index]) << 14U;
    }
    if (context->rampToHz >= context->rampFromHz)
    {
        gap = context->rampToHz - context->rampFromHz;
        return context->rampFromHz
               + (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U);
    }
    gap = context->rampFromHz - context->rampToHz;
    return context->rampFromHz
           - (uint32_t)(((uint64_t)gap * curveProgressQ32) >> 32U);
}

static uint64_t PlsrPlannerPredictedBoundaryQ32(
    const PLSR_PLANNER_CONTEXT *context,
    uint64_t targetAreaQ32)
{
    uint64_t previousQ32 = context->rampBoundaryQ32;
    uint64_t currentAreaQ32 = (previousQ32 == 0ULL)
                                  ? 0ULL
                                  : PlsrPlannerRampAreaQ32(
                                        context, context->rampFromHz,
                                        context->rampToHz, previousQ32);
    uint64_t candidateQ32;
    uint64_t candidateAreaQ32;
    uint64_t differenceQ32;
    uint64_t correctionQ32;
    uint32_t derivativeHz;

    if (context->rampLastPhaseStepQ32
        >= PLSR_PLANNER_Q32_ONE - previousQ32)
    {
        candidateQ32 = PLSR_PLANNER_Q32_ONE;
    }
    else if (context->rampLastPhaseStepQ32 != 0ULL)
    {
        candidateQ32 = previousQ32 + context->rampLastPhaseStepQ32;
    }
    else
    {
        derivativeHz = PlsrPlannerInstantFrequency(context, previousQ32);
        if (derivativeHz == 0UL)
        {
            derivativeHz = 1UL;
        }
        differenceQ32 = targetAreaQ32 - currentAreaQ32;
        correctionQ32 = (differenceQ32 + derivativeHz - 1UL)
                        / derivativeHz;
        candidateQ32 = (correctionQ32
                        >= PLSR_PLANNER_Q32_ONE - previousQ32)
                           ? PLSR_PLANNER_Q32_ONE
                           : previousQ32 + correctionQ32;
    }

    candidateAreaQ32 = PlsrPlannerRampAreaQ32(
        context, context->rampFromHz, context->rampToHz, candidateQ32);
    derivativeHz = PlsrPlannerInstantFrequency(context, candidateQ32);
    if (derivativeHz == 0UL)
    {
        derivativeHz = 1UL;
    }
    if (candidateAreaQ32 < targetAreaQ32)
    {
        differenceQ32 = targetAreaQ32 - candidateAreaQ32;
        correctionQ32 = (differenceQ32 + derivativeHz - 1UL)
                        / derivativeHz;
        candidateQ32 = (correctionQ32
                        >= PLSR_PLANNER_Q32_ONE - candidateQ32)
                           ? PLSR_PLANNER_Q32_ONE
                           : candidateQ32 + correctionQ32;
    }
    else if (candidateAreaQ32 > targetAreaQ32)
    {
        differenceQ32 = candidateAreaQ32 - targetAreaQ32;
        correctionQ32 = differenceQ32 / derivativeHz;
        if (correctionQ32 == 0ULL)
        {
            correctionQ32 = 1ULL;
        }
        candidateQ32 = (correctionQ32 >= candidateQ32 - previousQ32)
                           ? previousQ32 + 1ULL
                           : candidateQ32 - correctionQ32;
    }
    return candidateQ32;
}

static void PlsrPlannerStartRamp(PLSR_PLANNER_CONTEXT *context,
                                 uint8_t rampKind,
                                 uint32_t fromHz,
                                 uint32_t toHz,
                                 uint32_t pulseCount)
{
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    uint32_t totalStartedAt = PlsrPlannerTimingNow();
    uint32_t phaseStartedAt;
#endif

    context->rampKind = rampKind;
    context->rampRelativePulse = 0UL;
    context->rampPulseCount = pulseCount;
    context->rampFromHz = fromHz;
    context->rampToHz = toHz;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    context->rampDurationMs = PlsrPlannerRampTime(&context->block, fromHz, toHz);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTime, phaseStartedAt);
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    context->rampTotalAreaQ32 = PlsrPlannerRampAreaQ32(
        context, fromHz, toHz, PLSR_PLANNER_Q32_ONE);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampTotalArea,
                            phaseStartedAt);
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    context->rampAreaStepQ32 = context->rampTotalAreaQ32 / pulseCount;
    context->rampAreaRemainder =
        (uint32_t)(context->rampTotalAreaQ32 % pulseCount);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampAreaSplit,
                            phaseStartedAt);
#endif
    context->rampRemainderAccumulator = 0UL;
    context->rampTargetAreaQ32 = 0ULL;
    context->rampBoundaryQ32 = 0ULL;
    context->rampActualTimeQ32 = 0ULL;
    context->rampLastPhaseStepQ32 = 0ULL;
    context->lastRampHz = 0UL;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    context->rampFirstBoundaryQ32 = PlsrPlannerExactBoundaryQ32(
        context, 0ULL, context->rampAreaStepQ32);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampFirstBoundary,
                            phaseStartedAt);
#endif
    if (pulseCount > 1UL)
    {
        uint64_t secondTargetAreaQ32;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        phaseStartedAt = PlsrPlannerTimingNow();
#endif
        secondTargetAreaQ32 = context->rampAreaStepQ32 * 2ULL
            + ((uint64_t)context->rampAreaRemainder * 2ULL) / pulseCount;
        context->rampSecondBoundaryQ32 = PlsrPlannerExactBoundaryQ32(
            context, context->rampFirstBoundaryQ32,
            secondTargetAreaQ32);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampSecondBoundary,
                                phaseStartedAt);
#endif
    }
    else
    {
        context->rampSecondBoundaryQ32 = PLSR_PLANNER_Q32_ONE;
    }
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.startRamp, totalStartedAt);
#endif
}

static uint8_t PlsrPlannerSameSetting(
    const PLSR_PLATFORM_TIMER_SETTING *first,
    const PLSR_PLATFORM_TIMER_SETTING *second)
{
    return ((first->actualFrequencyHz == second->actualFrequencyHz)
            && (first->prescaler == second->prescaler)
            && (first->pairPrescaler == second->pairPrescaler)
            && (first->period == second->period)
            && (first->compare == second->compare)) ? 1U : 0U;
}

static uint8_t PlsrPlannerBuildStep(PLSR_PLANNER_CONTEXT *context,
                                    uint32_t requestedHz,
                                    PLSR_PLATFORM_TIMER_SETTING *setting,
                                    uint32_t *normalizedRequestedHz)
{
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    uint32_t startedAt;
    uint8_t result;
#endif

    if (requestedHz == 0UL)
    {
        requestedHz = 1UL;
    }
    if (requestedHz > PLSR_FREQUENCY_MAX_HZ)
    {
        requestedHz = PLSR_FREQUENCY_MAX_HZ;
    }
    *normalizedRequestedHz = requestedHz;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    startedAt = PlsrPlannerTimingNow();
    result = PlsrPlatformBuildTimerSetting(context->block.pulseOutput,
                                           PLSR_OUTPUT_PULSE_DIR,
                                           requestedHz, setting);
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.timerSetting, startedAt);
    return result;
#else
    return PlsrPlatformBuildTimerSetting(context->block.pulseOutput,
                                         PLSR_OUTPUT_PULSE_DIR,
                                         requestedHz, setting);
#endif
}

static uint8_t PlsrPlannerTakeRampStep(
    PLSR_PLANNER_CONTEXT *context,
    PLSR_PLATFORM_TIMER_SETTING *setting,
    uint32_t *requestedFrequencyHz)
{
    uint64_t nextBoundaryQ32;
    uint64_t desiredDeltaQ32;
    uint64_t denominator;
    uint64_t requestedHz;
    uint64_t actualDeltaQ32;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    uint32_t totalStartedAt = PlsrPlannerTimingNow();
    uint32_t phaseStartedAt;
#endif

    context->rampTargetAreaQ32 += context->rampAreaStepQ32;
    context->rampRemainderAccumulator += context->rampAreaRemainder;
    if (context->rampRemainderAccumulator >= context->rampPulseCount)
    {
        context->rampTargetAreaQ32++;
        context->rampRemainderAccumulator -= context->rampPulseCount;
    }
    if (context->rampRelativePulse + 1UL >= context->rampPulseCount)
    {
        context->rampTargetAreaQ32 = context->rampTotalAreaQ32;
        nextBoundaryQ32 = PLSR_PLANNER_Q32_ONE;
    }
    else if (context->rampRelativePulse == 0UL)
    {
        nextBoundaryQ32 = context->rampFirstBoundaryQ32;
    }
    else if (context->rampRelativePulse == 1UL)
    {
        nextBoundaryQ32 = context->rampSecondBoundaryQ32;
    }
    else
    {
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        phaseStartedAt = PlsrPlannerTimingNow();
#endif
        nextBoundaryQ32 = PlsrPlannerPredictedBoundaryQ32(
            context, context->rampTargetAreaQ32);
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampPredictedBoundary,
                                phaseStartedAt);
#endif
    }

    desiredDeltaQ32 = (nextBoundaryQ32 > context->rampActualTimeQ32)
                          ? (nextBoundaryQ32 - context->rampActualTimeQ32)
                          : 1ULL;
    /* The allocated pulse count closes the ramp area exactly.  Derive the
       physical duration from N/averageHz instead of rounding it to whole
       milliseconds; short clipped ramps can be well below 1 ms. */
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    denominator = (uint64_t)context->rampPulseCount * desiredDeltaQ32;
    requestedHz = (denominator == 0ULL)
                      ? context->rampToHz
                      : (context->rampTotalAreaQ32
                         + denominator / 2ULL) / denominator;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampRequestedDivide,
                            phaseStartedAt);
#endif
    if (requestedHz == 0ULL)
    {
        requestedHz = 1ULL;
    }
    if (requestedHz > PLSR_FREQUENCY_MAX_HZ)
    {
        requestedHz = PLSR_FREQUENCY_MAX_HZ;
    }
    if ((context->lastRampHz != 0UL)
        && (((context->rampToHz > context->rampFromHz)
             && (requestedHz < context->lastRampHz))
            || ((context->rampToHz < context->rampFromHz)
                && (requestedHz > context->lastRampHz))))
    {
        requestedHz = context->lastRampHz;
    }
    if (PlsrPlannerBuildStep(context, (uint32_t)requestedHz, setting,
                             requestedFrequencyHz) == 0U)
    {
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep,
                                totalStartedAt);
#endif
        return 0U;
    }

#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    phaseStartedAt = PlsrPlannerTimingNow();
#endif
    denominator = (uint64_t)context->rampPulseCount
                  * setting->actualFrequencyHz;
    actualDeltaQ32 = (denominator == 0ULL)
                         ? desiredDeltaQ32
                         : (context->rampTotalAreaQ32
                            + denominator / 2ULL) / denominator;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampActualDivide,
                            phaseStartedAt);
#endif
    context->rampActualTimeQ32 += actualDeltaQ32;
    context->rampLastPhaseStepQ32 =
        nextBoundaryQ32 - context->rampBoundaryQ32;
    context->rampBoundaryQ32 = nextBoundaryQ32;
    context->lastRampHz = setting->actualFrequencyHz;
    context->rampRelativePulse++;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.rampStep, totalStartedAt);
#endif
    return 1U;
}

static uint8_t PlsrPlannerTakeStep(PLSR_PLANNER_CONTEXT *context,
                                   PLSR_PLATFORM_TIMER_SETTING *setting,
                                   uint32_t *requestedFrequencyHz,
                                   uint32_t *repeatCount)
{
    uint32_t entryEnd = context->entryPulses;
    //加速段结束门槛。已经吐出的脉冲 < entryEnd 还在加速。例如加速 1000，这里就是 1000
    uint32_t steadyEnd = entryEnd + context->steadyPulses;
    //匀速段结束门槛。< steadyEnd 且 ≥ entryEnd 就是匀速。例如再加 5000 匀速，这里就是 6000。再往后是减速
    *repeatCount = 1UL;
    if (context->generatedPulses >= context->block.pulseBudget)
    {
        return 0U;
    }
    if (context->generatedPulses < entryEnd)
    { //加速段 
        if (context->rampKind != 1U)
        {   // 第一次走进加速
            PlsrPlannerStartRamp(context, 1U, context->startHz,context->peakHz, context->entryPulses);                              
        }
        return PlsrPlannerTakeRampStep(context, setting,requestedFrequencyHz);
                                       
    }
    if (context->generatedPulses < steadyEnd)
    {
        *repeatCount = steadyEnd - context->generatedPulses;
        context->rampKind = 0U;
        return PlsrPlannerBuildStep(context, context->peakHz, setting,
                                    requestedFrequencyHz);
    }
    if (context->rampKind != 2U)
    {   // 第一次走进减速
        PlsrPlannerStartRamp(context, 2U, context->peakHz,
                             context->endHz, context->exitPulses);
    }
    return PlsrPlannerTakeRampStep(context, setting,
                                   requestedFrequencyHz);
}

PLSR_PLANNER_STATUS PlsrPlannerBegin(PLSR_PLANNER_CONTEXT *context,
                                     const PLSR_MOTION_BLOCK *block,
                                     uint32_t appliedHz,
                                     uint64_t phasePulses)
{
    uint64_t directRequired;
    uint64_t entryRequired;
    uint64_t exitRequired;
    uint64_t entryWeight;
    uint64_t exitWeight;
    uint64_t totalWeight;
    uint64_t scaledEntry;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    uint32_t startedAt = PlsrPlannerTimingNow();
#endif

    if ((context == NULL) || (block == NULL)
        || (block->pulseBudget == 0UL)
        || (block->referenceSpeedHz == 0UL)
        || (block->referenceSpeedHz > PLSR_FREQUENCY_MAX_HZ)
        || (block->entryHz > PLSR_FREQUENCY_MAX_HZ)
        || (block->cruiseHz == 0UL)
        || (block->cruiseHz > PLSR_FREQUENCY_MAX_HZ)
        || (block->exitHz > PLSR_FREQUENCY_MAX_HZ)
        || (appliedHz > PLSR_FREQUENCY_MAX_HZ)
        || (block->curveMode > 2U) || (block->pulseOutput > 3U))
    {
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);
#endif
        return PLSR_PLANNER_INVALID;
    }

    (void)memset(context, 0, sizeof(*context));
    context->block = *block;
    context->phasePulses = phasePulses;
    context->startHz = (appliedHz != 0UL) ? appliedHz : block->entryHz;
    if (context->startHz == 0UL)
    {
        context->startHz = 1UL;
    }
    context->endHz = (block->exitHz == 0UL) ? 1UL : block->exitHz;

    directRequired = PlsrPlannerRequiredPulses(
        block, PlsrPlannerRampWeight(
                   context->startHz, context->endHz,
                   PlsrPlannerBaseRampTime(block, context->startHz,
                                           context->endHz)));
    if (directRequired > block->pulseBudget)
    {
        context->peakHz = PlsrPlannerReachableFrequency(
            block, context->startHz, context->endHz, block->pulseBudget);
        context->endHz = context->peakHz;
        context->entryPulses = block->pulseBudget;
        context->clipped = 1U;
        context->active = 1U;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);
#endif
        return PLSR_PLANNER_CLIPPED;
    }

    context->peakHz = PlsrPlannerPeak(block, context->startHz,
                                      context->endHz);
    entryWeight = PlsrPlannerRampWeight(
        context->startHz, context->peakHz,
        PlsrPlannerBaseRampTime(block, context->startHz, context->peakHz));
    exitWeight = PlsrPlannerRampWeight(
        context->peakHz, context->endHz,
        PlsrPlannerBaseRampTime(block, context->peakHz, context->endHz));
    entryRequired = PlsrPlannerRequiredPulses(block, entryWeight);
    exitRequired = PlsrPlannerRequiredPulses(block, exitWeight);

    if ((entryRequired + exitRequired) <= block->pulseBudget)
    {
        context->entryPulses = (uint32_t)entryRequired;
        context->exitPulses = (uint32_t)exitRequired;
        context->steadyPulses = block->pulseBudget
                                - context->entryPulses
                                - context->exitPulses;
    }
    else if (entryRequired == 0ULL)
    {
        context->exitPulses = block->pulseBudget;
        context->clipped = 1U;
    }
    else if (exitRequired == 0ULL)
    {
        context->entryPulses = block->pulseBudget;
        context->clipped = 1U;
    }
    else
    {
        totalWeight = entryWeight + exitWeight;
        scaledEntry = ((uint64_t)block->pulseBudget * entryWeight
                       + totalWeight / 2ULL) / totalWeight;
        if (scaledEntry == 0ULL)
        {
            scaledEntry = 1ULL;
        }
        if (scaledEntry >= block->pulseBudget)
        {
            scaledEntry = block->pulseBudget - 1UL;
        }
        context->entryPulses = (uint32_t)scaledEntry;
        context->exitPulses = block->pulseBudget
                              - context->entryPulses;
        context->clipped = 1U;
    }
    if (context->peakHz != block->cruiseHz)
    {
        context->clipped = 1U;
    }
    context->active = 1U;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.begin, startedAt);
#endif
    return (context->clipped != 0U) ? PLSR_PLANNER_CLIPPED
                                    : PLSR_PLANNER_OK;
}
//返回值：实际写了几项（合并后的项数，不是脉冲数）。0 = 没吐出任何东西
uint16_t PlsrPlannerGenerate(PLSR_PLANNER_CONTEXT *context,//规划账本：三段脉冲、已经吐了多少、斜坡面积指针
                             PLSR_STREAM_ITEM *output,//输出数组，调用方准备好的格子
                             uint16_t capacity)//这一次最多往 output 里写几项
{
    PLSR_PLATFORM_TIMER_SETTING setting;
    uint32_t requestedFrequencyHz;
    uint32_t repeatCount;
    uint16_t produced = 0U;
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    uint32_t startedAt = PlsrPlannerTimingNow();
#endif

    if ((context == NULL) || (output == NULL) || (capacity == 0U)
        || (context->active == 0U))
    {
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
        PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt);
#endif
        return 0U;
    }
    while ((produced < capacity)&& (context->generatedPulses < context->block.pulseBudget))
    {  //输出数组还没写满&&本段预算还没全部交给外面。generatedPulses 按脉冲个数计

        if (PlsrPlannerTakeStep(context, &setting, &requestedFrequencyHz, &repeatCount) == 0U)
        //本圈 TakeStep 算出的量化后 PSC/ARR/actualHz，先放栈上                       
        {
            context->active = 0U;//规划器关掉
            break;
        }
        if ((produced != 0U)
            && (PlsrPlannerSameSetting(&output[produced - 1U].setting,&setting) != 0U)                              
            && (output[produced - 1U].requestedFrequencyHz
                == requestedFrequencyHz)
            && (output[produced - 1U].repeatCount
                <= 0xFFFFFFFFUL - repeatCount))
        {
            output[produced - 1U].repeatCount += repeatCount;
            //不用开新格

        }
        else
        {
            output[produced].setting = setting;
            output[produced].requestedFrequencyHz = requestedFrequencyHz;
            output[produced].repeatCount = repeatCount;
            produced++;
        }
        context->generatedPulses += repeatCount;
    }
    if (context->generatedPulses >= context->block.pulseBudget)
    {
        context->active = 0U;
    }
#if defined(PLSR_DEBUG_TIMING) && (PLSR_DEBUG_TIMING != 0) \
    && !defined(PLSR_HOST_TEST)
    PlsrPlannerTimingRecord(&PlsrPlannerTiming.generate, startedAt);
#endif
    return produced;
}

#if !defined(PLSR_HOST_TEST)
static uint32_t PlsrPlannerBenchmarkCyclesNow(void)
{
    return *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT);
}

static void PlsrPlannerBenchmarkEnableCounter(void)
{
    *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DEMCR_ADDRESS) |=
        PLSR_PLANNER_BENCHMARK_TRCENA;
    *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CYCCNT) = 0UL;
    *((volatile uint32_t *)PLSR_PLANNER_BENCHMARK_DWT_CTRL) |=
        PLSR_PLANNER_BENCHMARK_CYCCNTENA;
}

static uint32_t PlsrPlannerBenchmarkCyclesQ16(uint32_t cycles,
                                               uint32_t pulses)
{
    if (pulses == 0UL)
    {
        return 0UL;
    }
    return (uint32_t)((((uint64_t)cycles << 16U) + pulses / 2UL)
                      / pulses);
}

static void PlsrPlannerBenchmarkMode(uint16_t curveMode)
{
    PLSR_MOTION_BLOCK block;
    PLSR_PLANNER_CONTEXT context;
    PLSR_STREAM_ITEM item;
    PLSR_PLANNER_BENCHMARK_RESULT result;
    PLSR_PLANNER_STATUS status;
    uint64_t totalCycles = 0ULL;
    uint32_t minimumBlockQ16 = 0xFFFFFFFFUL;
    uint32_t maximumBlockQ16 = 0UL;
    uint32_t generateCalls = 0UL;
    uint8_t failed = 0U;

    (void)memset(&block, 0, sizeof(block));
    (void)memset(&context, 0, sizeof(context));
    (void)memset(&item, 0, sizeof(item));
    (void)memset(&result, 0, sizeof(result));

    block.entryHz = 1UL;
    block.cruiseHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ;
    block.exitHz = 1UL;
    block.pulseBudget = PLSR_PLANNER_BENCHMARK_PULSES;
    block.referenceSpeedHz = PLSR_PLANNER_BENCHMARK_TARGET_HZ;
    block.accelerationTimeMs = 1000U;
    block.decelerationTimeMs = 1000U;
    block.curveMode = curveMode;
    block.pulseOutput = 0U;
    block.boundary = PLSR_BOUNDARY_STOP;

    status = PlsrPlannerBegin(&context, &block, 0UL, 0ULL);
    result.curveMode = curveMode;
    result.beginStatus = (uint32_t)status;
    if (status == PLSR_PLANNER_OK)
    {
        while (context.active != 0U)
        {
            uint32_t batchCalls = 0UL;
            uint32_t startedAt = PlsrPlannerBenchmarkCyclesNow();
            uint32_t elapsed;
            uint32_t blockQ16;

            while ((batchCalls < PLSR_PLANNER_BENCHMARK_BATCH_CALLS)
                   && (context.active != 0U))
            {
                if (PlsrPlannerGenerate(&context, &item, 1U) == 0U)
                {
                    failed = 1U;
                    break;
                }
                batchCalls++;
            }
            elapsed = PlsrPlannerBenchmarkCyclesNow() - startedAt;
            if (batchCalls != 0UL)
            {
                blockQ16 = PlsrPlannerBenchmarkCyclesQ16(elapsed,
                                                         batchCalls);
                totalCycles += elapsed;
                generateCalls += batchCalls;
                if (blockQ16 < minimumBlockQ16)
                {
                    minimumBlockQ16 = blockQ16;
                }
                if (blockQ16 > maximumBlockQ16)
                {
                    maximumBlockQ16 = blockQ16;
                }
            }
            if (failed != 0U)
            {
                break;
            }
        }
    }

    result.plannedPulses = context.generatedPulses;
    result.generateCalls = generateCalls;
    result.totalCycles = totalCycles;
    result.minimumBlockCyclesPerPulseQ16 =
        (minimumBlockQ16 == 0xFFFFFFFFUL) ? 0UL : minimumBlockQ16;
    result.maximumBlockCyclesPerPulseQ16 = maximumBlockQ16;
    if ((context.generatedPulses != 0UL) && (totalCycles != 0ULL))
    {
        uint64_t averageQ16 = ((totalCycles << 16U)
                               + context.generatedPulses / 2UL)
                              / context.generatedPulses;
        uint64_t estimatedHz =
            ((uint64_t)PlsrPlannerBenchmarkCoreClockHz
             * context.generatedPulses + totalCycles / 2ULL)
            / totalCycles;
        uint64_t targetCyclesQ16 =
            ((uint64_t)PlsrPlannerBenchmarkCoreClockHz << 16U)
            / PLSR_PLANNER_BENCHMARK_TARGET_HZ;

        result.averageCyclesPerPulseQ16 =
            (averageQ16 > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL
                                         : (uint32_t)averageQ16;
        result.estimatedPulsesPerSecond =
            (estimatedHz > 0xFFFFFFFFULL) ? 0xFFFFFFFFUL
                                          : (uint32_t)estimatedHz;
        result.passes100k = (averageQ16 <= targetCyclesQ16) ? 1UL : 0UL;
    }
    result.completed = ((failed == 0U)
                        && (context.generatedPulses
                            == PLSR_PLANNER_BENCHMARK_PULSES)
                        && (context.active == 0U)) ? 1UL : 0UL;
    PlsrPlannerBenchmarkResults[curveMode] = result;
}

void PlsrPlannerBenchmarkService(void)
{
    uint16_t curveMode;

    if ((PlsrPlannerBenchmarkRequest == 0UL)
        || (PlsrPlannerBenchmarkRunning != 0UL))
    {
        return;
    }
    PlsrPlannerBenchmarkRequest = 0UL;
    PlsrPlannerBenchmarkRunning = 1UL;
    (void)memset((void *)PlsrPlannerBenchmarkResults, 0,
                 sizeof(PlsrPlannerBenchmarkResults));
    PlsrPlannerBenchmarkEnableCounter();
    for (curveMode = 0U; curveMode < 3U; curveMode++)
    {
        PlsrPlannerBenchmarkMode(curveMode);
    }
    if (PlsrPlannerBenchmarkRunCount != 0xFFFFFFFFUL)
    {
        PlsrPlannerBenchmarkRunCount++;
    }
    PlsrPlannerBenchmarkRunning = 0UL;
}
#endif

static uint64_t PlsrPlannerRampDurationUs(uint32_t pulseCount,
                                           uint32_t fromHz,
                                           uint32_t toHz)
{
    uint64_t frequencySum = (uint64_t)fromHz + toHz;

    if ((pulseCount == 0UL) || (frequencySum == 0ULL))
    {
        return 0ULL;
    }
    return ((uint64_t)2U * pulseCount * 1000000ULL
            + frequencySum - 1ULL) / frequencySum;
}

/* Return floor(numerator / denominator * 2^32) without requiring a
   128-bit intermediate.  Both operands are bounded by the planner's
   100 kHz Q32 ramp area, so the normalized remainder can be doubled safely. */
static uint32_t PlsrPlannerRatioQ32(uint64_t numerator,
                                    uint64_t denominator)
{
    uint64_t remainder;
    uint32_t ratio = 0UL;
    uint8_t bit;

    if ((numerator == 0ULL) || (denominator == 0ULL))
    {
        return 0UL;
    }
    if (numerator >= denominator)
    {
        return 0xFFFFFFFFUL;
    }
    remainder = numerator;
    for (bit = 0U; bit < 32U; bit++)
    {
        ratio <<= 1U;
        remainder <<= 1U;
        if (remainder >= denominator)
        {
            remainder -= denominator;
            ratio |= 1UL;
        }
    }
    return ratio;
}

static uint32_t PlsrPlannerRampPulsesAtTime(
    const PLSR_PLANNER_CONTEXT *context,
    uint32_t pulseCount,
    uint32_t fromHz,
    uint32_t toHz,
    uint64_t elapsedUs,
    uint64_t durationUs,
    uint64_t *progressQ32)
{
    PLSR_PLANNER_CONTEXT ramp = *context;
    uint64_t partialAreaQ32;
    uint64_t totalAreaQ32;
    uint64_t product;
    uint32_t areaRatioQ32;
    uint32_t result;

    if ((pulseCount == 0UL) || (elapsedUs == 0ULL)
        || (durationUs == 0ULL))
    {
        *progressQ32 = 0ULL;
        return 0UL;
    }
    if (elapsedUs >= durationUs)
    {
        *progressQ32 = PLSR_PLANNER_Q32_ONE;
        return pulseCount;
    }
    *progressQ32 = (elapsedUs * PLSR_PLANNER_Q32_ONE) / durationUs;
    ramp.rampFromHz = fromHz;
    ramp.rampToHz = toHz;
    partialAreaQ32 = PlsrPlannerRampAreaQ32(
        &ramp, fromHz, toHz, *progressQ32);
    totalAreaQ32 = PlsrPlannerRampAreaQ32(
        &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE);
    areaRatioQ32 = PlsrPlannerRatioQ32(partialAreaQ32,
                                       totalAreaQ32);
    product = (uint64_t)pulseCount * areaRatioQ32;
    result = (uint32_t)(product >> 32U);
    if ((uint32_t)product != 0UL)
    {
        result++;
    }
    return (result > pulseCount) ? pulseCount : result;
}

static uint8_t PlsrPlannerSetPredictedFrequency(
    const PLSR_PLANNER_CONTEXT *context,
    uint32_t fromHz,
    uint32_t toHz,
    uint64_t progressQ32,
    PLSR_PLANNER_TIME_PREDICTION *prediction)
{
    PLSR_PLANNER_CONTEXT ramp = *context;
    PLSR_PLATFORM_TIMER_SETTING setting;
    uint32_t requestedHz;

    ramp.rampFromHz = fromHz;
    ramp.rampToHz = toHz;
    requestedHz = PlsrPlannerInstantFrequency(&ramp, progressQ32);
    if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,
                                      PLSR_OUTPUT_PULSE_DIR,
                                      requestedHz, &setting) == 0U)
    {
        return 0U;
    }
    prediction->actualFrequencyHz = setting.actualFrequencyHz;
    return 1U;
}

static uint64_t PlsrPlannerRampTargetAreaQ32(uint64_t totalAreaQ32,
                                             uint32_t pulseCount,
                                             uint32_t pulseIndex)
{
    uint64_t step = totalAreaQ32 / pulseCount;
    uint64_t remainder = totalAreaQ32 % pulseCount;

    return step * pulseIndex
           + (remainder * pulseIndex) / pulseCount;
}

/* Predict the timer setting of the last complete ramp pulse at the deadline.
   A ramp pulse represents the average frequency between two equal-area curve
   boundaries; carrying that run setting is closer to the hardware state than
   carrying the mathematical instantaneous frequency at the boundary. */
static uint8_t PlsrPlannerSetPredictedRampRunFrequency(
    const PLSR_PLANNER_CONTEXT *context,
    uint32_t pulseCount,
    uint32_t fromHz,
    uint32_t toHz,
    uint32_t pulseIndex,
    PLSR_PLANNER_TIME_PREDICTION *prediction)
{
    PLSR_PLANNER_CONTEXT ramp = *context;
    PLSR_PLATFORM_TIMER_SETTING setting;
    uint64_t totalAreaQ32;
    uint64_t previousTargetAreaQ32;
    uint64_t targetAreaQ32;
    uint64_t previousBoundaryQ32;
    uint64_t boundaryQ32;
    uint64_t denominator;
    uint64_t requestedHz;

    if ((pulseCount == 0UL) || (pulseIndex == 0UL))
    {
        return PlsrPlannerSetPredictedFrequency(
            context, fromHz, toHz, 0ULL, prediction);
    }
    if (pulseIndex > pulseCount)
    {
        pulseIndex = pulseCount;
    }
    ramp.rampFromHz = fromHz;
    ramp.rampToHz = toHz;
    totalAreaQ32 = PlsrPlannerRampAreaQ32(
        &ramp, fromHz, toHz, PLSR_PLANNER_Q32_ONE);
    previousTargetAreaQ32 = PlsrPlannerRampTargetAreaQ32(
        totalAreaQ32, pulseCount, pulseIndex - 1UL);
    targetAreaQ32 = PlsrPlannerRampTargetAreaQ32(
        totalAreaQ32, pulseCount, pulseIndex);
    previousBoundaryQ32 = (pulseIndex == 1UL)
                              ? 0ULL
                              : PlsrPlannerExactBoundaryQ32(
                                    &ramp, 0ULL,
                                    previousTargetAreaQ32);
    boundaryQ32 = (pulseIndex == pulseCount)
                      ? PLSR_PLANNER_Q32_ONE
                      : PlsrPlannerExactBoundaryQ32(
                            &ramp, previousBoundaryQ32,
                            targetAreaQ32);
    if (boundaryQ32 <= previousBoundaryQ32)
    {
        return 0U;
    }
    denominator = (uint64_t)pulseCount
                  * (boundaryQ32 - previousBoundaryQ32);
    requestedHz = (denominator == 0ULL)
                      ? toHz
                      : (totalAreaQ32 + denominator / 2ULL)
                            / denominator;
    if (requestedHz == 0ULL)
    {
        requestedHz = 1ULL;
    }
    if (requestedHz > PLSR_FREQUENCY_MAX_HZ)
    {
        requestedHz = PLSR_FREQUENCY_MAX_HZ;
    }
    if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,
                                      PLSR_OUTPUT_PULSE_DIR,
                                      (uint32_t)requestedHz,
                                      &setting) == 0U)
    {
        return 0U;
    }
    prediction->actualFrequencyHz = setting.actualFrequencyHz;
    return 1U;
}

uint8_t PlsrPlannerPredictTime(
    const PLSR_PLANNER_CONTEXT *context,
    uint32_t elapsedUs,
    PLSR_PLANNER_TIME_PREDICTION *prediction)
{
    PLSR_PLATFORM_TIMER_SETTING steadySetting;
    uint64_t remainingUs = elapsedUs;
    uint64_t durationUs;
    uint64_t progressQ32;
    uint64_t partialPulses;

    if ((context == NULL) || (prediction == NULL)
        || (context->block.pulseBudget == 0UL))
    {
        return 0U;
    }
    (void)memset(prediction, 0, sizeof(*prediction));

    durationUs = PlsrPlannerRampDurationUs(
        context->entryPulses, context->startHz, context->peakHz);
    if ((context->entryPulses != 0UL) && (remainingUs <= durationUs))
    {
        prediction->pulseCount = PlsrPlannerRampPulsesAtTime(
            context, context->entryPulses, context->startHz,
            context->peakHz, remainingUs, durationUs, &progressQ32);
        prediction->phase = PLSR_PLANNER_PHASE_ENTRY;
        prediction->deadlineInProfile = 1U;
        return PlsrPlannerSetPredictedRampRunFrequency(
            context, context->entryPulses, context->startHz,
            context->peakHz, prediction->pulseCount, prediction);
    }
    if (context->entryPulses != 0UL)
    {
        remainingUs -= durationUs;
    }

    if (PlsrPlatformBuildTimerSetting(context->block.pulseOutput,
                                      PLSR_OUTPUT_PULSE_DIR,
                                      context->peakHz,
                                      &steadySetting) == 0U)
    {
        return 0U;
    }
    durationUs = (context->steadyPulses == 0UL)
                     ? 0ULL
                     : ((uint64_t)context->steadyPulses * 1000000ULL
                        + steadySetting.actualFrequencyHz - 1UL)
                           / steadySetting.actualFrequencyHz;
    if ((context->steadyPulses != 0UL) && (remainingUs <= durationUs))
    {
        partialPulses = (remainingUs * steadySetting.actualFrequencyHz
                         + 999999ULL) / 1000000ULL;
        if (partialPulses > context->steadyPulses)
        {
            partialPulses = context->steadyPulses;
        }
        prediction->pulseCount = context->entryPulses
                                 + (uint32_t)partialPulses;
        prediction->actualFrequencyHz = steadySetting.actualFrequencyHz;
        prediction->phase = PLSR_PLANNER_PHASE_STEADY;
        prediction->deadlineInProfile = 1U;
        return 1U;
    }
    if (context->steadyPulses != 0UL)
    {
        remainingUs -= durationUs;
    }

    durationUs = PlsrPlannerRampDurationUs(
        context->exitPulses, context->peakHz, context->endHz);
    if ((context->exitPulses != 0UL) && (remainingUs <= durationUs))
    {
        partialPulses = PlsrPlannerRampPulsesAtTime(
            context, context->exitPulses, context->peakHz,
            context->endHz, remainingUs, durationUs, &progressQ32);
        prediction->pulseCount = context->entryPulses
                                 + context->steadyPulses
                                 + (uint32_t)partialPulses;
        prediction->phase = PLSR_PLANNER_PHASE_EXIT;
        prediction->deadlineInProfile = 1U;
        return PlsrPlannerSetPredictedRampRunFrequency(
            context, context->exitPulses, context->peakHz,
            context->endHz, (uint32_t)partialPulses, prediction);
    }

    prediction->pulseCount = context->block.pulseBudget;
    prediction->phase = PLSR_PLANNER_PHASE_COMPLETE;
    prediction->deadlineInProfile = 0U;
    return PlsrPlannerSetPredictedFrequency(
        context, context->endHz, context->endHz,
        PLSR_PLANNER_Q32_ONE, prediction);
}