/**
  ******************************************************************************
  * @file    main_ma600_idiq_pi_lowcurrent_ki8_12p5pct.c
  * @brief   Conservative low-current Id/Iq PI commissioning test
  ******************************************************************************
  * This is an independent safety-test image. It does not replace the verified
  * MA600 closed-loop six-step firmware.
  ******************************************************************************
  */

#include "main.h"
#include "adc.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

#include <stdio.h>
#include <limits.h>

typedef struct
{
  uint16_t u;
  uint16_t v;
  uint16_t w;
} CurrentRaw_t;

typedef struct
{
  uint32_t samples;
  uint32_t errors;
  uint32_t duration_us;
  uint32_t rate_hz;
  uint16_t first_u;
  uint16_t first_v;
  uint16_t first_w;
  uint16_t last_u;
  uint16_t last_v;
  uint16_t last_w;
  uint16_t min_u;
  uint16_t min_v;
  uint16_t min_w;
  uint16_t max_u;
  uint16_t max_v;
  uint16_t max_w;
  uint16_t mean_u;
  uint16_t mean_v;
  uint16_t mean_w;
  uint16_t cnt_min;
  uint16_t cnt_max;
  uint32_t dir_up_samples;
  uint32_t dir_down_samples;
  int32_t sum_delta_min;
  int32_t sum_delta_max;
  int32_t sum_delta_mean;
  uint32_t fault_flags;
} SyncCapture_t;

typedef struct
{
  uint32_t adc_samples;
  uint32_t control_samples;
  uint32_t spi_errors;
  uint32_t overruns;
  uint32_t fault_flags;
  uint16_t raw_start;
  uint16_t raw_previous;
  uint16_t raw_end;
  int32_t net_counts;
  int32_t peak_u;
  int32_t peak_v;
  int32_t peak_w;
  int32_t id_min;
  int32_t id_max;
  int32_t iq_min;
  int32_t iq_max;
  int32_t isum_min;
  int32_t isum_max;
  int64_t id_sum;
  int64_t iq_sum;
  int64_t isum_sum;
  uint32_t duration_us;
  const char *reason;
} VectorRunStats_t;

#define TEST_TIM_PSC                    1U
#define TEST_TIM_ARR                    2124U
#define TEST_TIM_HALF                   1062U
#define TEST_DEADTIME_DTG               0xDFU
#define TEST_BKIN_FILTER                8U
#define TEST_SYNC_SAMPLES               1000U
#define TEST_ZERO_SAMPLES               1024U
#define TEST_POWERED_SAMPLES            2000U
#define TEST_ADC_TIMEOUT_CYCLES         1700000U
#define TEST_ADC_MAX_INTERVAL_CYCLES       12750U
#define TEST_ADC_MISS_CONSECUTIVE              2U
#define TEST_ADC_NORMAL_DELTA           148
#define TEST_ADC_HARD_DELTA             205
#define TEST_ADC_NORMAL_CONSECUTIVE      3U
#define TEST_ADC_ZERO_MIN               1800U
#define TEST_ADC_ZERO_MAX               2300U
#define TEST_ADC_ZERO_SPAN_MAX          64U

#define TEST_FAULT_PB12_LOW             (1UL << 0)
#define TEST_FAULT_TIM1_BIF             (1UL << 1)
#define TEST_FAULT_ADC_TIMEOUT          (1UL << 2)
#define TEST_FAULT_ADC_U                (1UL << 3)
#define TEST_FAULT_ADC_V                (1UL << 4)
#define TEST_FAULT_ADC_W                (1UL << 5)
#define TEST_FAULT_USER_STOP            (1UL << 6)
#define TEST_FAULT_SPI                  (1UL << 7)
#define TEST_FAULT_ENCODER_JUMP         (1UL << 8)
#define TEST_FAULT_WRONG_DIRECTION      (1UL << 9)
#define TEST_FAULT_STALL                (1UL << 10)
#define TEST_FAULT_TIMING               (1UL << 11)

#define VECTOR_MA600_COUNTS             16384U
#define VECTOR_POLE_PAIRS               14U
#define VECTOR_ELECTRICAL_OFFSET        5731
#define VECTOR_QUARTER_TURN             4096
#define VECTOR_PHASE_120                5461
#define VECTOR_MAX_AMPLITUDE_TICKS      212U
#define VECTOR_RUN_ADC_SAMPLES          20000U
#define VECTOR_RAMP_CONTROL_SAMPLES     2500U
#define VECTOR_DIRECTION_CHECK_SAMPLES  2500U
#define VECTOR_STALL_CHECK_SAMPLES      5000U
#define VECTOR_DIRECTION_MIN_COUNTS     64
#define VECTOR_STALL_MIN_COUNTS         64
#define VECTOR_MAX_RAW_STEP             64
#define VECTOR_ENCODER_TEST_SAMPLES     1000U
#define VECTOR_ENCODER_SPAN_LIMIT       64U
#define VECTOR_ADC_TRIGGER_CCR4         1750U
#define CURRENT_AVERAGE_SAMPLES             4U

#define TEST_ALL_CHANNELS (TIM_CCER_CC1E | TIM_CCER_CC1NE | \
                           TIM_CCER_CC2E | TIM_CCER_CC2NE | \
                           TIM_CCER_CC3E | TIM_CCER_CC3NE)

static uint32_t clock_test_status = 0U;
static CurrentRaw_t current_offset = {0U, 0U, 0U};
static uint8_t current_offset_valid = 0U;

void SystemClock_Config(void);
static void GatePins_ForceLowGPIO(void);
static void PowerStage_AllOff(void);
static void Tim1_Prepare20kHz(void);
static HAL_StatusTypeDef Adc_ConfigureTim1Trgo(void);
static HAL_StatusTypeDef SyncEngine_Start(uint8_t enable_outputs);
static void SyncEngine_Stop(void);
static HAL_StatusTypeDef SyncAdc_Read(CurrentRaw_t *raw);
static HAL_StatusTypeDef CaptureSamples(uint32_t requested,
                                        uint8_t enable_outputs,
                                        uint8_t apply_protection,
                                        SyncCapture_t *capture);
static HAL_StatusTypeDef ZeroCalibrate(void);
static void PrintCapture(const char *label, const SyncCapture_t *capture);
static uint8_t CaptureZeroPlausible(const SyncCapture_t *capture);
static uint32_t CurrentFaults(const CurrentRaw_t *raw,
                              uint8_t *over_u,
                              uint8_t *over_v,
                              uint8_t *over_w);
static int32_t AbsI32(int32_t value);
static uint8_t ClockReadyForPoweredTest(void);

int __io_putchar(int ch)
{
  (void)HAL_UART_Transmit(&huart2, (uint8_t *)&ch, 1U, HAL_MAX_DELAY);
  return ch;
}

static int32_t AbsI32(int32_t value)
{
  return (value < 0) ? -value : value;
}

static uint8_t ClockReadyForPoweredTest(void)
{
  return ((clock_test_status == 0U) &&
          (HAL_RCC_GetSysClockFreq() == 170000000U)) ? 1U : 0U;
}

static void GatePins_ForceLowGPIO(void)
{
  GPIO_InitTypeDef gpio = {0};
  const uint16_t high_pins = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10;
  const uint16_t low_pins = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;

  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  HAL_GPIO_WritePin(GPIOA, high_pins, GPIO_PIN_RESET);
  HAL_GPIO_WritePin(GPIOB, low_pins, GPIO_PIN_RESET);

  gpio.Mode = GPIO_MODE_OUTPUT_PP;
  gpio.Pull = GPIO_NOPULL;
  gpio.Speed = GPIO_SPEED_FREQ_LOW;
  gpio.Pin = high_pins;
  HAL_GPIO_Init(GPIOA, &gpio);
  gpio.Pin = low_pins;
  HAL_GPIO_Init(GPIOB, &gpio);
}

static void PowerStage_AllOff(void)
{
  CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);
  TIM1->CCER = 0U;
  CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN);
  TIM1->CCR1 = TEST_TIM_HALF;
  TIM1->CCR2 = TEST_TIM_HALF;
  TIM1->CCR3 = TEST_TIM_HALF;
  TIM1->CNT = 0U;
  TIM1->SR = 0U;
  GatePins_ForceLowGPIO();
}

static void Tim1_Prepare20kHz(void)
{
  PowerStage_AllOff();
  HAL_TIM_MspPostInit(&htim1);

  /*
   * DIR is not software-writable while a center-aligned mode is active.
   * Return to edge-aligned/up-counting first so every restart begins from
   * CNT=0 with a deterministic direction and trigger phase.
   */
  CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN);
  MODIFY_REG(TIM1->CR1, TIM_CR1_CMS | TIM_CR1_DIR, 0U);

  TIM1->PSC = TEST_TIM_PSC;
  TIM1->ARR = TEST_TIM_ARR;
  TIM1->RCR = 1U;
  TIM1->CCR1 = TEST_TIM_HALF;
  TIM1->CCR2 = TEST_TIM_HALF;
  TIM1->CCR3 = TEST_TIM_HALF;

  MODIFY_REG(TIM1->CR1,
             TIM_CR1_CMS | TIM_CR1_CKD | TIM_CR1_ARPE,
             TIM_COUNTERMODE_CENTERALIGNED1 | TIM_CLOCKDIVISION_DIV1);
  /*
   * CH4 PWM2 produces one rising OC4REF edge per center-aligned period.
   * CCR4=1750 preserves the verified 82.35% upper-count sample phase.
   */
  CLEAR_BIT(TIM1->CCER, TIM_CCER_CC4E | TIM_CCER_CC4P);
  MODIFY_REG(TIM1->CCMR2,
             TIM_CCMR2_CC4S | TIM_CCMR2_OC4M | TIM_CCMR2_OC4PE,
             TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1 |
             TIM_CCMR2_OC4M_0 | TIM_CCMR2_OC4PE);
  TIM1->CCR4 = VECTOR_ADC_TRIGGER_CCR4;
  MODIFY_REG(TIM1->CR2, TIM_CR2_MMS, TIM_TRGO_OC4REF);
  MODIFY_REG(TIM1->BDTR, TIM_BDTR_DTG, TEST_DEADTIME_DTG);
  MODIFY_REG(TIM1->BDTR, TIM_BDTR_BKF,
             (TEST_BKIN_FILTER << TIM_BDTR_BKF_Pos));
  SET_BIT(TIM1->BDTR, TIM_BDTR_BKE);
  CLEAR_BIT(TIM1->BDTR, TIM_BDTR_BKP | TIM_BDTR_AOE | TIM_BDTR_MOE);

  TIM1->CCER = 0U;
  TIM1->CNT = 0U;
  TIM1->EGR = TIM_EGR_UG;
  TIM1->SR = 0U;
}

static HAL_StatusTypeDef Adc_ConfigureTim1Trgo(void)
{
  ADC_InjectionConfTypeDef cfg = {0};

  (void)HAL_ADCEx_InjectedStop(&hadc1);

  cfg.InjectedChannel = ADC_CHANNEL_1;
  cfg.InjectedRank = ADC_INJECTED_RANK_1;
  cfg.InjectedSamplingTime = ADC_SAMPLETIME_24CYCLES_5;
  cfg.InjectedSingleDiff = ADC_SINGLE_ENDED;
  cfg.InjectedOffsetNumber = ADC_OFFSET_NONE;
  cfg.InjectedOffset = 0;
  cfg.InjectedNbrOfConversion = 3;
  cfg.InjectedDiscontinuousConvMode = DISABLE;
  cfg.AutoInjectedConv = DISABLE;
  cfg.QueueInjectedContext = DISABLE;
  cfg.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJEC_T1_TRGO;
  cfg.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING;
  cfg.InjecOversamplingMode = DISABLE;

  if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &cfg) != HAL_OK)
  {
    return HAL_ERROR;
  }

  cfg.InjectedChannel = ADC_CHANNEL_2;
  cfg.InjectedRank = ADC_INJECTED_RANK_2;
  if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &cfg) != HAL_OK)
  {
    return HAL_ERROR;
  }

  cfg.InjectedChannel = ADC_CHANNEL_12;
  cfg.InjectedRank = ADC_INJECTED_RANK_3;
  if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &cfg) != HAL_OK)
  {
    return HAL_ERROR;
  }

  return HAL_OK;
}

static HAL_StatusTypeDef SyncEngine_Start(uint8_t enable_outputs)
{
  PowerStage_AllOff();
  Tim1_Prepare20kHz();

  __HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_JEOC | ADC_FLAG_JEOS | ADC_FLAG_OVR);
  if (HAL_ADCEx_InjectedStart(&hadc1) != HAL_OK)
  {
    PowerStage_AllOff();
    return HAL_ERROR;
  }

  if (enable_outputs != 0U)
  {
    if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET)
    {
      (void)HAL_ADCEx_InjectedStop(&hadc1);
      PowerStage_AllOff();
      return HAL_ERROR;
    }
    TIM1->CCER = TEST_ALL_CHANNELS;
    SET_BIT(TIM1->BDTR, TIM_BDTR_MOE);
  }

  SET_BIT(TIM1->CR1, TIM_CR1_CEN);
  return HAL_OK;
}

static void SyncEngine_Stop(void)
{
  CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);
  TIM1->CCER = 0U;
  CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN);
  (void)HAL_ADCEx_InjectedStop(&hadc1);
  PowerStage_AllOff();
}

static HAL_StatusTypeDef SyncAdc_Read(CurrentRaw_t *raw)
{
  uint32_t start_cycle = DWT->CYCCNT;

  while ((ADC1->ISR & ADC_ISR_JEOS) == 0U)
  {
    if ((uint32_t)(DWT->CYCCNT - start_cycle) > TEST_ADC_TIMEOUT_CYCLES)
    {
      return HAL_TIMEOUT;
    }
  }

  raw->u = (uint16_t)ADC1->JDR1;
  raw->v = (uint16_t)ADC1->JDR2;
  raw->w = (uint16_t)ADC1->JDR3;
  ADC1->ISR = ADC_ISR_JEOC | ADC_ISR_JEOS;
  return HAL_OK;
}

static uint32_t CurrentFaults(const CurrentRaw_t *raw,
                              uint8_t *over_u,
                              uint8_t *over_v,
                              uint8_t *over_w)
{
  uint32_t flags = 0U;
  int32_t du = (int32_t)raw->u - (int32_t)current_offset.u;
  int32_t dv = (int32_t)raw->v - (int32_t)current_offset.v;
  int32_t dw = (int32_t)raw->w - (int32_t)current_offset.w;
  int32_t au = AbsI32(du);
  int32_t av = AbsI32(dv);
  int32_t aw = AbsI32(dw);

  if (au >= TEST_ADC_HARD_DELTA)
  {
    flags |= TEST_FAULT_ADC_U;
  }
  else if (au >= TEST_ADC_NORMAL_DELTA)
  {
    if (++(*over_u) >= TEST_ADC_NORMAL_CONSECUTIVE)
    {
      flags |= TEST_FAULT_ADC_U;
    }
  }
  else
  {
    *over_u = 0U;
  }

  if (av >= TEST_ADC_HARD_DELTA)
  {
    flags |= TEST_FAULT_ADC_V;
  }
  else if (av >= TEST_ADC_NORMAL_DELTA)
  {
    if (++(*over_v) >= TEST_ADC_NORMAL_CONSECUTIVE)
    {
      flags |= TEST_FAULT_ADC_V;
    }
  }
  else
  {
    *over_v = 0U;
  }

  if (aw >= TEST_ADC_HARD_DELTA)
  {
    flags |= TEST_FAULT_ADC_W;
  }
  else if (aw >= TEST_ADC_NORMAL_DELTA)
  {
    if (++(*over_w) >= TEST_ADC_NORMAL_CONSECUTIVE)
    {
      flags |= TEST_FAULT_ADC_W;
    }
  }
  else
  {
    *over_w = 0U;
  }

  return flags;
}

static HAL_StatusTypeDef CaptureSamples(uint32_t requested,
                                        uint8_t enable_outputs,
                                        uint8_t apply_protection,
                                        SyncCapture_t *capture)
{
  uint32_t sum_u = 0U;
  uint32_t sum_v = 0U;
  uint32_t sum_w = 0U;
  int32_t sum_delta_acc = 0;
  uint8_t over_u = 0U;
  uint8_t over_v = 0U;
  uint8_t over_w = 0U;
  CurrentRaw_t raw = {0U, 0U, 0U};
  uint32_t start_cycles;
  uint32_t end_cycles;

  *capture = (SyncCapture_t){0};
  capture->min_u = UINT16_MAX;
  capture->min_v = UINT16_MAX;
  capture->min_w = UINT16_MAX;
  capture->cnt_min = UINT16_MAX;
  capture->sum_delta_min = INT32_MAX;
  capture->sum_delta_max = INT32_MIN;

  if (SyncEngine_Start(enable_outputs) != HAL_OK)
  {
    capture->errors = 1U;
    return HAL_ERROR;
  }

  start_cycles = DWT->CYCCNT;

  for (uint32_t i = 0U; i < requested; i++)
  {
    uint16_t cnt;
    int32_t sum_delta;

    if (SyncAdc_Read(&raw) != HAL_OK)
    {
      capture->errors++;
      capture->fault_flags |= TEST_FAULT_ADC_TIMEOUT;
      break;
    }

    cnt = (uint16_t)TIM1->CNT;
    if ((cnt < VECTOR_ADC_TRIGGER_CCR4) ||
        (cnt >= TEST_TIM_ARR) ||
        ((TIM1->CR1 & TIM_CR1_DIR) != 0U))
    {
      capture->fault_flags |= TEST_FAULT_TIMING;
    }
    if ((TIM1->CR1 & TIM_CR1_DIR) != 0U)
    {
      capture->dir_down_samples++;
    }
    else
    {
      capture->dir_up_samples++;
    }

    if (i == 0U)
    {
      capture->first_u = raw.u;
      capture->first_v = raw.v;
      capture->first_w = raw.w;
    }
    capture->last_u = raw.u;
    capture->last_v = raw.v;
    capture->last_w = raw.w;
    if (raw.u < capture->min_u) { capture->min_u = raw.u; }
    if (raw.v < capture->min_v) { capture->min_v = raw.v; }
    if (raw.w < capture->min_w) { capture->min_w = raw.w; }
    if (raw.u > capture->max_u) { capture->max_u = raw.u; }
    if (raw.v > capture->max_v) { capture->max_v = raw.v; }
    if (raw.w > capture->max_w) { capture->max_w = raw.w; }
    if (cnt < capture->cnt_min) { capture->cnt_min = cnt; }
    if (cnt > capture->cnt_max) { capture->cnt_max = cnt; }

    sum_u += raw.u;
    sum_v += raw.v;
    sum_w += raw.w;
    sum_delta = ((int32_t)raw.u - (int32_t)current_offset.u) +
                ((int32_t)raw.v - (int32_t)current_offset.v) +
                ((int32_t)raw.w - (int32_t)current_offset.w);
    sum_delta_acc += sum_delta;
    if (sum_delta < capture->sum_delta_min) { capture->sum_delta_min = sum_delta; }
    if (sum_delta > capture->sum_delta_max) { capture->sum_delta_max = sum_delta; }

    capture->samples++;

    if ((enable_outputs != 0U) &&
        (__HAL_UART_GET_FLAG(&huart2, UART_FLAG_RXNE) != RESET))
    {
      uint8_t stop_rx = (uint8_t)huart2.Instance->RDR;
      if ((stop_rx == 'X') || (stop_rx == 'x'))
      {
        capture->fault_flags |= TEST_FAULT_USER_STOP;
      }
    }

    if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET)
    {
      capture->fault_flags |= TEST_FAULT_PB12_LOW;
    }
    if ((TIM1->SR & TIM_SR_BIF) != 0U)
    {
      capture->fault_flags |= TEST_FAULT_TIM1_BIF;
    }
    if ((apply_protection != 0U) && (current_offset_valid != 0U))
    {
      capture->fault_flags |= CurrentFaults(&raw, &over_u, &over_v, &over_w);
    }

    if (capture->fault_flags != 0U)
    {
      break;
    }
  }

  end_cycles = DWT->CYCCNT;
  SyncEngine_Stop();

  if (capture->samples != 0U)
  {
    capture->mean_u = (uint16_t)(sum_u / capture->samples);
    capture->mean_v = (uint16_t)(sum_v / capture->samples);
    capture->mean_w = (uint16_t)(sum_w / capture->samples);
    capture->sum_delta_mean = sum_delta_acc / (int32_t)capture->samples;
  }
  capture->duration_us = (uint32_t)(end_cycles - start_cycles) / 170U;
  if (capture->duration_us != 0U)
  {
    capture->rate_hz = (capture->samples * 1000000U) / capture->duration_us;
  }

  return ((capture->samples == requested) &&
          (capture->errors == 0U) &&
          (capture->fault_flags == 0U)) ? HAL_OK : HAL_ERROR;
}

static HAL_StatusTypeDef ZeroCalibrate(void)
{
  SyncCapture_t capture;
  HAL_StatusTypeDef status;

  current_offset = (CurrentRaw_t){0U, 0U, 0U};
  current_offset_valid = 0U;
  status = CaptureSamples(TEST_ZERO_SAMPLES, 0U, 0U, &capture);
  if (status != HAL_OK)
  {
    PrintCapture("ZERO FAIL", &capture);
    return HAL_ERROR;
  }

  PrintCapture("ZERO SYNC", &capture);
  if (CaptureZeroPlausible(&capture) == 0U)
  {
    printf("ZERO PLAUSIBILITY FAIL: mean must be %u..%u and each span <=%u counts.\r\n",
           TEST_ADC_ZERO_MIN, TEST_ADC_ZERO_MAX, TEST_ADC_ZERO_SPAN_MAX);
    return HAL_ERROR;
  }
  current_offset.u = capture.mean_u;
  current_offset.v = capture.mean_v;
  current_offset.w = capture.mean_w;
  current_offset_valid = 1U;
  return HAL_OK;
}

static uint8_t CaptureZeroPlausible(const SyncCapture_t *capture)
{
  if (capture == NULL) { return 0U; }
  if ((capture->mean_u < TEST_ADC_ZERO_MIN) ||
      (capture->mean_u > TEST_ADC_ZERO_MAX) ||
      (capture->mean_v < TEST_ADC_ZERO_MIN) ||
      (capture->mean_v > TEST_ADC_ZERO_MAX) ||
      (capture->mean_w < TEST_ADC_ZERO_MIN) ||
      (capture->mean_w > TEST_ADC_ZERO_MAX))
  {
    return 0U;
  }
  if (((uint16_t)(capture->max_u - capture->min_u) >
       TEST_ADC_ZERO_SPAN_MAX) ||
      ((uint16_t)(capture->max_v - capture->min_v) >
       TEST_ADC_ZERO_SPAN_MAX) ||
      ((uint16_t)(capture->max_w - capture->min_w) >
       TEST_ADC_ZERO_SPAN_MAX))
  {
    return 0U;
  }
  return 1U;
}

static void PrintCapture(const char *label, const SyncCapture_t *capture)
{
  printf("%s: samples=%lu errors=%lu duration=%lu us rate=%lu Hz flags=0x%02lX PB12_NOW=%s BIF_LATCHED=%lu\r\n",
         label,
         capture->samples,
         capture->errors,
         capture->duration_us,
         capture->rate_hz,
         capture->fault_flags,
         (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) == GPIO_PIN_SET) ? "HIGH" : "LOW",
         ((capture->fault_flags & TEST_FAULT_TIM1_BIF) != 0U) ? 1UL : 0UL);
  printf("ADC U mean=%u min=%u max=%u span=%u first=%u last=%u\r\n",
         capture->mean_u, capture->min_u, capture->max_u,
         (uint16_t)(capture->max_u - capture->min_u),
         capture->first_u, capture->last_u);
  printf("ADC V mean=%u min=%u max=%u span=%u first=%u last=%u\r\n",
         capture->mean_v, capture->min_v, capture->max_v,
         (uint16_t)(capture->max_v - capture->min_v),
         capture->first_v, capture->last_v);
  printf("ADC W mean=%u min=%u max=%u span=%u first=%u last=%u\r\n",
         capture->mean_w, capture->min_w, capture->max_w,
         (uint16_t)(capture->max_w - capture->min_w),
         capture->first_w, capture->last_w);
  printf("TRIGGER CNT after JEOS min=%u max=%u direction_samples up=%lu down=%lu\r\n",
         capture->cnt_min, capture->cnt_max,
         capture->dir_up_samples, capture->dir_down_samples);
  if (current_offset_valid != 0U)
  {
    printf("I_SUM delta counts mean=%ld min=%ld max=%ld; zero U=%u V=%u W=%u\r\n",
           capture->sum_delta_mean,
           capture->sum_delta_min,
           capture->sum_delta_max,
           current_offset.u, current_offset.v, current_offset.w);
  }
}

static const int16_t vector_sin_lut_48[48] = {
      0,   4277,   8481,  12539,  16383,  19947,  23170,  25996,
  28377,  30273,  31650,  32487,  32767,  32487,  31650,  30273,
  28377,  25996,  23170,  19947,  16383,  12539,   8481,   4277,
      0,  -4277,  -8481, -12539, -16383, -19947, -23170, -25996,
 -28377, -30273, -31650, -32487, -32767, -32487, -31650, -30273,
 -28377, -25996, -23170, -19947, -16384, -12539,  -8481,  -4277
};

static uint16_t Vector_WrapRaw14(int32_t value)
{
  return (uint16_t)((uint32_t)value & 0x3FFFU);
}

static int32_t Vector_WrappedDelta(uint16_t current, uint16_t previous)
{
  int32_t delta = (int32_t)current - (int32_t)previous;
  if (delta > 8192) { delta -= 16384; }
  if (delta < -8192) { delta += 16384; }
  return delta;
}

static int16_t Vector_SinQ15(uint16_t angle)
{
  uint32_t scaled = (uint32_t)(angle & 0x3FFFU) * 48U;
  uint32_t index = scaled >> 14U;
  uint32_t fraction = scaled & 0x3FFFU;
  int32_t y0 = vector_sin_lut_48[index];
  int32_t y1 = vector_sin_lut_48[(index + 1U) % 48U];
  return (int16_t)(y0 + (((y1 - y0) * (int32_t)fraction) >> 14U));
}

static uint16_t Vector_ElectricalAngle(uint16_t mechanical_raw)
{
  return Vector_WrapRaw14(VECTOR_ELECTRICAL_OFFSET -
                          ((int32_t)VECTOR_POLE_PAIRS *
                           (int32_t)mechanical_raw));
}

static HAL_StatusTypeDef Vector_MA600Read(uint16_t *raw)
{
  uint8_t tx[2] = {0U, 0U};
  uint8_t rx[2] = {0U, 0U};
  HAL_StatusTypeDef status;

  if (raw == NULL) { return HAL_ERROR; }
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
  __NOP(); __NOP(); __NOP(); __NOP();
  status = HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2U, 2U);
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
  if (status != HAL_OK) { return status; }
  *raw = (uint16_t)((((uint16_t)rx[0] << 8U) | rx[1]) >> 2U);
  *raw &= 0x3FFFU;
  return HAL_OK;
}

static uint16_t Vector_ClampCompare(int32_t compare)
{
  if (compare < 1) { return 1U; }
  if (compare >= (int32_t)TEST_TIM_ARR) { return (uint16_t)(TEST_TIM_ARR - 1U); }
  return (uint16_t)compare;
}

static int32_t Vector_CountsToMilliamp(int32_t counts)
{
  return (int32_t)(((int64_t)counts * 12207LL) / 1000LL);
}

static int32_t Vector_PhaseCurrent(uint16_t raw, uint16_t zero)
{
  /* MCS1823 low-side sensors were verified: positive phase current lowers raw. */
  return (int32_t)zero - (int32_t)raw;
}

static HAL_StatusTypeDef Vector_MA600BusOffTest(void)
{
  uint16_t first = 0U;
  uint16_t last = 0U;
  int32_t min_delta = 0;
  int32_t max_delta = 0;
  uint32_t samples = 0U;
  uint32_t errors = 0U;
  uint32_t overruns = 0U;
  uint32_t period_cycles = SystemCoreClock / 5000U;
  uint32_t deadline = DWT->CYCCNT;

  PowerStage_AllOff();
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
  for (uint32_t i = 0U; i < VECTOR_ENCODER_TEST_SAMPLES; i++)
  {
    uint16_t raw = 0U;
    deadline += period_cycles;
    if (Vector_MA600Read(&raw) != HAL_OK)
    {
      errors++;
    }
    else if (samples == 0U)
    {
      first = raw;
      last = raw;
      samples = 1U;
    }
    else
    {
      int32_t delta = Vector_WrappedDelta(raw, first);
      last = raw;
      if (delta < min_delta) { min_delta = delta; }
      if (delta > max_delta) { max_delta = delta; }
      samples++;
    }
    if ((int32_t)(deadline - DWT->CYCCNT) > 0)
    {
      while ((int32_t)(deadline - DWT->CYCCNT) > 0) { __NOP(); }
    }
    else
    {
      overruns++;
      deadline = DWT->CYCCNT;
    }
  }
  PowerStage_AllOff();
  uint32_t span = (uint32_t)(max_delta - min_delta);
  HAL_StatusTypeDef result = ((samples == VECTOR_ENCODER_TEST_SAMPLES) &&
                              (errors == 0U) && (overruns == 0U) &&
                              (span <= VECTOR_ENCODER_SPAN_LIMIT)) ? HAL_OK : HAL_ERROR;
  printf("MA600 5KHZ BUS-OFF: samples=%lu errors=%lu overruns=%lu first=%u last=%u span=%lu result=%s\r\n",
         samples, errors, overruns, first, last, span,
         (result == HAL_OK) ? "PASS" : "FAIL");
  return result;
}
#define SWEEP_STAGE_COUNT                 2U
#define SWEEP_ZERO_CONTROL_SAMPLES      250U
#define SWEEP_POSITION_CONTROL_SAMPLES 51000U
#define SWEEP_TRACE_SAMPLES              10U

#define POSITION_STAGE_ZERO               0U
#define POSITION_STAGE_ACTIVE             1U

#define POSITION_MAX_OUTPUT_DEG           360
#define POSITION_GEAR_RATIO                8
#define POSITION_OUTER_DIVIDER             5U
#define POSITION_SPEED_LIMIT_RPM_X10     6000
#define POSITION_SPEED_ACCEL_SLEW_RPM_X10  60
#define POSITION_SPEED_DECEL_SLEW_RPM_X10 100
#define POSITION_KP_RPM_X10_Q8            224
#define POSITION_DEADBAND_COUNTS          364
#define POSITION_BRAKE_180DEG_COUNTS    65536
#define POSITION_BRAKE_90DEG_COUNTS     32768
#define POSITION_BRAKE_45DEG_COUNTS     16384
#define POSITION_BRAKE_15DEG_COUNTS      5461
#define POSITION_BRAKE_5DEG_COUNTS       1820
#define POSITION_BRAKE_180DEG_RPM_X10    4000
#define POSITION_BRAKE_90DEG_RPM_X10     2500
#define POSITION_BRAKE_45DEG_RPM_X10     1200
#define POSITION_BRAKE_15DEG_RPM_X10      500
#define POSITION_BRAKE_5DEG_RPM_X10       200
#define POSITION_SETTLE_SPEED_RPM_X10      20
#define POSITION_SETTLE_UPDATES            30U
#define POSITION_MOTION_CHECK_CONTROLS   5000U
#define POSITION_MOTION_MIN_COUNTS         16
#define POSITION_GUARD_MARGIN_COUNTS    21845
#define POSITION_TIMEOUT_BASE_CONTROLS  25000U
#define POSITION_TIMEOUT_PER_DEG_CONTROLS 500U
#define ANGLE_ENTRY_TIMEOUT_MS          10000U
#define ANGLE_AUTO_COMMIT_MS              200U

#define SPEED_COMMAND_MAX_OUTPUT_RPM       75
#define SPEED_COMMAND_RUN_CONTROLS      50000U
#define SPEED_COMMAND_ACCEL_SLEW_RPM_X10    2
#define SPEED_COMMAND_DECEL_SLEW_RPM_X10   10

/* 50 samples at 5 kHz = about 9.98 ms. Raw decreases for forward. */
#define SPEED_WINDOW_SAMPLES              50U
#define SPEED_LOOP_DIVIDER                10U
#define SPEED_WINDOW_US                 9978U
/* 600 rpm was validated; 700 rpm remains the independent hard trip. */
#define SPEED_HARD_LIMIT_RPM_X10        7000
#define SPEED_HARD_LIMIT_COUNTS          1910
#define SPEED_KP_Q8                        26
#define SPEED_KI_Q16                        8
#define SPEED_INTEGRAL_LIMIT_COUNTS        30
#define SPEED_INTEGRAL_LIMIT_Q16 \
  ((int64_t)SPEED_INTEGRAL_LIMIT_COUNTS << 16U)
#define SPEED_IQ_FAR_LIMIT_COUNTS          82
#define SPEED_IQ_NEAR_LIMIT_COUNTS         41
#define SPEED_IQ_RISE_SLEW_Q8              96
#define SPEED_IQ_RELEASE_SLEW_Q8          512
#define SPEED_FRICTION_FF_COUNTS            12
#define SPEED_FRICTION_FADE_START_RPM_X10  100
#define SPEED_FRICTION_FADE_END_RPM_X10    200
#define SPEED_STARTUP_BOOST_COUNTS          20
#define SPEED_STARTUP_ARM_RPM_X10           20
#define SPEED_STARTUP_RELEASE_RPM_X10       60
#define SPEED_STARTUP_RELEASE_CONFIRM_UPDATES 25U
#define SPEED_STARTUP_MAX_UPDATES          250U
#define SWEEP_MIN_MOTION_COUNTS            64
#define SWEEP_FAULT_SPEED               (1UL << 12)

#define PI_ID_REF_COUNTS                   0
#define PI_KP_Q8                         192
#define PI_KI_Q8                           4
#define PI_OUTPUT_LIMIT_TICKS            850
#define PI_INTEGRAL_LIMIT_Q8  (PI_OUTPUT_LIMIT_TICKS << 8)
#define PI_INTEGRAL_DEADBAND_COUNTS        2
#define PI_ZERO_LEAK_SHIFT                 8

#define PI_ACTION_ID_DEADBAND       (1U << 0)
#define PI_ACTION_IQ_DEADBAND       (1U << 1)
#define PI_ACTION_ID_LEAK           (1U << 2)
#define PI_ACTION_IQ_LEAK           (1U << 3)

typedef struct
{
  uint32_t n;
  int64_t id_sum;
  int64_t iq_sum;
  int64_t isum_sum;
  int32_t id_min;
  int32_t id_max;
  int32_t iq_min;
  int32_t iq_max;
  int32_t isum_min;
  int32_t isum_max;
} SweepWindowStats_t;

typedef struct
{
  const char *name;
  uint32_t target_controls;
  uint32_t controls;
  uint32_t first_control_cycle;
  uint32_t last_control_cycle;
  uint16_t raw_start;
  uint16_t raw_end;
  int32_t net_counts;
  SweepWindowStats_t all;
  SweepWindowStats_t transient_0_2ms;
  SweepWindowStats_t early_2_20ms;
  SweepWindowStats_t steady_second_half;
  int32_t trace_iq[SWEEP_TRACE_SAMPLES];
  uint32_t trace_count;
  int64_t vd_sum;
  int64_t vq_sum;
  int32_t vd_min;
  int32_t vd_max;
  int32_t vq_min;
  int32_t vq_max;
  uint32_t saturation_controls;
  uint32_t id_deadband_controls;
  uint32_t iq_deadband_controls;
  uint32_t id_leak_controls;
  uint32_t iq_leak_controls;
  int64_t speed_ref_sum;
  int64_t speed_measured_sum;
  int64_t iq_ref_sum;
  int64_t speed_p_sum;
  int64_t speed_i_sum;
  int64_t friction_ff_sum;
  int32_t speed_ref_min;
  int32_t speed_ref_max;
  int32_t speed_measured_min;
  int32_t speed_measured_max;
  int32_t iq_ref_min;
  int32_t iq_ref_max;
  int32_t speed_p_min;
  int32_t speed_p_max;
  int32_t speed_i_min;
  int32_t speed_i_max;
  int32_t friction_ff_min;
  int32_t friction_ff_max;
  uint32_t speed_aw_updates;
  uint32_t speed_integral_limit_updates;
  uint32_t speed_integral_reset_updates;
  uint32_t startup_boost_updates;
  uint32_t startup_release_confirm_max;
  uint32_t startup_release_speed_events;
  uint32_t startup_release_timeout_events;
} SweepStageStats_t;

typedef struct
{
  uint32_t adc_samples;
  uint32_t control_samples;
  uint32_t spi_errors;
  uint32_t timing_overruns;
  uint32_t fault_flags;
  uint16_t raw_start;
  uint16_t raw_previous;
  uint16_t raw_end;
  int32_t net_counts;
  int32_t peak_u;
  int32_t peak_v;
  int32_t peak_w;
  uint32_t duration_us;
  const char *reason;
  int32_t id_integral_q8_final;
  int32_t iq_integral_q8_final;
  int64_t speed_integral_q16_final;
  int32_t target_position_counts;
  int32_t final_position_counts;
  int32_t final_error_counts;
  uint32_t settle_updates_max;
  uint32_t settle_control;
  int32_t settle_position_counts;
  int32_t settle_error_counts;
  int32_t settle_speed_rpm_x10;
  uint8_t settled;
  uint8_t control_mode;
  int32_t target_output_rpm;
  SweepStageStats_t stage[SWEEP_STAGE_COUNT];
} SweepRunStats_t;

typedef struct
{
  int32_t id_integral_q8;
  int32_t iq_integral_q8;
} CurrentPiState_t;

typedef enum
{
  CONTROL_MODE_NONE = 0,
  CONTROL_MODE_POSITION = 1,
  CONTROL_MODE_SPEED = 2
} ControlMode_t;

static const uint32_t sweep_target_controls[SWEEP_STAGE_COUNT] =
{
  SWEEP_ZERO_CONTROL_SAMPLES,
  SWEEP_POSITION_CONTROL_SAMPLES
};

static const char *const sweep_stage_name[SWEEP_STAGE_COUNT] =
{
  "ZERO_50MS",
  "POSITION_ACTIVE_DYNAMIC_TIMEOUT"
};

static void SweepWindow_Init(SweepWindowStats_t *stats)
{
  *stats = (SweepWindowStats_t){0};
  stats->id_min = INT32_MAX;
  stats->id_max = INT32_MIN;
  stats->iq_min = INT32_MAX;
  stats->iq_max = INT32_MIN;
  stats->isum_min = INT32_MAX;
  stats->isum_max = INT32_MIN;
}

static void SweepWindow_Add(SweepWindowStats_t *stats,
                            int32_t id,
                            int32_t iq,
                            int32_t isum)
{
  stats->n++;
  stats->id_sum += id;
  stats->iq_sum += iq;
  stats->isum_sum += isum;
  if (id < stats->id_min) { stats->id_min = id; }
  if (id > stats->id_max) { stats->id_max = id; }
  if (iq < stats->iq_min) { stats->iq_min = iq; }
  if (iq > stats->iq_max) { stats->iq_max = iq; }
  if (isum < stats->isum_min) { stats->isum_min = isum; }
  if (isum > stats->isum_max) { stats->isum_max = isum; }
}

static void SweepStage_Init(SweepStageStats_t *stage,
                            const char *name,
                            uint32_t controls,
                            uint16_t raw_start)
{
  *stage = (SweepStageStats_t){0};
  stage->name = name;
  stage->target_controls = controls;
  stage->raw_start = raw_start;
  stage->raw_end = raw_start;
  stage->vd_min = INT32_MAX;
  stage->vd_max = INT32_MIN;
  stage->vq_min = INT32_MAX;
  stage->vq_max = INT32_MIN;
  stage->speed_ref_min = INT32_MAX;
  stage->speed_ref_max = INT32_MIN;
  stage->speed_measured_min = INT32_MAX;
  stage->speed_measured_max = INT32_MIN;
  stage->iq_ref_min = INT32_MAX;
  stage->iq_ref_max = INT32_MIN;
  stage->speed_p_min = INT32_MAX;
  stage->speed_p_max = INT32_MIN;
  stage->speed_i_min = INT32_MAX;
  stage->speed_i_max = INT32_MIN;
  stage->friction_ff_min = INT32_MAX;
  stage->friction_ff_max = INT32_MIN;
  SweepWindow_Init(&stage->all);
  SweepWindow_Init(&stage->transient_0_2ms);
  SweepWindow_Init(&stage->early_2_20ms);
  SweepWindow_Init(&stage->steady_second_half);
}

static int32_t Position_OutputDegreesToMotorCounts(int32_t degrees)
{
  return (int32_t)(((int64_t)degrees * VECTOR_MA600_COUNTS *
                    POSITION_GEAR_RATIO + 180LL) / 360LL);
}

static int32_t Position_IqLimitCounts(int32_t error_counts)
{
  /* Use 1 A only while more than 5 output degrees remain. */
  return (AbsI32(error_counts) > POSITION_BRAKE_5DEG_COUNTS) ?
      SPEED_IQ_FAR_LIMIT_COUNTS : SPEED_IQ_NEAR_LIMIT_COUNTS;
}
static int32_t Position_SpeedReferenceRpmX10(int32_t error_counts)
{
  int32_t abs_error = AbsI32(error_counts);
  int32_t braking_limit = POSITION_SPEED_LIMIT_RPM_X10;
  int32_t speed_ref;
  if (abs_error <= POSITION_DEADBAND_COUNTS)
  {
    return 0;
  }

  /*
   * Distance-aware output-side braking envelope. The 600 rpm ceiling is
   * reachable only while ample travel remains; every smaller distance band
   * lowers the motor-speed command before the final +/-1 output-deg window.
   */
  if (abs_error <= POSITION_BRAKE_5DEG_COUNTS)
  {
    braking_limit = POSITION_BRAKE_5DEG_RPM_X10;
  }
  else if (abs_error <= POSITION_BRAKE_15DEG_COUNTS)
  {
    braking_limit = POSITION_BRAKE_15DEG_RPM_X10;
  }
  else if (abs_error <= POSITION_BRAKE_45DEG_COUNTS)
  {
    braking_limit = POSITION_BRAKE_45DEG_RPM_X10;
  }
  else if (abs_error <= POSITION_BRAKE_90DEG_COUNTS)
  {
    braking_limit = POSITION_BRAKE_90DEG_RPM_X10;
  }
  else if (abs_error <= POSITION_BRAKE_180DEG_COUNTS)
  {
    braking_limit = POSITION_BRAKE_180DEG_RPM_X10;
  }

  speed_ref = (int32_t)(((int64_t)error_counts *
                         POSITION_KP_RPM_X10_Q8) / 256LL);
  if (speed_ref > braking_limit)
  {
    return braking_limit;
  }
  if (speed_ref < -braking_limit)
  {
    return -braking_limit;
  }
  return speed_ref;
}
static int32_t Position_SlewSpeedReference(int32_t current_rpm_x10,
                                           int32_t target_rpm_x10)
{
  int32_t delta = target_rpm_x10 - current_rpm_x10;
  int32_t slew = POSITION_SPEED_DECEL_SLEW_RPM_X10;

  /*
   * Keep outward acceleration gentle.  A reduction in magnitude, a zero
   * target, or a requested reversal is braking and must release ten times
   * faster so the old positive speed command cannot carry the rotor through
   * the position target.
   */
  if (((current_rpm_x10 == 0) && (target_rpm_x10 != 0)) ||
      (((current_rpm_x10 > 0) == (target_rpm_x10 > 0)) &&
       (AbsI32(target_rpm_x10) > AbsI32(current_rpm_x10))))
  {
    slew = POSITION_SPEED_ACCEL_SLEW_RPM_X10;
  }

  if (delta > slew)
  {
    delta = slew;
  }
  else if (delta < -slew)
  {
    delta = -slew;
  }
  return current_rpm_x10 + delta;
}

static int32_t SpeedCommand_SlewReference(int32_t current_rpm_x10,
                                          int32_t target_rpm_x10)
{
  int32_t delta = target_rpm_x10 - current_rpm_x10;
  int32_t slew = SPEED_COMMAND_ACCEL_SLEW_RPM_X10;

  if ((target_rpm_x10 == 0) ||
      (((current_rpm_x10 > 0) != (target_rpm_x10 > 0)) &&
       (current_rpm_x10 != 0)) ||
      (AbsI32(target_rpm_x10) < AbsI32(current_rpm_x10)))
  {
    slew = SPEED_COMMAND_DECEL_SLEW_RPM_X10;
  }
  if (delta > slew) { delta = slew; }
  if (delta < -slew) { delta = -slew; }
  return current_rpm_x10 + delta;
}
static int32_t Speed_Clamp(int32_t value, int32_t limit)
{
  if (value > limit) { return limit; }
  if (value < -limit) { return -limit; }
  return value;
}

static int32_t Speed_SlewIqReferenceQ8(int32_t current_q8,
                                      int32_t target_counts,
                                      int8_t direction)
{
  int32_t target_q8 = target_counts * 256;
  int32_t delta = target_q8 - current_q8;
  int32_t signed_current_q8 = (int32_t)direction * current_q8;
  int32_t signed_target_q8 = (int32_t)direction * target_q8;
  int32_t slew_q8 = SPEED_IQ_RISE_SLEW_Q8;

  /*
   * Keep propulsion torque application gentle, but remove existing torque or
   * cross into commanded braking four times faster.  This prevents the old
   * symmetric slew limiter from holding positive Iq during an overspeed.
   */
  if (((signed_current_q8 > 0) &&
       (signed_target_q8 < signed_current_q8)) ||
      ((signed_current_q8 < 0) &&
       (signed_target_q8 > signed_current_q8)))
  {
    slew_q8 = SPEED_IQ_RELEASE_SLEW_Q8;
  }
  if (delta > slew_q8) { delta = slew_q8; }
  if (delta < -slew_q8) { delta = -slew_q8; }
  return current_q8 + delta;
}

static int32_t Speed_FrictionFeedforward(uint32_t stage_index,
                                         int32_t ref_rpm_x10,
                                         int32_t measured_rpm_x10,
                                         uint8_t *startup_state,
                                         uint32_t *startup_updates,
                                         uint8_t *startup_active,
                                         uint32_t *release_confirm_updates,
                                         uint8_t *release_reason)
{
  int32_t abs_ref = AbsI32(ref_rpm_x10);
  int32_t magnitude = 0;
  int32_t sign = (ref_rpm_x10 >= 0) ? 1 : -1;

  *startup_active = 0U;
  *release_reason = 0U;
  if ((ref_rpm_x10 == 0) ||
      (stage_index == POSITION_STAGE_ZERO))
  {
    return 0;
  }

  if (abs_ref <= SPEED_FRICTION_FADE_START_RPM_X10)
  {
    magnitude = SPEED_FRICTION_FF_COUNTS;
  }
  else if (abs_ref < SPEED_FRICTION_FADE_END_RPM_X10)
  {
    magnitude = (int32_t)(
        ((int64_t)SPEED_FRICTION_FF_COUNTS *
         (SPEED_FRICTION_FADE_END_RPM_X10 - abs_ref)) /
        (SPEED_FRICTION_FADE_END_RPM_X10 -
         SPEED_FRICTION_FADE_START_RPM_X10));
  }

  /* One bounded startup boost per run; it cannot re-arm after release. */
  if ((*startup_state == 0U) &&
      (abs_ref >= SPEED_STARTUP_ARM_RPM_X10))
  {
    *startup_state = 1U;
    *startup_updates = 0U;
    *release_confirm_updates = 0U;
  }
  if (*startup_state == 1U)
  {
    int32_t signed_measured_rpm_x10 = sign * measured_rpm_x10;
    if (signed_measured_rpm_x10 >= SPEED_STARTUP_RELEASE_RPM_X10)
    {
      if (*release_confirm_updates < SPEED_STARTUP_RELEASE_CONFIRM_UPDATES)
      {
        (*release_confirm_updates)++;
      }
    }
    else
    {
      *release_confirm_updates = 0U;
    }

    if (*release_confirm_updates >= SPEED_STARTUP_RELEASE_CONFIRM_UPDATES)
    {
      *startup_state = 2U;
      *release_reason = 1U;
    }
    else if (*startup_updates >= SPEED_STARTUP_MAX_UPDATES)
    {
      *startup_state = 2U;
      *release_reason = 2U;
    }
    else
    {
      magnitude = SPEED_STARTUP_BOOST_COUNTS;
      (*startup_updates)++;
      *startup_active = 1U;
    }
  }
  return sign * magnitude;
}

static int32_t SpeedPi_Update(int32_t error_rpm_x10,
                              int32_t feedforward_counts,
                              int32_t output_limit_counts,
                              int64_t *integral_q16,
                              int32_t *p_counts,
                              int32_t *i_counts,
                              uint8_t *anti_windup,
                              uint8_t *integral_limited)
{
  int64_t candidate_q16 =
      *integral_q16 + ((int64_t)error_rpm_x10 * SPEED_KI_Q16);
  int32_t candidate_i;
  int32_t raw_output;

  *anti_windup = 0U;
  *integral_limited = 0U;
  *p_counts =
      (int32_t)(((int64_t)error_rpm_x10 * SPEED_KP_Q8) / 256LL);

  if (candidate_q16 > SPEED_INTEGRAL_LIMIT_Q16)
  {
    candidate_q16 = SPEED_INTEGRAL_LIMIT_Q16;
    *integral_limited = 1U;
  }
  else if (candidate_q16 < -SPEED_INTEGRAL_LIMIT_Q16)
  {
    candidate_q16 = -SPEED_INTEGRAL_LIMIT_Q16;
    *integral_limited = 1U;
  }

  candidate_i = (int32_t)(candidate_q16 / 65536LL);
  raw_output = *p_counts + candidate_i + feedforward_counts;
  if (((raw_output > output_limit_counts) && (error_rpm_x10 > 0)) ||
      ((raw_output < -output_limit_counts) && (error_rpm_x10 < 0)))
  {
    *anti_windup = 1U;
  }
  else
  {
    *integral_q16 = candidate_q16;
  }

  *i_counts = (int32_t)(*integral_q16 / 65536LL);
  return Speed_Clamp(*p_counts + *i_counts + feedforward_counts,
                     output_limit_counts);
}

static void Sweep_SetDq(uint16_t electrical, int16_t vd_ticks, int16_t vq_ticks)
{
  int32_t sin_e = Vector_SinQ15(electrical);
  int32_t cos_e = Vector_SinQ15(
      Vector_WrapRaw14((int32_t)electrical + VECTOR_QUARTER_TURN));
  int32_t v_alpha = (int32_t)((((int64_t)vd_ticks * cos_e) -
                               ((int64_t)vq_ticks * sin_e)) >> 15U);
  int32_t v_beta = (int32_t)((((int64_t)vd_ticks * sin_e) +
                              ((int64_t)vq_ticks * cos_e)) >> 15U);
  int32_t vu = v_alpha;
  int32_t vv = (int32_t)(((-(int64_t)v_alpha * 16384LL) +
                           ((int64_t)v_beta * 28378LL)) >> 15U);
  int32_t vw = (int32_t)(((-(int64_t)v_alpha * 16384LL) -
                           ((int64_t)v_beta * 28378LL)) >> 15U);

  /* UDIS makes the three preload writes atomic at the next timer update. */
  SET_BIT(TIM1->CR1, TIM_CR1_UDIS);
  TIM1->CCR1 = Vector_ClampCompare((int32_t)TEST_TIM_HALF + vu);
  TIM1->CCR2 = Vector_ClampCompare((int32_t)TEST_TIM_HALF + vv);
  TIM1->CCR3 = Vector_ClampCompare((int32_t)TEST_TIM_HALF + vw);
  CLEAR_BIT(TIM1->SR, TIM_SR_UIF);
  CLEAR_BIT(TIM1->CR1, TIM_CR1_UDIS);
}

static int32_t CurrentPi_Clamp(int32_t value, int32_t limit)
{
  if (value > limit) { return limit; }
  if (value < -limit) { return -limit; }
  return value;
}

static int32_t CurrentPi_LeakTowardZero(int32_t value)
{
  int32_t magnitude = AbsI32(value);
  int32_t step;

  if (magnitude == 0) { return 0; }
  step = magnitude >> PI_ZERO_LEAK_SHIFT;
  if (step < 1) { step = 1; }
  return (value > 0) ? (value - step) : (value + step);
}

static uint8_t CurrentPi_Step(CurrentPiState_t *pi,
                              int32_t id,
                              int32_t iq,
                              int32_t iq_ref,
                              int16_t *vd_ticks,
                              int16_t *vq_ticks,
                              uint8_t *saturated)
{
  int32_t error_d = PI_ID_REF_COUNTS - id;
  int32_t error_q = iq_ref - iq;
  int32_t integral_error_d = error_d;
  int32_t integral_error_q = error_q;
  int32_t raw_d;
  int32_t raw_q;
  int32_t abs_sum;
  int32_t limited_d;
  int32_t limited_q;
  uint8_t is_saturated = 0U;
  uint8_t actions = 0U;
  uint8_t leak_d = 0U;
  uint8_t leak_q = 0U;

  if (AbsI32(integral_error_d) <= PI_INTEGRAL_DEADBAND_COUNTS)
  {
    integral_error_d = 0;
    actions |= PI_ACTION_ID_DEADBAND;
  }
  if (AbsI32(integral_error_q) <= PI_INTEGRAL_DEADBAND_COUNTS)
  {
    integral_error_q = 0;
    actions |= PI_ACTION_IQ_DEADBAND;
  }

  /*
   * Leak only at zero reference after measured current has entered the
   * deadband. Active torque/current regulation keeps normal integration.
   */
  if ((PI_ID_REF_COUNTS == 0) &&
      (AbsI32(id) <= PI_INTEGRAL_DEADBAND_COUNTS))
  {
    pi->id_integral_q8 = CurrentPi_LeakTowardZero(pi->id_integral_q8);
    actions |= PI_ACTION_ID_LEAK;
    leak_d = 1U;
  }
  if ((iq_ref == 0) &&
      (AbsI32(iq) <= PI_INTEGRAL_DEADBAND_COUNTS))
  {
    pi->iq_integral_q8 = CurrentPi_LeakTowardZero(pi->iq_integral_q8);
    actions |= PI_ACTION_IQ_LEAK;
    leak_q = 1U;
  }

  raw_d = (int32_t)(((int64_t)PI_KP_Q8 * error_d) >> 8U) +
          (pi->id_integral_q8 >> 8U);
  raw_q = (int32_t)(((int64_t)PI_KP_Q8 * error_q) >> 8U) +
          (pi->iq_integral_q8 >> 8U);
  abs_sum = AbsI32(raw_d) + AbsI32(raw_q);
  limited_d = raw_d;
  limited_q = raw_q;

  if (abs_sum > PI_OUTPUT_LIMIT_TICKS)
  {
    limited_d = (int32_t)(((int64_t)raw_d * PI_OUTPUT_LIMIT_TICKS) /
                          abs_sum);
    limited_q = (int32_t)(((int64_t)raw_q * PI_OUTPUT_LIMIT_TICKS) /
                          abs_sum);
    is_saturated = 1U;
  }

  /* Conditional integration prevents windup while still allowing unwind. */
  if ((leak_d == 0U) &&
      ((is_saturated == 0U) ||
       ((raw_d > limited_d) && (integral_error_d < 0)) ||
       ((raw_d < limited_d) && (integral_error_d > 0))))
  {
    pi->id_integral_q8 = CurrentPi_Clamp(
        pi->id_integral_q8 + (PI_KI_Q8 * integral_error_d),
        PI_INTEGRAL_LIMIT_Q8);
  }
  if ((leak_q == 0U) &&
      ((is_saturated == 0U) ||
       ((raw_q > limited_q) && (integral_error_q < 0)) ||
       ((raw_q < limited_q) && (integral_error_q > 0))))
  {
    pi->iq_integral_q8 = CurrentPi_Clamp(
        pi->iq_integral_q8 + (PI_KI_Q8 * integral_error_q),
        PI_INTEGRAL_LIMIT_Q8);
  }

  *vd_ticks = (int16_t)limited_d;
  *vq_ticks = (int16_t)limited_q;
  *saturated = is_saturated;
  return actions;
}
static void Sweep_ParkFromPhase(int32_t iu,
                                int32_t iv,
                                int32_t iw,
                                uint16_t electrical,
                                int32_t *id,
                                int32_t *iq,
                                int32_t *isum)
{
  int32_t i_alpha = iu;
  int32_t i_beta = (int32_t)((((int64_t)iu + (2LL * iv)) * 18919LL) >> 15U);
  int32_t sin_e = Vector_SinQ15(electrical);
  int32_t cos_e = Vector_SinQ15(
      Vector_WrapRaw14((int32_t)electrical + VECTOR_QUARTER_TURN));

  (void)iw;
  *id = (int32_t)((((int64_t)i_alpha * cos_e) +
                   ((int64_t)i_beta * sin_e)) >> 15U);
  *iq = (int32_t)(((-(int64_t)i_alpha * sin_e) +
                   ((int64_t)i_beta * cos_e)) >> 15U);
  *isum = iu + iv + iw;
}

static void SweepStage_AddControl(SweepStageStats_t *stage,
                                  int32_t id,
                                  int32_t iq,
                                  int32_t isum,
                                  int32_t vd_ticks,
                                  int32_t vq_ticks,
                                  uint8_t saturated,
                                  int32_t speed_ref_rpm_x10,
                                  int32_t speed_measured_rpm_x10,
                                  int32_t iq_ref_counts,
                                  int32_t speed_p_counts,
                                  int32_t speed_i_counts,
                                  int32_t friction_ff_counts)
{
  uint32_t index = stage->controls;

  SweepWindow_Add(&stage->all, id, iq, isum);
  stage->vd_sum += vd_ticks;
  stage->vq_sum += vq_ticks;
  if (vd_ticks < stage->vd_min) { stage->vd_min = vd_ticks; }
  if (vd_ticks > stage->vd_max) { stage->vd_max = vd_ticks; }
  if (vq_ticks < stage->vq_min) { stage->vq_min = vq_ticks; }
  if (vq_ticks > stage->vq_max) { stage->vq_max = vq_ticks; }
  if (saturated != 0U) { stage->saturation_controls++; }
  stage->speed_ref_sum += speed_ref_rpm_x10;
  stage->speed_measured_sum += speed_measured_rpm_x10;
  stage->iq_ref_sum += iq_ref_counts;
  stage->speed_p_sum += speed_p_counts;
  stage->speed_i_sum += speed_i_counts;
  stage->friction_ff_sum += friction_ff_counts;
  if (speed_ref_rpm_x10 < stage->speed_ref_min) { stage->speed_ref_min = speed_ref_rpm_x10; }
  if (speed_ref_rpm_x10 > stage->speed_ref_max) { stage->speed_ref_max = speed_ref_rpm_x10; }
  if (speed_measured_rpm_x10 < stage->speed_measured_min) { stage->speed_measured_min = speed_measured_rpm_x10; }
  if (speed_measured_rpm_x10 > stage->speed_measured_max) { stage->speed_measured_max = speed_measured_rpm_x10; }
  if (iq_ref_counts < stage->iq_ref_min) { stage->iq_ref_min = iq_ref_counts; }
  if (iq_ref_counts > stage->iq_ref_max) { stage->iq_ref_max = iq_ref_counts; }
  if (speed_p_counts < stage->speed_p_min) { stage->speed_p_min = speed_p_counts; }
  if (speed_p_counts > stage->speed_p_max) { stage->speed_p_max = speed_p_counts; }
  if (speed_i_counts < stage->speed_i_min) { stage->speed_i_min = speed_i_counts; }
  if (speed_i_counts > stage->speed_i_max) { stage->speed_i_max = speed_i_counts; }
  if (friction_ff_counts < stage->friction_ff_min) { stage->friction_ff_min = friction_ff_counts; }
  if (friction_ff_counts > stage->friction_ff_max) { stage->friction_ff_max = friction_ff_counts; }
  if (index < 10U)
  {
    SweepWindow_Add(&stage->transient_0_2ms, id, iq, isum);
    stage->trace_iq[stage->trace_count++] = iq;
  }
  else if (index < 100U)
  {
    SweepWindow_Add(&stage->early_2_20ms, id, iq, isum);
  }
  if ((stage->target_controls > SWEEP_TRACE_SAMPLES) &&
      (index >= (stage->target_controls / 2U)))
  {
    SweepWindow_Add(&stage->steady_second_half, id, iq, isum);
  }
  stage->controls++;
}

static HAL_StatusTypeDef Control_Run(ControlMode_t control_mode,
                                      int8_t direction, int32_t target_value,
                                      SweepRunStats_t *stats)
{
  CurrentRaw_t raw = {0U, 0U, 0U};
  uint8_t over_u = 0U;
  uint8_t over_v = 0U;
  uint8_t over_w = 0U;
  uint8_t timing_consecutive = 0U;
  uint8_t command_pending = 0U;
  uint8_t command_wait_samples = 0U;
  uint16_t encoder_raw = 0U;
  uint16_t electrical = 0U;
  uint32_t start_cycles = 0U;
  uint32_t previous_sample_cycles = 0U;
  int32_t average_u_sum = 0;
  int32_t average_v_sum = 0;
  int32_t average_w_sum = 0;
  uint32_t average_count = 0U;
  uint32_t stage_index = 0U;
  static int16_t speed_ring[SPEED_WINDOW_SAMPLES];
  uint32_t speed_ring_count = 0U;
  uint32_t speed_ring_index = 0U;
  uint32_t speed_loop_divider = 0U;
  uint32_t position_outer_divider = 0U;
  int32_t speed_window_sum = 0;
  int32_t speed_measured_rpm_x10 = 0;
  int32_t speed_ref_rpm_x10 = 0;
  int32_t iq_ref_q8 = 0;
  int32_t iq_ref_counts = 0;
  int32_t speed_p_counts = 0;
  int32_t speed_i_counts = 0;
  int32_t friction_ff_counts = 0;
  int64_t speed_integral_q16 = 0;
  uint8_t startup_state = 0U;
  uint32_t startup_updates = 0U;
  uint32_t release_confirm_updates = 0U;
  int32_t target_position_counts = 0;
  int32_t lower_guard_counts = 0;
  int32_t upper_guard_counts = 0;
  uint32_t position_timeout_controls = 0U;
  int32_t logical_position_counts = 0;
  int32_t position_error_counts = 0;
  uint32_t position_settle_updates = 0U;
  HAL_StatusTypeDef result = HAL_ERROR;
  CurrentPiState_t pi = {0, 0};

  if (stats == NULL) { return HAL_ERROR; }
  *stats = (SweepRunStats_t){0};
  stats->control_mode = (uint8_t)control_mode;
  for (uint32_t i = 0U; i < SPEED_WINDOW_SAMPLES; i++)
  {
    speed_ring[i] = 0;
  }
  stats->reason = "INIT";

  if ((direction != 1) && (direction != -1))
  {
    stats->reason = "BAD_DIRECTION";
    goto shutdown;
  }
  if ((control_mode != CONTROL_MODE_POSITION) &&
      (control_mode != CONTROL_MODE_SPEED))
  {
    stats->reason = "BAD_MODE";
    goto shutdown;
  }
  if (control_mode == CONTROL_MODE_POSITION)
  {
    if ((target_value < 1) || (target_value > POSITION_MAX_OUTPUT_DEG))
    {
      stats->reason = "BAD_POSITION_STEP";
      goto shutdown;
    }
    target_position_counts =
        (int32_t)direction *
        Position_OutputDegreesToMotorCounts(target_value);
    stats->target_position_counts = target_position_counts;
    lower_guard_counts = ((target_position_counts < 0) ?
                          target_position_counts : 0) -
                         POSITION_GUARD_MARGIN_COUNTS;
    upper_guard_counts = ((target_position_counts > 0) ?
                          target_position_counts : 0) +
                         POSITION_GUARD_MARGIN_COUNTS;
    position_timeout_controls = POSITION_TIMEOUT_BASE_CONTROLS +
        (uint32_t)target_value * POSITION_TIMEOUT_PER_DEG_CONTROLS;
  }
  else
  {
    if ((target_value < 1) ||
        (target_value > SPEED_COMMAND_MAX_OUTPUT_RPM))
    {
      stats->reason = "BAD_OUTPUT_SPEED";
      goto shutdown;
    }
    stats->target_output_rpm = (int32_t)direction * target_value;
    position_timeout_controls = SPEED_COMMAND_RUN_CONTROLS;
  }
  if (ClockReadyForPoweredTest() == 0U)
  {
    stats->reason = "CLOCK";
    goto shutdown;
  }
  if (current_offset_valid == 0U)
  {
    stats->reason = "NO_ZERO";
    goto shutdown;
  }
  if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET)
  {
    stats->fault_flags |= TEST_FAULT_PB12_LOW;
    stats->reason = "PB12_LOW";
    goto shutdown;
  }
  if (Vector_MA600Read(&encoder_raw) != HAL_OK)
  {
    stats->fault_flags |= TEST_FAULT_SPI;
    stats->spi_errors++;
    stats->reason = "SPI_START";
    goto shutdown;
  }

  stats->raw_start = encoder_raw;
  stats->raw_previous = encoder_raw;
  stats->raw_end = encoder_raw;
  electrical = Vector_ElectricalAngle(encoder_raw);
  for (uint32_t i = 0U; i < SWEEP_STAGE_COUNT; i++)
  {
    SweepStage_Init(&stats->stage[i], sweep_stage_name[i],
                    sweep_target_controls[i], encoder_raw);
  }
  stats->stage[POSITION_STAGE_ACTIVE].target_controls =
      position_timeout_controls;
  if (control_mode == CONTROL_MODE_SPEED)
  {
    stats->stage[POSITION_STAGE_ACTIVE].name =
        "SPEED_ACTIVE_10000MS";
  }

  if (SyncEngine_Start(1U) != HAL_OK)
  {
    stats->reason = "PWM_ADC_START";
    goto shutdown;
  }
  SET_BIT(TIM1->CCMR1, TIM_CCMR1_OC1PE | TIM_CCMR1_OC2PE);
  SET_BIT(TIM1->CCMR2, TIM_CCMR2_OC3PE);
  Sweep_SetDq(electrical, 0, 0);
  command_pending = 1U;
  start_cycles = DWT->CYCCNT;
  previous_sample_cycles = start_cycles;

  while (stage_index < SWEEP_STAGE_COUNT)
  {
    uint32_t sample_cycles;
    int32_t iu;
    int32_t iv;
    int32_t iw;
    int32_t au;
    int32_t av;
    int32_t aw;

    if (SyncAdc_Read(&raw) != HAL_OK)
    {
      stats->fault_flags |= TEST_FAULT_ADC_TIMEOUT;
      stats->reason = "ADC_TIMEOUT";
      goto shutdown_running;
    }

    /*
     * H/Z strictly prove that the hardware trigger occurs on the intended
     * up-count CCR4 window.  CNT/DIR at the later CPU read is not a sample
     * timestamp: at 20 kHz a valid completed sample can be consumed after a
     * counter boundary.  Detect real missed samples from the DWT interval
     * instead, and require two consecutive long intervals before shutdown.
     */
    sample_cycles = DWT->CYCCNT;
    if ((uint32_t)(sample_cycles - previous_sample_cycles) >
        TEST_ADC_MAX_INTERVAL_CYCLES)
    {
      stats->timing_overruns++;
      timing_consecutive++;
      if (timing_consecutive >= TEST_ADC_MISS_CONSECUTIVE)
      {
        stats->fault_flags |= TEST_FAULT_TIMING;
        stats->reason = "ADC_MISSED";
        goto shutdown_running;
      }
    }
    else
    {
      timing_consecutive = 0U;
    }
    previous_sample_cycles = sample_cycles;
    stats->adc_samples++;

    if (__HAL_UART_GET_FLAG(&huart2, UART_FLAG_RXNE) != RESET)
    {
      uint8_t stop_rx = (uint8_t)huart2.Instance->RDR;
      if ((stop_rx == 'X') || (stop_rx == 'x'))
      {
        stats->fault_flags |= TEST_FAULT_USER_STOP;
        stats->reason = "USER_STOP";
        goto shutdown_running;
      }
    }
    if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET)
    {
      stats->fault_flags |= TEST_FAULT_PB12_LOW;
    }
    if ((TIM1->SR & TIM_SR_BIF) != 0U)
    {
      stats->fault_flags |= TEST_FAULT_TIM1_BIF;
    }
    stats->fault_flags |= CurrentFaults(&raw, &over_u, &over_v, &over_w);
    if (stats->fault_flags != 0U)
    {
      stats->reason = "CURRENT_BKIN";
      goto shutdown_running;
    }

    /* Exclude startup samples until the initial all-zero vector is transferred. */
    if (command_pending != 0U)
    {
      if ((TIM1->SR & TIM_SR_UIF) == 0U)
      {
        command_wait_samples++;
        if (command_wait_samples > 3U)
        {
          stats->fault_flags |= TEST_FAULT_TIMING;
          stats->reason = "PWM_UPDATE_TIMEOUT";
          goto shutdown_running;
        }
        continue;
      }
      CLEAR_BIT(TIM1->SR, TIM_SR_UIF);
      command_pending = 0U;
      command_wait_samples = 0U;
      average_u_sum = 0;
      average_v_sum = 0;
      average_w_sum = 0;
      average_count = 0U;
    }

    iu = Vector_PhaseCurrent(raw.u, current_offset.u);
    iv = Vector_PhaseCurrent(raw.v, current_offset.v);
    iw = Vector_PhaseCurrent(raw.w, current_offset.w);
    au = AbsI32(iu);
    av = AbsI32(iv);
    aw = AbsI32(iw);
    if (au > stats->peak_u) { stats->peak_u = au; }
    if (av > stats->peak_v) { stats->peak_v = av; }
    if (aw > stats->peak_w) { stats->peak_w = aw; }

    average_u_sum += iu;
    average_v_sum += iv;
    average_w_sum += iw;
    average_count++;
    if (average_count == CURRENT_AVERAGE_SAMPLES)
    {
      int32_t id;
      int32_t iq;
      int32_t isum;
      int32_t raw_delta;
      SweepStageStats_t *stage = &stats->stage[stage_index];

      iu = average_u_sum / (int32_t)CURRENT_AVERAGE_SAMPLES;
      iv = average_v_sum / (int32_t)CURRENT_AVERAGE_SAMPLES;
      iw = average_w_sum / (int32_t)CURRENT_AVERAGE_SAMPLES;
      average_u_sum = 0;
      average_v_sum = 0;
      average_w_sum = 0;
      average_count = 0U;

      if (Vector_MA600Read(&encoder_raw) != HAL_OK)
      {
        stats->fault_flags |= TEST_FAULT_SPI;
        stats->spi_errors++;
        stats->reason = "SPI";
        goto shutdown_running;
      }
      raw_delta = Vector_WrappedDelta(encoder_raw, stats->raw_previous);
      if (AbsI32(raw_delta) > VECTOR_MAX_RAW_STEP)
      {
        stats->fault_flags |= TEST_FAULT_ENCODER_JUMP;
        stats->reason = "ENCODER_JUMP";
        goto shutdown_running;
      }

      stats->net_counts += raw_delta;
      stats->raw_previous = encoder_raw;
      stats->raw_end = encoder_raw;
      if (stage->controls == 0U)
      {
        /* The first encoder sample defines the exact start of this stage. */
        stage->raw_start = encoder_raw;
        stage->raw_end = encoder_raw;
        stage->first_control_cycle = sample_cycles;
        stage->last_control_cycle = sample_cycles;
      }
      else
      {
        stage->net_counts += raw_delta;
        stage->raw_end = encoder_raw;
        stage->last_control_cycle = sample_cycles;
      }

      if (speed_ring_count < SPEED_WINDOW_SAMPLES)
      {
        speed_ring[speed_ring_count++] = (int16_t)raw_delta;
        speed_window_sum += raw_delta;
      }
      else
      {
        speed_window_sum -= speed_ring[speed_ring_index];
        speed_ring[speed_ring_index] = (int16_t)raw_delta;
        speed_window_sum += raw_delta;
        speed_ring_index++;
        if (speed_ring_index >= SPEED_WINDOW_SAMPLES)
        {
          speed_ring_index = 0U;
        }
      }
      if ((speed_ring_count >= SPEED_WINDOW_SAMPLES) &&
          (AbsI32(speed_window_sum) >= SPEED_HARD_LIMIT_COUNTS))
      {
        stats->fault_flags |= SWEEP_FAULT_SPEED;
        stats->reason = "SPEED_LIMIT_700_MOTOR_RPM";
        goto shutdown_running;
      }

      speed_loop_divider++;
      if (speed_loop_divider >= SPEED_LOOP_DIVIDER)
      {
        int32_t desired_iq_counts;
        int32_t speed_error_rpm_x10;
        uint8_t speed_anti_windup = 0U;
        uint8_t speed_integral_limited = 0U;
        uint8_t startup_active = 0U;
        uint8_t release_reason = 0U;

        speed_loop_divider = 0U;
        if (speed_ring_count >= SPEED_WINDOW_SAMPLES)
        {
          /* Logical speed is positive forward; MA600 raw decreases forward. */
          speed_measured_rpm_x10 =
              (int32_t)((-(int64_t)speed_window_sum * 600000000LL) /
                        ((int64_t)VECTOR_MA600_COUNTS * SPEED_WINDOW_US));
        }
        else
        {
          speed_measured_rpm_x10 = 0;
        }
        logical_position_counts = -stats->net_counts;
        position_error_counts =
            target_position_counts - logical_position_counts;
        if (stage_index == POSITION_STAGE_ZERO)
        {
          speed_ref_rpm_x10 = 0;
          position_outer_divider = 0U;
          desired_iq_counts = 0;
          speed_integral_q16 = 0;
          speed_p_counts = 0;
          speed_i_counts = 0;
          friction_ff_counts = 0;
        }
        else
        {
          int32_t iq_limit_counts = SPEED_IQ_NEAR_LIMIT_COUNTS;
          if (control_mode == CONTROL_MODE_POSITION)
          {
            position_outer_divider++;
          if (position_outer_divider >= POSITION_OUTER_DIVIDER)
          {
            int32_t position_target_speed =
                Position_SpeedReferenceRpmX10(position_error_counts);
            uint8_t braking_command =
                ((position_target_speed == 0) ||
                 (((speed_ref_rpm_x10 > 0) !=
                   (position_target_speed > 0)) &&
                  (speed_ref_rpm_x10 != 0)) ||
                 (AbsI32(position_target_speed) <
                  AbsI32(speed_ref_rpm_x10))) ? 1U : 0U;
            position_outer_divider = 0U;
            if (braking_command != 0U)
            {
              /* Remove stored propulsion torque before approaching/crossing target. */
              speed_integral_q16 = 0;
            }
            speed_ref_rpm_x10 = Position_SlewSpeedReference(
                speed_ref_rpm_x10, position_target_speed);

            if ((AbsI32(position_error_counts) <=
                 POSITION_DEADBAND_COUNTS) &&
                (AbsI32(speed_measured_rpm_x10) <=
                 POSITION_SETTLE_SPEED_RPM_X10))
            {
              position_settle_updates++;
              if (position_settle_updates > stats->settle_updates_max)
              {
                stats->settle_updates_max = position_settle_updates;
              }
            }
            else
            {
              position_settle_updates = 0U;
            }
            if (position_settle_updates >= POSITION_SETTLE_UPDATES)
            {
              stats->settled = 1U;
              stats->settle_control = stage->controls;
              stats->settle_position_counts = logical_position_counts;
              stats->settle_error_counts = position_error_counts;
              stats->settle_speed_rpm_x10 = speed_measured_rpm_x10;
              result = HAL_OK;
              stats->reason = "DONE_SETTLED";
              goto shutdown_running;
            }
          }
            iq_limit_counts =
                Position_IqLimitCounts(position_error_counts);
          }
          else
          {
            int32_t target_motor_rpm_x10 =
                (int32_t)direction * target_value *
                POSITION_GEAR_RATIO * 10;
            speed_ref_rpm_x10 = SpeedCommand_SlewReference(
                speed_ref_rpm_x10, target_motor_rpm_x10);
          }
          speed_error_rpm_x10 =
              speed_ref_rpm_x10 - speed_measured_rpm_x10;
          friction_ff_counts = Speed_FrictionFeedforward(
              stage_index, speed_ref_rpm_x10, speed_measured_rpm_x10,
              &startup_state, &startup_updates, &startup_active,
              &release_confirm_updates, &release_reason);
          if (startup_active != 0U)
          {
            stage->startup_boost_updates++;
          }
          if (release_confirm_updates > stage->startup_release_confirm_max)
          {
            stage->startup_release_confirm_max = release_confirm_updates;
          }
          if (release_reason == 1U)
          {
            stage->startup_release_speed_events++;
          }
          else if (release_reason == 2U)
          {
            stage->startup_release_timeout_events++;
          }
          desired_iq_counts = SpeedPi_Update(
              speed_error_rpm_x10, friction_ff_counts,
              iq_limit_counts,
              &speed_integral_q16, &speed_p_counts, &speed_i_counts,
              &speed_anti_windup, &speed_integral_limited);
          if (speed_anti_windup != 0U)
          {
            stage->speed_aw_updates++;
          }
          if (speed_integral_limited != 0U)
          {
            stage->speed_integral_limit_updates++;
          }
        }
        {
          int8_t slew_direction =
              (speed_ref_rpm_x10 > 0) ? 1 :
              ((speed_ref_rpm_x10 < 0) ? -1 : direction);
          iq_ref_q8 = Speed_SlewIqReferenceQ8(
              iq_ref_q8, desired_iq_counts, slew_direction);
        }
        iq_ref_counts = iq_ref_q8 / 256;
      }
      logical_position_counts = -stats->net_counts;
      position_error_counts =
          target_position_counts - logical_position_counts;
      if (stage_index == POSITION_STAGE_ACTIVE)
      {
        if (((direction > 0) &&
             (logical_position_counts < -SWEEP_MIN_MOTION_COUNTS)) ||
            ((direction < 0) &&
             (logical_position_counts > SWEEP_MIN_MOTION_COUNTS)))
        {
          stats->fault_flags |= TEST_FAULT_WRONG_DIRECTION;
          stats->reason = "WRONG_DIRECTION";
          goto shutdown_running;
        }
        if ((stage->controls >= POSITION_MOTION_CHECK_CONTROLS) &&
            (AbsI32(logical_position_counts) <
             POSITION_MOTION_MIN_COUNTS))
        {
          stats->fault_flags |= TEST_FAULT_STALL;
          stats->reason = "NO_MOTION_1S";
          goto shutdown_running;
        }
        if ((control_mode == CONTROL_MODE_POSITION) &&
            ((logical_position_counts < lower_guard_counts) ||
             (logical_position_counts > upper_guard_counts)))
        {
          stats->fault_flags |= TEST_FAULT_STALL;
          stats->reason = "POSITION_PATH_GUARD";
          goto shutdown_running;
        }
      }

      electrical = Vector_ElectricalAngle(encoder_raw);
      Sweep_ParkFromPhase(iu, iv, iw, electrical, &id, &iq, &isum);
      {
        int16_t vd_ticks = 0;
        int16_t vq_ticks = 0;
        uint8_t saturated = 0U;
        uint8_t pi_actions = CurrentPi_Step(
            &pi, id, iq, iq_ref_counts,
            &vd_ticks, &vq_ticks, &saturated);
        if ((pi_actions & PI_ACTION_ID_DEADBAND) != 0U)
        {
          stage->id_deadband_controls++;
        }
        if ((pi_actions & PI_ACTION_IQ_DEADBAND) != 0U)
        {
          stage->iq_deadband_controls++;
        }
        if ((pi_actions & PI_ACTION_ID_LEAK) != 0U)
        {
          stage->id_leak_controls++;
        }
        if ((pi_actions & PI_ACTION_IQ_LEAK) != 0U)
        {
          stage->iq_leak_controls++;
        }
        Sweep_SetDq(electrical, vd_ticks, vq_ticks);
        SweepStage_AddControl(stage, id, iq, isum,
                              vd_ticks, vq_ticks, saturated,
                              speed_ref_rpm_x10,
                              speed_measured_rpm_x10,
                              iq_ref_counts,
                              speed_p_counts,
                              speed_i_counts,
                              friction_ff_counts);
      }
      stats->control_samples++;

      if (stage->controls >= stage->target_controls)
      {
        if (stage_index == POSITION_STAGE_ACTIVE)
        {
          if (control_mode == CONTROL_MODE_SPEED)
          {
            result = HAL_OK;
            stats->reason = "SPEED_DURATION_DONE";
            goto shutdown_running;
          }
          if (stats->settled == 0U)
          {
            stats->fault_flags |= TEST_FAULT_STALL;
            stats->reason = "NO_SETTLE_DYNAMIC_TIMEOUT";
            goto shutdown_running;
          }
        }
        stage_index++;
      }
    }
  }

  stats->fault_flags |= TEST_FAULT_STALL;
  stats->reason = "NO_SETTLE";

shutdown_running:
  stats->id_integral_q8_final = pi.id_integral_q8;
  stats->iq_integral_q8_final = pi.iq_integral_q8;
  stats->speed_integral_q16_final = speed_integral_q16;
  stats->final_position_counts = -stats->net_counts;
  stats->final_error_counts =
      stats->target_position_counts - stats->final_position_counts;
  stats->duration_us = (uint32_t)(DWT->CYCCNT - start_cycles) / 170U;
shutdown:
  CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);
  TIM1->CCER = 0U;
  SyncEngine_Stop();
  PowerStage_AllOff();
  return result;
}

static int32_t SweepWindow_Mean(int64_t sum, uint32_t n)
{
  return (n != 0U) ? (int32_t)(sum / (int64_t)n) : 0;
}

static void Sweep_PrintWindow(const char *label, const SweepWindowStats_t *window)
{
  printf("%s n=%lu Id=%ld[%ld,%ld] Iq=%ld[%ld,%ld] Isum=%ld[%ld,%ld]\r\n",
         label, window->n,
         SweepWindow_Mean(window->id_sum, window->n),
         (window->n != 0U) ? window->id_min : 0,
         (window->n != 0U) ? window->id_max : 0,
         SweepWindow_Mean(window->iq_sum, window->n),
         (window->n != 0U) ? window->iq_min : 0,
         (window->n != 0U) ? window->iq_max : 0,
         SweepWindow_Mean(window->isum_sum, window->n),
         (window->n != 0U) ? window->isum_min : 0,
         (window->n != 0U) ? window->isum_max : 0);
}

static void Control_PrintStats(int8_t direction,
                             HAL_StatusTypeDef run_status,
                             const SweepRunStats_t *stats)
{
  int32_t rpm_x10 = 0;

  if (stats->duration_us != 0U)
  {
    rpm_x10 = (int32_t)(((int64_t)stats->net_counts * 600000000LL) /
                        ((int64_t)VECTOR_MA600_COUNTS *
                         stats->duration_us));
  }
  printf("%s LOOP ALL OFF: direction=%s reason=%s result=%s flags=0x%04lX PB12_NOW=%s BIF_LATCHED=%lu\r\n",
         (stats->control_mode == CONTROL_MODE_SPEED) ? "SPEED" : "POSITION",
         (direction > 0) ? "FORWARD" : "REVERSE", stats->reason,
         (run_status == HAL_OK) ? "PASS" :
         ((stats->fault_flags == TEST_FAULT_USER_STOP) ? "STOPPED" : "ABORTED"),
         stats->fault_flags,
         (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) == GPIO_PIN_SET) ?
             "HIGH" : "LOW",
         ((stats->fault_flags & TEST_FAULT_TIM1_BIF) != 0U) ? 1UL : 0UL);
  printf("%s LOOP LOG: adc=%lu control=%lu spi_errors=%lu timing_overruns=%lu raw_start=%u raw_end=%u raw_net=%ld raw_rpm_x10=%ld duration=%lu us\r\n",
         (stats->control_mode == CONTROL_MODE_SPEED) ? "SPEED" : "POSITION",
         stats->adc_samples, stats->control_samples, stats->spi_errors,
         stats->timing_overruns, stats->raw_start, stats->raw_end,
         stats->net_counts, rpm_x10, stats->duration_us);
  if (stats->control_mode == CONTROL_MODE_POSITION)
  {
    printf("POSITION RESULT: motor_target=%ld counts output_target=%ld mdeg motor_final=%ld counts motor_error=%ld counts output_error=%ld mdeg settled=%u settle_max=%lu/%u settle_control=%lu settle_motor_pos=%ld settle_motor_error=%ld settle_motor_speed_x10=%ld\r\n",
           stats->target_position_counts,
           (int32_t)(((int64_t)stats->target_position_counts * 360000LL) /
                     (VECTOR_MA600_COUNTS * POSITION_GEAR_RATIO)),
           stats->final_position_counts, stats->final_error_counts,
           (int32_t)(((int64_t)stats->final_error_counts * 360000LL) /
                     (VECTOR_MA600_COUNTS * POSITION_GEAR_RATIO)),
           stats->settled, stats->settle_updates_max,
           POSITION_SETTLE_UPDATES, stats->settle_control,
           stats->settle_position_counts, stats->settle_error_counts,
           stats->settle_speed_rpm_x10);
  }
  else
  {
    printf("SPEED RESULT: target_output_rpm=%ld target_motor_rpm=%ld average_motor_rpm_x10=%ld auto_off_ms=10000\r\n",
           stats->target_output_rpm,
           stats->target_output_rpm * POSITION_GEAR_RATIO,
           rpm_x10);
  }
  printf("CURRENT PEAK: U=%ld mA V=%ld mA W=%ld mA\r\n",
         Vector_CountsToMilliamp(stats->peak_u),
         Vector_CountsToMilliamp(stats->peak_v),
         Vector_CountsToMilliamp(stats->peak_w));

  for (uint32_t i = 0U; i < SWEEP_STAGE_COUNT; i++)
  {
    const SweepStageStats_t *stage = &stats->stage[i];
    uint32_t duration_us = (stage->controls > 1U) ?
        ((uint32_t)(stage->last_control_cycle -
                    stage->first_control_cycle) / 170U) : 0U;
    int32_t stage_rpm_x10 = (duration_us != 0U) ?
        (int32_t)(((int64_t)stage->net_counts * 600000000LL) /
                  ((int64_t)VECTOR_MA600_COUNTS * duration_us)) : 0;
    int32_t mean_iq_ref = SweepWindow_Mean(stage->iq_ref_sum, stage->controls);
    printf("STEP %lu %s: controls=%lu/%lu duration=%lu us raw=%u->%u net=%ld average_rpm_x10=%ld\r\n",
           i, stage->name, stage->controls, stage->target_controls,
           duration_us, stage->raw_start, stage->raw_end,
           stage->net_counts, stage_rpm_x10);
    printf("  SPEED ref_rpm_x10=%ld[%ld,%ld] measured_rpm_x10=%ld[%ld,%ld]\r\n",
           SweepWindow_Mean(stage->speed_ref_sum, stage->controls),
           (stage->controls != 0U) ? stage->speed_ref_min : 0,
           (stage->controls != 0U) ? stage->speed_ref_max : 0,
           SweepWindow_Mean(stage->speed_measured_sum, stage->controls),
           (stage->controls != 0U) ? stage->speed_measured_min : 0,
           (stage->controls != 0U) ? stage->speed_measured_max : 0);
    printf("  IQ_REF mean=%ld counts (%ld mA) range=[%ld,%ld] counts\r\n",
           mean_iq_ref, Vector_CountsToMilliamp(mean_iq_ref),
           (stage->controls != 0U) ? stage->iq_ref_min : 0,
           (stage->controls != 0U) ? stage->iq_ref_max : 0);
printf("  SPEED_PI P=%ld[%ld,%ld] I=%ld[%ld,%ld] counts; AW=%lu ILIMIT=%lu IRESET=%lu\r\n",
           SweepWindow_Mean(stage->speed_p_sum, stage->controls),
           (stage->controls != 0U) ? stage->speed_p_min : 0,
           (stage->controls != 0U) ? stage->speed_p_max : 0,
           SweepWindow_Mean(stage->speed_i_sum, stage->controls),
           (stage->controls != 0U) ? stage->speed_i_min : 0,
           (stage->controls != 0U) ? stage->speed_i_max : 0,
           stage->speed_aw_updates,
           stage->speed_integral_limit_updates,
           stage->speed_integral_reset_updates);
    printf("  FRICTION_FF mean=%ld range=[%ld,%ld] counts; START_BOOST=%lu/%u speed-loop updates\r\n",
           SweepWindow_Mean(stage->friction_ff_sum, stage->controls),
           (stage->controls != 0U) ? stage->friction_ff_min : 0,
           (stage->controls != 0U) ? stage->friction_ff_max : 0,
           stage->startup_boost_updates, SPEED_STARTUP_MAX_UPDATES);
    printf("  BOOST_RELEASE confirm_max=%lu/%u speed_events=%lu timeout_events=%lu\r\n",
           stage->startup_release_confirm_max,
           SPEED_STARTUP_RELEASE_CONFIRM_UPDATES,
           stage->startup_release_speed_events,
           stage->startup_release_timeout_events);
    Sweep_PrintWindow("  ALL", &stage->all);
    Sweep_PrintWindow("  TRANSIENT_0_2MS", &stage->transient_0_2ms);
    Sweep_PrintWindow("  EARLY_2_20MS", &stage->early_2_20ms);
    Sweep_PrintWindow("  STEADY_SECOND_HALF", &stage->steady_second_half);
    printf("  PI_OUT Vd_mean=%ld[%ld,%ld] Vq_mean=%ld[%ld,%ld] saturated=%lu/%lu\r\n",
           SweepWindow_Mean(stage->vd_sum, stage->controls),
           (stage->controls != 0U) ? stage->vd_min : 0,
           (stage->controls != 0U) ? stage->vd_max : 0,
           SweepWindow_Mean(stage->vq_sum, stage->controls),
           (stage->controls != 0U) ? stage->vq_min : 0,
           (stage->controls != 0U) ? stage->vq_max : 0,
           stage->saturation_controls, stage->controls);
    printf("  PI_ACTION deadband Id=%lu Iq=%lu leak Id=%lu Iq=%lu\r\n",
           stage->id_deadband_controls, stage->iq_deadband_controls,
           stage->id_leak_controls, stage->iq_leak_controls);
    printf("  IQ_FIRST10:");
    for (uint32_t j = 0U; j < stage->trace_count; j++)
    {
      printf(" %ld", stage->trace_iq[j]);
    }
    printf("\r\n");
  }
  printf("CURRENT PI FINAL INTEGRAL_Q8: Id=%ld Iq=%ld\r\n",
         stats->id_integral_q8_final, stats->iq_integral_q8_final);
  printf("FAULT BITS: 0001=PB12 0002=BIF 0004=ADC 0008=U 0010=V 0020=W 0040=X 0080=SPI 0100=ENCODER 0200=DIR 0400=MOTION_OR_POSITION 0800=TIMING 1000=SPEED\r\n");
}

int main(void)
{
  uint8_t rx = 0U;
  uint8_t preflight_valid = 0U;
  ControlMode_t armed_mode = CONTROL_MODE_NONE;
  ControlMode_t entry_mode = CONTROL_MODE_NONE;
  int8_t armed_direction = 0;
  uint32_t arm_tick = 0U;
  int32_t armed_value = 0;
  uint8_t entry_active = 0U;
  uint8_t entry_invalid = 0U;
  uint8_t entry_digits = 0U;
  int8_t entry_direction = 0;
  int32_t entry_value = 0;
  uint32_t entry_tick = 0U;

  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  GatePins_ForceLowGPIO();
  MX_ADC1_Init();
  GatePins_ForceLowGPIO();
  MX_SPI1_Init();
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
  MX_TIM1_Init();
  PowerStage_AllOff();
  MX_USART2_UART_Init();

  CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
  DWT->CYCCNT = 0U;
  DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;

  if (HAL_ADCEx_Calibration_Start(&hadc1, ADC_SINGLE_ENDED) != HAL_OK)
  {
    printf("ADC CALIBRATION FAILED; ALL OFF\r\n");
    Error_Handler();
  }
  if (Adc_ConfigureTim1Trgo() != HAL_OK)
  {
    printf("ADC TIM1 TRGO CONFIG FAILED; ALL OFF\r\n");
    Error_Handler();
  }
  Tim1_Prepare20kHz();
  PowerStage_AllOff();

  printf("\r\n6010 UNIFIED OUTPUT SPEED + RELATIVE POSITION CONTROL\r\n");
  printf("48 V BUS; 1.0 A bench limit; verified 8:1 reduction; motor required for powered commands.\r\n");
  printf("PWM=20 kHz, current PI=5 kHz, speed PI=500 Hz, dead-time about 3 us, voltage limit=40%%.\r\n");
  printf("POSITION: A1..A360 forward, B1..B360 reverse; unit=output degree; staged Iq 1.0 A then 0.5 A.\r\n");
  printf("SPEED: F1..F75 forward, R1..R75 reverse; unit=output rpm; motor target=8x; Iq limit=0.5 A.\r\n");
  printf("Speed command ramps at 100 motor-rpm/s, runs 10 s, then automatic ALL-OFF; 700 motor-rpm hard trip.\r\n");
  printf("Mode/direction changes are accepted only while ALL-OFF; every command requires Y within 5 s; X always stops.\r\n");
  printf("Position mode retains no position integrator, distance braking, dynamic timeout, and +/-60 output-deg path guard.\r\n");
  printf("Aborts: wrong direction, no motion at 1 s, encoder jump, 700 motor-rpm, 1.8 A x3/2.5 A, BKIN, timing, X.\r\n");
  printf("Commands: H=preflight, Z=zero, A/B=position, F/R=speed, Y=confirm, X=stop.\r\n");
  printf("Examples: A30, B90, F20, R50; auto-commit after 200 ms, or terminate with ENTER.\r\n");
  printf("theta=wrap(-14*raw+%d); MA600 raw decreases for logical forward.\r\n", VECTOR_ELECTRICAL_OFFSET);
  printf("HSE_PLL_TEST status=%lu SYSCLK=%lu result=%s\r\n",
         clock_test_status, HAL_RCC_GetSysClockFreq(),
         (ClockReadyForPoweredTest() != 0U) ? "PASS" : "FAIL");
  printf("READY: BUS OFF and rotor still; send H then Z; connect motor; enter one command, wait ARMED, then Y.\r\n");

  while (1)
  {
    HAL_StatusTypeDef uart_rx_status =
        HAL_UART_Receive(&huart2, &rx, 1U, 100U);
    if (uart_rx_status != HAL_OK)
    {
      uint32_t now = HAL_GetTick();
      if ((uart_rx_status == HAL_TIMEOUT) &&
          (entry_active != 0U) &&
          (entry_digits != 0U) &&
          ((now - entry_tick) >= ANGLE_AUTO_COMMIT_MS))
      {
        rx = '\r';
      }
      else
      {
        if (uart_rx_status != HAL_TIMEOUT)
        {
          __HAL_UART_CLEAR_OREFLAG(&huart2);
          entry_active = 0U;
          entry_invalid = 0U;
          entry_digits = 0U;
          entry_direction = 0;
          entry_value = 0;
          entry_mode = CONTROL_MODE_NONE;
          PowerStage_AllOff();
          printf("UART RX ERROR/OVERRUN CLEARED; COMMAND ENTRY CANCELLED; ALL OFF\r\n");
        }
        if ((entry_active != 0U) &&
            ((now - entry_tick) > ANGLE_ENTRY_TIMEOUT_MS))
        {
          entry_active = 0U;
          entry_invalid = 0U;
          entry_digits = 0U;
          entry_direction = 0;
          entry_value = 0;
          entry_mode = CONTROL_MODE_NONE;
          PowerStage_AllOff();
          printf("COMMAND ENTRY TIMEOUT; ALL OFF. Send A/B degrees or F/R output rpm.\r\n");
        }
        if ((armed_mode != CONTROL_MODE_NONE) &&
            ((now - arm_tick) > 5000U))
        {
          armed_mode = CONTROL_MODE_NONE;
          armed_direction = 0;
          armed_value = 0;
          PowerStage_AllOff();
          printf("ARM TIMEOUT; ALL OFF\r\n");
        }
        continue;
      }
    }
    if ((rx >= 'a') && (rx <= 'z'))
    {
      rx = (uint8_t)(rx - ('a' - 'A'));
    }

    if (rx == 'X')
    {
      armed_mode = CONTROL_MODE_NONE;
      armed_direction = 0;
      armed_value = 0;
      entry_active = 0U;
      entry_invalid = 0U;
      entry_digits = 0U;
      entry_direction = 0;
      entry_value = 0;
      entry_mode = CONTROL_MODE_NONE;
      SyncEngine_Stop();
      PowerStage_AllOff();
      printf("STOP/RESET; ALL OFF\r\n");
      continue;
    }

    if (entry_active != 0U)
    {
      if ((rx == 'A') || (rx == 'B') ||
          (rx == 'F') || (rx == 'R'))
      {
        entry_invalid = 0U;
        entry_digits = 0U;
        entry_direction = ((rx == 'A') || (rx == 'F')) ? 1 : -1;
        entry_mode = ((rx == 'A') || (rx == 'B')) ?
            CONTROL_MODE_POSITION : CONTROL_MODE_SPEED;
        entry_value = 0;
        entry_tick = HAL_GetTick();
        PowerStage_AllOff();
        continue;
      }
      if ((rx == ' ') || (rx == '\t'))
      {
        continue;
      }
      if ((rx == '\r') || (rx == '\n'))
      {
        int32_t maximum = (entry_mode == CONTROL_MODE_POSITION) ?
            POSITION_MAX_OUTPUT_DEG : SPEED_COMMAND_MAX_OUTPUT_RPM;
        PowerStage_AllOff();
        if ((entry_invalid != 0U) ||
            (entry_mode == CONTROL_MODE_NONE) ||
            (entry_direction == 0) ||
            (entry_digits == 0U) ||
            (entry_value < 1) || (entry_value > maximum))
        {
          armed_mode = CONTROL_MODE_NONE;
          armed_direction = 0;
          armed_value = 0;
          printf("COMMAND REJECTED: mode=%u direction=%d digits=%u value=%ld; use A/B 1..360 deg or F/R 1..75 output rpm. ALL OFF\r\n",
                 (unsigned int)entry_mode, entry_direction,
                 entry_digits, (long)entry_value);
        }
        else if (ClockReadyForPoweredTest() == 0U)
        {
          printf("REFUSED: 170 MHz HSE/PLL not verified. ALL OFF\r\n");
        }
        else if (preflight_valid == 0U)
        {
          printf("REFUSED: send H successfully first. ALL OFF\r\n");
        }
        else if (current_offset_valid == 0U)
        {
          printf("REFUSED: send Z successfully first. ALL OFF\r\n");
        }
        else if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET)
        {
          printf("REFUSED: PB12/OCD LOW. ALL OFF\r\n");
        }
        else
        {
          armed_mode = entry_mode;
          armed_direction = entry_direction;
          armed_value = entry_value;
          arm_tick = HAL_GetTick();
          if (armed_mode == CONTROL_MODE_POSITION)
          {
            printf("ARMED POSITION: output_step=%ld deg motor_target=%ld counts; send Y within 5 s.\r\n",
                   (long)armed_direction * armed_value,
                   (long)armed_direction *
                       Position_OutputDegreesToMotorCounts(armed_value));
          }
          else
          {
            printf("ARMED SPEED: output_target=%ld rpm motor_target=%ld rpm; 10 s auto-off; send Y within 5 s.\r\n",
                   (long)armed_direction * armed_value,
                   (long)armed_direction * armed_value *
                       POSITION_GEAR_RATIO);
          }
        }
        entry_active = 0U;
        entry_invalid = 0U;
        entry_digits = 0U;
        entry_direction = 0;
        entry_value = 0;
        entry_mode = CONTROL_MODE_NONE;
        continue;
      }

      if ((rx >= '0') && (rx <= '9') &&
          (entry_mode != CONTROL_MODE_NONE) &&
          (entry_direction != 0) &&
          (entry_digits < 3U))
      {
        entry_value = entry_value * 10 + (int32_t)(rx - '0');
        entry_digits++;
        entry_tick = HAL_GetTick();
        if (((entry_mode == CONTROL_MODE_POSITION) &&
             (entry_value > POSITION_MAX_OUTPUT_DEG)) ||
            ((entry_mode == CONTROL_MODE_SPEED) &&
             (entry_value > SPEED_COMMAND_MAX_OUTPUT_RPM)))
        {
          entry_invalid = 1U;
        }
      }
      else
      {
        entry_invalid = 1U;
      }
      continue;
    }

    if ((rx == '\r') || (rx == '\n')) { continue; }

    if ((rx == 'A') || (rx == 'B') ||
        (rx == 'F') || (rx == 'R'))
    {
      armed_mode = CONTROL_MODE_NONE;
      armed_direction = 0;
      armed_value = 0;
      entry_active = 1U;
      entry_invalid = 0U;
      entry_digits = 0U;
      entry_direction = ((rx == 'A') || (rx == 'F')) ? 1 : -1;
      entry_mode = ((rx == 'A') || (rx == 'B')) ?
          CONTROL_MODE_POSITION : CONTROL_MODE_SPEED;
      entry_value = 0;
      entry_tick = HAL_GetTick();
      PowerStage_AllOff();
      continue;
    }

    if (rx == 'H')
    {
      SyncCapture_t capture;
      HAL_StatusTypeDef adc_status;
      HAL_StatusTypeDef encoder_status;
      uint8_t adc_plausible;
      armed_mode = CONTROL_MODE_NONE;
      armed_direction = 0;
      armed_value = 0;
      preflight_valid = 0U;
      current_offset = (CurrentRaw_t){0U, 0U, 0U};
      current_offset_valid = 0U;
      PowerStage_AllOff();
      printf("PREFLIGHT START: BUS OFF, rotor still, outputs OFF.\r\n");
      adc_status = CaptureSamples(TEST_SYNC_SAMPLES, 0U, 0U, &capture);
      PrintCapture("SYNC BUS-OFF", &capture);
      adc_plausible = CaptureZeroPlausible(&capture);
      printf("PREFLIGHT ADC PLAUSIBILITY: mean=%u..%u span<=%u result=%s\r\n",
             TEST_ADC_ZERO_MIN, TEST_ADC_ZERO_MAX,
             TEST_ADC_ZERO_SPAN_MAX,
             (adc_plausible != 0U) ? "PASS" : "FAIL");
      encoder_status = Vector_MA600BusOffTest();
      if ((adc_status == HAL_OK) && (adc_plausible != 0U) &&
          (encoder_status == HAL_OK) &&
          (ClockReadyForPoweredTest() != 0U))
      {
        preflight_valid = 1U;
        printf("PREFLIGHT PASS; ALL OFF\r\n");
      }
      else
      {
        printf("PREFLIGHT FAIL; POWERED COMMANDS LOCKED; ALL OFF\r\n");
      }
      continue;
    }

    if (rx == 'Z')
    {
      armed_mode = CONTROL_MODE_NONE;
      armed_direction = 0;
      armed_value = 0;
      PowerStage_AllOff();
      printf("ZERO START: motor current must be zero; outputs OFF.\r\n");
      if (ZeroCalibrate() == HAL_OK)
      {
        printf("ZERO PASS: U=%u V=%u W=%u\r\n",
               current_offset.u, current_offset.v, current_offset.w);
      }
      else
      {
        current_offset_valid = 0U;
        printf("ZERO FAILED; POWERED COMMANDS LOCKED; ALL OFF\r\n");
      }
      continue;
    }

    if ((rx == 'Y') && (armed_mode != CONTROL_MODE_NONE) &&
        (armed_direction != 0) &&
        ((HAL_GetTick() - arm_tick) <= 5000U))
    {
      ControlMode_t run_mode = armed_mode;
      int8_t run_direction = armed_direction;
      int32_t run_value = armed_value;
      static SweepRunStats_t stats;
      armed_mode = CONTROL_MODE_NONE;
      armed_direction = 0;
      armed_value = 0;
      if ((ClockReadyForPoweredTest() == 0U) ||
          (current_offset_valid == 0U) ||
          (preflight_valid == 0U) ||
          (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_12) != GPIO_PIN_SET))
      {
        PowerStage_AllOff();
        printf("REFUSED AT CONFIRM: interlock changed. ALL OFF\r\n");
        continue;
      }
      if (run_mode == CONTROL_MODE_POSITION)
      {
        printf("CONFIRMED POSITION: staged-Iq 1.0->0.5 A output_step=%ld deg; timeout=%lu ms; X stops.\r\n",
               (long)run_direction * run_value,
               (unsigned long)(5000U + (uint32_t)run_value * 100U));
      }
      else
      {
        printf("CONFIRMED SPEED: output_target=%ld rpm motor_target=%ld rpm; Iq<=0.5 A; 10 s auto-off; X stops.\r\n",
               (long)run_direction * run_value,
               (long)run_direction * run_value * POSITION_GEAR_RATIO);
      }
      {
        HAL_StatusTypeDef run_status = Control_Run(
            run_mode, run_direction, run_value, &stats);
        Control_PrintStats(run_direction, run_status, &stats);
      }
      printf("READY AGAIN: inspect current/log; use A/B position or F/R speed, wait ARMED, then Y.\r\n");
      continue;
    }

    armed_mode = CONTROL_MODE_NONE;
    armed_direction = 0;
    armed_value = 0;
    PowerStage_AllOff();
    printf("REJECTED. Commands: H, Z, A/B 1..360 deg, F/R 1..75 output rpm, Y, X. ALL OFF\r\n");
  }
}


void SystemClock_Config(void)
{
  RCC_OscInitTypeDef osc = {0};
  RCC_ClkInitTypeDef clk = {0};

  HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST);

  osc.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  osc.HSEState = RCC_HSE_ON;
  osc.PLL.PLLState = RCC_PLL_ON;
  osc.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  osc.PLL.PLLM = RCC_PLLM_DIV2;
  osc.PLL.PLLN = 85;
  osc.PLL.PLLP = RCC_PLLP_DIV2;
  osc.PLL.PLLQ = RCC_PLLQ_DIV2;
  osc.PLL.PLLR = RCC_PLLR_DIV2;
  if (HAL_RCC_OscConfig(&osc) != HAL_OK)
  {
    clock_test_status = 1U;
    return;
  }

  clk.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
                  RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  clk.AHBCLKDivider = RCC_SYSCLK_DIV1;
  clk.APB1CLKDivider = RCC_HCLK_DIV1;
  clk.APB2CLKDivider = RCC_HCLK_DIV1;
  if (HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_4) != HAL_OK)
  {
    clock_test_status = 2U;
  }
}

void Error_Handler(void)
{
  __disable_irq();
  if (TIM1 != NULL)
  {
    CLEAR_BIT(TIM1->BDTR, TIM_BDTR_MOE);
    TIM1->CCER = 0U;
    CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN);
  }
  while (1)
  {
  }
}

#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
  (void)file;
  (void)line;
}
#endif





