/*
 * Файл: estimate.c
 * Модуль идентификации параметров асинхронного двигателя
 */

#include "estimate.h"
#include "def.h"
#include <math.h>
#include <string.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

/*
 * Настройки эксперимента по определению активного сопротивления статора Rs
 * Метод: разностный метод с двумя уровнями постоянного тока
 */
float RS_IREF1            = 0.1f;     /* первый уровень тока, А */
float RS_IREF2            = 0.2f;     /* второй уровень тока, А */
float RS_SETTLE_TIME      = 0.5f;     /* время установления тока, с */
float RS_THRESHOLD        = 0.98f;    /* относительный порог достижения тока */

/*
 * Настройки эксперимента по определению сопротивления ротора Rr
 * и индуктивностей рассеяния Llr, Lls
 * Метод: подача переменного тока по оси q, RMS измерения
 */
float RRL_IQ_AMP          = 0.2f;     /* амплитуда тока, А */
float RRL_RAMP_TIME       = 2.0f;     /* время нарастания амплитуды, с */
float RRL_FREQ            = 1.0f;     /* частота тестового сигнала, Гц (дефолт) */
int   RRL_SAMPLES         = 200;      /* количество выборок для RMS */
int   RRL_AVG             = 10;       /* количество усреднений на частоте */

/* Частоты для многоточечной идентификации (Гц) */
#define RRL_FREQ_COUNT 5
static const float RRL_FREQS[RRL_FREQ_COUNT] = { 1.0f, 2.0f, 3.0f, 5.0f, 10.0f };

/*
 * Настройки эксперимента по определению взаимной индуктивности Lm
 * Метод: подача постоянного тока по оси d, интегрирование ЭДС на фронте
 */
float LM_IDREF            = 0.3f;     /* заданный постоянный ток, о.е. */
float LM_SETTLE_THRESHOLD = 0.95f;    /* порог окончания переходного процесса */
float LM_SETTLE_TIME      = 0.5f;     /* выдержка для усреднения Id, с */
float LM_DEMAG_TIME       = 0.3f;     /* размагничивание между циклами, с */
int   LM_AVG              = 5;        /* количество измерений для усреднения */

/*
 * Настройки переключения между экспериментами
 */
float STEP_PAUSE_TIME = 0.5f;

/* Текущий шаг идентификации */
static Estimate_Test_t test_step = ESTIMATE_TEST_IDLE;

/* Структура с вычисленными параметрами двигателя */
static Params_t params = { 0 };

/* ============================================
 * ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜
 * ============================================ */
static RsState_t rs_state = {0};
static RrlState_t rrl_state = {0};
static LmState_t lm_state = {0};
static int in_pause;
static float pause_timer;

/* ============================================
 * ˜˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜
 * ============================================ */

/*
 * ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ Vd/Vq ˜ ˜˜˜˜˜˜
 * ˜˜˜˜: v - ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ (˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜)
 * ˜˜˜˜˜: ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜
 */
static inline float vdq_to_volts(float vq) {
    float vq_oe = vq / T1_PRD;
    float dt_oe = DT / T_PWM;
    if (vq_oe > dt_oe) vq_oe -= dt_oe;
    else if (vq_oe < -dt_oe) vq_oe += dt_oe;
    return vq_oe * U_BAZ / SQRT3;
}

/*
 * ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜ Iq ˜ ˜˜˜˜˜˜
 * ˜˜˜˜: iq - ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜
 * ˜˜˜˜˜: ˜˜˜ ˜ ˜˜˜˜˜˜˜
 */
static inline float idq_to_amps(float iq) {
    return iq * I_BAZ;
}

/* ============================================
 * ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜
 * ============================================ */

/*
 * ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 * ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜
 */
void estimate_init(void) {
    params.Rs = 0.0192f;
    params.Rr = 0.0085f;
    params.Lls = 0.0019364f;
    params.Llr = 0.0019364f;
    params.Lk = params.Llr + params.Lls;
    params.Zk = 0;
    params.Rk = params.Rs+ params.Rr;
    params.Xk = 0;
    params.Lm = 0;
    
    memset(&rs_state, 0, sizeof(RsState_t));
    memset(&rrl_state, 0, sizeof(RrlState_t));
    memset(&lm_state, 0, sizeof(LmState_t));
    in_pause = 0;
    pause_timer = 0;
}

void estimate_reset(void) {
  memset(&rs_state, 0, sizeof(RsState_t));
  memset(&rrl_state, 0, sizeof(RrlState_t));
  memset(&lm_state, 0, sizeof(LmState_t));
  test_step = ESTIMATE_TEST_IDLE;
}

void estimate_start(Estimate_Test_t start_test) {
  estimate_reset();
  test_step = start_test;
}

/*
 * ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 * ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜
 */
Estimate_Test_t estimate_get_step(void) {
    return test_step;
}

/*
 * ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜˜˜
 * ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜ params
 */
Params_t* estimate_get_params(void) {
    return &params;
}

/* ============================================
 * ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 * ============================================ */

/*
 * ˜˜˜˜˜˜˜˜˜˜˜: ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ Rs
 * 
 * ˜˜˜˜˜˜˜˜:
 *   step0: ˜˜˜˜˜˜ ˜˜˜˜ IREF1, ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜
 *   step1: ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜, ˜˜˜˜˜˜˜˜˜ U1 ˜ I1
 *   step2: ˜˜˜˜˜˜ ˜˜˜˜ IREF2, ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜
 *   step3: ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜, ˜˜˜˜˜˜˜˜˜ U2 ˜ I2
 *   step4: ˜˜˜˜˜˜ Rs = (U2-U1)/(I2-I1), ˜˜˜˜˜˜˜˜˜˜
 * 
 * ˜˜˜˜˜˜˜˜˜:
 *   vq, iq - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜ (˜˜˜. ˜˜.)
 *   dt - ˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 *   iq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜
 *   freq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜ (0 ˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜)
 */
static void process_Rs(float vq, float iq, float dt, float* iq_ref, float* freq_ref) {
  if (freq_ref)
    *freq_ref = 0;

  switch (rs_state.step) {
  case 0:
    *iq_ref = RS_IREF1;
    rs_state.sum_v = 0;
    rs_state.sum_i = 0;
    rs_state.sample_cnt = 0;
    if (fabsf(iq) >= RS_IREF1 * RS_THRESHOLD) {
      rs_state.step = 1;
      rs_state.timer = 0;
    }
    break;

  case 1:
    *iq_ref = RS_IREF1;
    rs_state.timer += dt;
    rs_state.sum_v += vdq_to_volts(vq);
    rs_state.sum_i += idq_to_amps(iq);
    rs_state.sample_cnt++;
    if (rs_state.timer >= RS_SETTLE_TIME) {
      if (rs_state.sample_cnt > 0) {
        rs_state.meas1 = rs_state.sum_v / rs_state.sample_cnt;
        rs_state.val1 = rs_state.sum_i / rs_state.sample_cnt;
      }
      rs_state.step = 2;
    }
    break;

  case 2:
    *iq_ref = RS_IREF2;
    rs_state.sum_v = 0;
    rs_state.sum_i = 0;
    rs_state.sample_cnt = 0;
    if (fabsf(iq) >= RS_IREF2 * RS_THRESHOLD) {
      rs_state.step = 3;
      rs_state.timer = 0;
    }
    break;

  case 3:
    *iq_ref = RS_IREF2;
    rs_state.timer += dt;
    rs_state.sum_v += vdq_to_volts(vq);
    rs_state.sum_i += idq_to_amps(iq);
    rs_state.sample_cnt++;
    if (rs_state.timer >= RS_SETTLE_TIME) {
      if (rs_state.sample_cnt > 0) {
        rs_state.meas2 = rs_state.sum_v / rs_state.sample_cnt;
        rs_state.val2 = rs_state.sum_i / rs_state.sample_cnt;
      }
      rs_state.step = 4;
    }
    break;

  case 4:
    *iq_ref = 0;
    if (fabsf(rs_state.val2 - rs_state.val1) > 0.001f) {
      params.Rs = (rs_state.meas2 - rs_state.meas1) / (rs_state.val2 - rs_state.val1);
      if (params.Rs < 0) params.Rs = 0;
    }
    rs_state.step = 5;
    rs_state.done = 1;  /* ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ */
    break;

  default:
    *iq_ref = 0;
    break;
  }
}



/*
 * ˜˜˜˜˜˜˜˜˜˜˜: ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜ Rr ˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜
 *
 * ˜˜˜˜˜˜˜˜:
 *   - ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜ ˜˜ 0 ˜˜ RRL_IQ_AMP ˜˜ ˜˜˜˜˜ RRL_RAMP_TIME
 *   - ˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜
 *   - ˜˜˜˜˜˜˜˜˜˜ RRL_SAMPLES ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜, Vq^2, Iq^2
 *   - ˜˜˜˜˜˜ RMS ˜˜˜˜˜˜˜˜, ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ Zk ˜ ˜˜˜˜˜˜˜˜˜ Rk
 *   - ˜˜˜˜˜˜ Rr = Rk - Rs, Xk, Lk
 *   - ˜˜˜˜˜˜˜˜˜˜ RRL_AVG ˜˜˜
 *
 * ˜˜˜˜˜˜˜˜˜:
 *   vq, iq - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜ (˜˜˜. ˜˜.)
 *   dt - ˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 *   iq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜
 *   freq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜
 */
static void process_RrL(float vq, float iq, float dt, float* iq_ref, float* freq_ref) {

  float f = RRL_FREQS[(rrl_state.freq_idx < RRL_FREQ_COUNT) ? rrl_state.freq_idx : (RRL_FREQ_COUNT - 1)];

  /* ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ (0...1) */
  float amp_k = rrl_state.ramp_timer / RRL_RAMP_TIME;
  if (amp_k > 1.0f) amp_k = 1.0f;

  /* ˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜ */
  float current_amp = RRL_IQ_AMP * amp_k;

  /* ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜ */
  *iq_ref = current_amp * sinf(2.0f * M_PI * f * rrl_state.timer);
  if (freq_ref) *freq_ref = f;
  rrl_state.timer += dt;
  rrl_state.ramp_timer += dt;

  /* ˜˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜ ˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ - ˜˜ ˜˜˜˜˜˜˜˜ */
  if (amp_k < 1.0f) {
    return;
  }

  /* ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜, ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ */
  float vq_oe = vq / T1_PRD;
  float dt_oe = DT / T_PWM;
  if (vq_oe > dt_oe) vq_oe -= dt_oe;
  else if (vq_oe < -dt_oe) vq_oe += dt_oe;

  float vq_V = vdq_to_volts(vq);
  float iq_A = idq_to_amps(iq);

  rrl_state.sum_p += vq_V * iq_A;
  rrl_state.sum_vq2 += vq_V * vq_V;
  rrl_state.sum_iq2 += iq_A * iq_A;
  rrl_state.sample_count++;

  if (rrl_state.sample_count >= RRL_SAMPLES) {
    float P = rrl_state.sum_p / (float)RRL_SAMPLES;
    float Vq_rms = sqrtf(rrl_state.sum_vq2 / (float)RRL_SAMPLES);
    float Iq_rms = sqrtf(rrl_state.sum_iq2 / (float)RRL_SAMPLES);

    if (Iq_rms > 0.001f) {
      float Zk = Vq_rms / Iq_rms;
      float Rk = P / (Iq_rms * Iq_rms);
      float Rr = Rk - params.Rs;
      if (Rr < 0) Rr = 0;
      float Xk = (Zk > Rk) ? sqrtf(Zk * Zk - Rk * Rk) / 2 : 0;
      float Lk = (f > 1e-6f) ? (Xk / (2.0f * M_PI * f)) : 0.0f;

      (void)Rr;
      rrl_state.avg_Zk += Zk;
      rrl_state.avg_Xk += Xk;
      rrl_state.avg_Lk += Lk;
      rrl_state.avg_Rk += Rk;
      rrl_state.avg_count++;
    }

    rrl_state.sum_p = 0;
    rrl_state.sum_vq2 = 0;
    rrl_state.sum_iq2 = 0;
    rrl_state.sample_count = 0;
  }

  if (rrl_state.avg_count >= RRL_AVG) {
    /* ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ */
    int k = rrl_state.freq_idx;
    if (k < RRL_FREQ_COUNT) {
      rrl_state.freq_Zk[k] = rrl_state.avg_Zk / (float)RRL_AVG;
      rrl_state.freq_Xk[k] = rrl_state.avg_Xk / (float)RRL_AVG;
      rrl_state.freq_Lk[k] = rrl_state.avg_Lk / (float)RRL_AVG;
      rrl_state.freq_Rk[k] = rrl_state.avg_Rk / (float)RRL_AVG;
      rrl_state.freq_ready = k + 1;
    }

    /* ˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ */
    rrl_state.freq_idx++;
    rrl_state.timer = 0.0f;
    rrl_state.ramp_timer = 0.0f;
    rrl_state.sum_p = 0.0f;
    rrl_state.sum_vq2 = 0.0f;
    rrl_state.sum_iq2 = 0.0f;
    rrl_state.sample_count = 0;
    rrl_state.avg_Zk = 0.0f;
    rrl_state.avg_Xk = 0.0f;
    rrl_state.avg_Lk = 0.0f;
    rrl_state.avg_Rk = 0.0f;
    rrl_state.avg_count = 0;

    /* ˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜ 0 ˜˜ */
    if (rrl_state.freq_idx >= RRL_FREQ_COUNT) {
      float s1 = 0.0f, sf = 0.0f, sf2 = 0.0f;
      float sR = 0.0f, sfR = 0.0f;
      float sfX = 0.0f;
      int n = rrl_state.freq_ready;
      for (int i = 0; i < n; i++) {
        float fi = RRL_FREQS[i];
        float Ri = rrl_state.freq_Rk[i];
        float Xi = rrl_state.freq_Xk[i];
        s1 += 1.0f;
        sf += fi;
        sf2 += fi * fi;
        sR += Ri;
        sfR += fi * Ri;
        sfX += fi * Xi;
      }

      /* Rk(f) = a + b f */
      float den = (s1 * sf2 - sf * sf);
      float aR = (fabsf(den) > 1e-9f) ? ((sR * sf2 - sf * sfR) / den) : (n > 0 ? (sR / (float)n) : 0.0f);

      /* Xk(f) = c f (˜˜˜˜˜ 0) */
      float cX = (sf2 > 1e-9f) ? (sfX / sf2) : 0.0f;

      params.Rk = aR;
      params.Xk = 0.0f; /* ˜˜˜ f=0 ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜ -> 0 */
      params.Zk = params.Rk;
      params.Lk = cX / (2.0f * M_PI);

      params.Rr = params.Rk - params.Rs;
      if (params.Rr < 0.0f) params.Rr = 0.0f;
      params.Lls = params.Lk / 2.0f;
      params.Llr = params.Lk / 2.0f;

      rrl_state.done = 1;
    }
  }
}



/*
 * ˜˜˜˜˜˜˜˜˜˜˜: ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ Lm
 *
 * ˜˜˜˜˜˜˜˜:
 *   - ˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜ Idref
 *   - ˜˜˜˜˜˜˜˜˜˜˜ Ed = Vd - Rs*Id ˜˜˜˜˜˜ ˜˜˜˜ ˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜
 *   - ˜˜˜ ˜˜˜˜˜˜ ˜˜˜ ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜ LM_SETTLE_THRESHOLD - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜
 *   - Ls = integral(Ed*dt) / Id_steady
 *   - Lm = Ls - Lls
 *
 * ˜˜˜˜˜˜˜˜˜:
 *   vd, id - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜ (˜˜˜. ˜˜.)
 *   dt - ˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
 *   id_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜
 *   freq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜
 */
static void process_Lm(float vd, float id, float dt, float* id_ref, float* freq_ref) {
  float vd_abs = vdq_to_volts(vd);
  float id_abs = idq_to_amps(id);
  float ed = vd_abs - params.Rs * id_abs;
  float id_pu = fabsf(id);
  float id_thresh = LM_IDREF * LM_SETTLE_THRESHOLD;

  if (freq_ref)
    *freq_ref = 0.0f;

  switch (lm_state.step) {
  case 0:
    *id_ref = 0.0f;
    lm_state.timer += dt;
    if (lm_state.timer >= LM_DEMAG_TIME) {
      lm_state.timer = 0.0f;
      lm_state.integral_psi = 0.0f;
      lm_state.first_sample = 0;
      lm_state.prev_ed = ed;
      lm_state.step = 1;
    }
    break;

  case 1:
    *id_ref = LM_IDREF;
    if (!lm_state.first_sample) {
      lm_state.prev_ed = ed;
      lm_state.first_sample = 1;
    }
    else {
      lm_state.integral_psi += (ed + lm_state.prev_ed) * 0.5f * dt;
      lm_state.prev_ed = ed;
    }

    if (id_pu >= id_thresh) {
      lm_state.step = 2;
      lm_state.timer = 0.0f;
      lm_state.sum_id = 0.0f;
      lm_state.sample_cnt = 0;
    }
    break;

  case 2:
    *id_ref = LM_IDREF;
    lm_state.timer += dt;
    lm_state.sum_id += id_abs;
    lm_state.sample_cnt++;
    if (lm_state.timer >= LM_SETTLE_TIME) {
      lm_state.step = 3;
    }
    break;

  case 3: {
    *id_ref = LM_IDREF;
    int measure_ok = 0;
    if (lm_state.sample_cnt > 0 && lm_state.integral_psi > 0.0f) {
      float id_avg = lm_state.sum_id / (float)lm_state.sample_cnt;
      if (id_avg > 0.001f) {
        float Ls = lm_state.integral_psi / id_avg;
        float Lm = Ls - params.Lls;
        if (Lm > 0.0f) {
          lm_state.avg_Lm += Lm;
          lm_state.avg_count++;
          measure_ok = 1;
        }
      }
    }

    if (lm_state.avg_count >= LM_AVG) {
      params.Lm = lm_state.avg_Lm / (float)LM_AVG;
      lm_state.done = 1;
      lm_state.step = 4;
    }
    else if (measure_ok) {
      lm_state.timer = 0.0f;
      lm_state.step = 0;
    }
    else {
      lm_state.timer = 0.0f;
      lm_state.step = 0;
    }
    break;
  }

  case 4:
    *id_ref = 0.0f;
    break;

  default:
    *id_ref = 0.0f;
    lm_state.step = 0;
    break;
  }
}

/* ============================================
 * ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜
 * ============================================ */

 /*
  * estimate_process - ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
  *
  * ˜˜˜˜˜˜˜˜˜˜: ˜˜˜˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜ ˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜, ˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜
  *            ˜˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜
  *
  * ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜:
  *   vd, vq  - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜ ˜˜˜˜ d,q (˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜)
  *   id, iq  - ˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜ ˜ ˜˜˜˜ d,q (˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜)
  *   dt      - ˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜ (˜˜˜˜˜˜˜)
  *
  * ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜:
  *   vd_ref  - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜ ˜˜˜ d (˜˜˜. ˜˜.)
  *   vq_ref  - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜ ˜˜˜ q (˜˜˜. ˜˜.)
  *   freq_ref - ˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜ ˜˜˜˜ (˜˜)
  */
void estimate_process(float vd, float vq, float id, float iq, float dt,
  float* vd_ref, float* vq_ref, float* freq_ref) {

  *vd_ref = 0;
  *vq_ref = 0;
  if (freq_ref) *freq_ref = 0;

  /* ˜˜˜˜ ˜ ˜˜˜˜˜˜ IDLE - ˜˜˜˜˜˜ ˜˜ ˜˜˜˜˜˜ */
  if (test_step == ESTIMATE_TEST_IDLE) {
    return;
  }

  /* ˜˜˜˜˜ ˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜˜˜ */
  if (in_pause) {
    pause_timer += dt;
    if (pause_timer >= STEP_PAUSE_TIME) {
      in_pause = 0;
      /* ˜˜˜˜˜˜˜ ˜ ˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜˜˜˜ */
      if (test_step == ESTIMATE_TEST_RS) test_step = ESTIMATE_TEST_RR_L;
      else if (test_step == ESTIMATE_TEST_RR_L) test_step = ESTIMATE_TEST_LM;
      else if (test_step == ESTIMATE_TEST_LM) test_step = ESTIMATE_TEST_DONE;
    }
    return;
  }

  switch (test_step) {
  case ESTIMATE_TEST_RS:
    process_Rs(vq, iq, dt, vq_ref, freq_ref);
    if (rs_state.done) {
      in_pause = 1;
      pause_timer = 0;
    }
    break;

  case ESTIMATE_TEST_RR_L:
    process_RrL(vq, iq, dt, vq_ref, freq_ref);
    if (rrl_state.done) {
      in_pause = 1;
      pause_timer = 0;
    }
    break;

  case ESTIMATE_TEST_LM:
    if (vq_ref)
      *vq_ref = 0.0f;
    process_Lm(vd, id, dt, vd_ref, freq_ref);
    if (lm_state.done) {
      in_pause = 1;
      pause_timer = 0;
    }
    break;

  case ESTIMATE_TEST_DONE:
    /* ˜˜˜˜˜˜˜˜˜˜˜˜˜ ˜˜˜˜˜˜˜˜˜ */
    break;

  default:
    break;
  }
}