#include "motor.hpp"
#include "axis.hpp"
#include "low_level.h"
#include "odrive_main.h"

#include <algorithm>

static constexpr auto CURRENT_ADC_LOWER_BOUND =        (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MIN_VOLT / 3.3f);
static constexpr auto CURRENT_ADC_UPPER_BOUND =        (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MAX_VOLT / 3.3f);

/**
 * @brief This control law adjusts the output voltage such that a predefined
 * current is tracked. A hardcoded integrator gain is used for this.
 * 
 * TODO: this might as well be implemented using the FieldOrientedController.
 */
struct ResistanceMeasurementControlLaw : AlphaBetaFrameController {
    void reset() final {
        test_voltage_ = 0.0f;
        test_mod_ = std::nullopt;
    }

    ODriveIntf::MotorIntf::Error on_measurement(
            std::optional<float> vbus_voltage,
            std::optional<float2D> Ialpha_beta,
            uint32_t input_timestamp) final {

        if (Ialpha_beta.has_value()) {
            actual_current_ = Ialpha_beta->first;
            test_voltage_ += (kI * current_meas_period) * (target_current_ - actual_current_);
            I_beta_ += (kIBetaFilt * current_meas_period) * (Ialpha_beta->second - I_beta_);
        } else {
            actual_current_ = 0.0f;
            test_voltage_ = 0.0f;
        }
    
        if (std::abs(test_voltage_) > max_voltage_) {
            test_voltage_ = NAN;
            return Motor::ERROR_PHASE_RESISTANCE_OUT_OF_RANGE;
        } else if (!vbus_voltage.has_value()) {
            return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
        } else {
            float vfactor = 1.0f / ((2.0f / 3.0f) * *vbus_voltage);
            test_mod_ = test_voltage_ * vfactor;
            return Motor::ERROR_NONE;
        }
    }

    ODriveIntf::MotorIntf::Error get_alpha_beta_output(
            uint32_t output_timestamp,
            std::optional<float2D>* mod_alpha_beta,
            std::optional<float>* ibus) final {
        if (!test_mod_.has_value()) {
            return Motor::ERROR_CONTROLLER_INITIALIZING;
        } else {
            *mod_alpha_beta = {*test_mod_, 0.0f};
            *ibus = *test_mod_ * actual_current_;
            return Motor::ERROR_NONE;
        }
    }

    float get_resistance() {
        return test_voltage_ / target_current_;
    }

    float get_Ibeta() {
        return I_beta_;
    }

    const float kI = 1.0f; // [(V/s)/A]
    const float kIBetaFilt = 80.0f;
    float max_voltage_ = 0.0f;
    float actual_current_ = 0.0f;
    float target_current_ = 0.0f;
    float test_voltage_ = 0.0f;
    float I_beta_ = 0.0f; // [A] low pass filtered Ibeta response
    std::optional<float> test_mod_ = NAN;
};

/**
 * @brief This control law toggles rapidly between positive and negative output
 * voltage. By measuring how large the current ripples are, the phase inductance
 * can be determined.
 * 
 * TODO: this method assumes a certain synchronization between current measurement and output application
 */
struct InductanceMeasurementControlLaw : AlphaBetaFrameController {
    void reset() final {
        attached_ = false;
    }

    ODriveIntf::MotorIntf::Error on_measurement(
            std::optional<float> vbus_voltage,
            std::optional<float2D> Ialpha_beta,
            uint32_t input_timestamp) final
    {
        if (!Ialpha_beta.has_value()) {
            return {Motor::ERROR_UNKNOWN_CURRENT_MEASUREMENT};
        }

        float Ialpha = Ialpha_beta->first;

        if (attached_) {
            float sign = test_voltage_ >= 0.0f ? 1.0f : -1.0f;
            deltaI_ += -sign * (Ialpha - last_Ialpha_);
        } else {
            start_timestamp_ = input_timestamp;
            attached_ = true;
        }

        last_Ialpha_ = Ialpha;
        last_input_timestamp_ = input_timestamp;

        return Motor::ERROR_NONE;
    }

    ODriveIntf::MotorIntf::Error get_alpha_beta_output(
            uint32_t output_timestamp, std::optional<float2D>* mod_alpha_beta,
            std::optional<float>* ibus) final
    {
        test_voltage_ *= -1.0f;
        float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
        *mod_alpha_beta = {test_voltage_ * vfactor, 0.0f};
        *ibus = 0.0f;
        return Motor::ERROR_NONE;
    }

    float get_inductance() {
        // Note: A more correct formula would also take into account that there is a finite timestep.
        // However, the discretisation in the current control loop inverts the same discrepancy
        float dt = (float)(last_input_timestamp_ - start_timestamp_) / (float)TIM_1_8_CLOCK_HZ; // at 216MHz this overflows after 19 seconds
        return std::abs(test_voltage_) / (deltaI_ / dt);
    }

    // Config
    float test_voltage_ = 0.0f;

    // State
    bool attached_ = false;
    float sign_ = 0;

    // Outputs
    uint32_t start_timestamp_ = 0;
    float last_Ialpha_ = NAN;
    uint32_t last_input_timestamp_ = 0;
    float deltaI_ = 0.0f;
};


Motor::Motor(TIM_HandleTypeDef* timer,
             uint8_t current_sensor_mask,
             float shunt_conductance,
             TGateDriver& gate_driver,
             TOpAmp& opamp,
             OnboardThermistorCurrentLimiter& fet_thermistor,
             OffboardThermistorCurrentLimiter& motor_thermistor) :
        timer_(timer),
        current_sensor_mask_(current_sensor_mask),
        shunt_conductance_(shunt_conductance),
        gate_driver_(gate_driver),
        opamp_(opamp),
        fet_thermistor_(fet_thermistor),
        motor_thermistor_(motor_thermistor) {
    apply_config();
    fet_thermistor_.motor_ = this;
    motor_thermistor_.motor_ = this;
}

/**
 * @brief Arms the PWM outputs that belong to this motor.
 *
 * Note that this does not activate the PWM outputs immediately, it just sets
 * a flag so they will be enabled later.
 * 
 * The sequence goes like this:
 *  - Motor::arm() sets the is_armed_ flag.
 *  - On the next timer update event Motor::timer_update_cb() gets called in an
 *    interrupt context
 *  - Motor::timer_update_cb() runs specified control law to determine PWM values
 *  - Motor::timer_update_cb() calls Motor::apply_pwm_timings()
 *  - Motor::apply_pwm_timings() sets the output compare registers and the AOE
 *    (automatic output enable) bit.
 *  - On the next update event the timer latches the configured values into the
 *    active shadow register and enables the outputs at the same time.
 * 
 * The sequence can be aborted at any time by calling Motor::disarm().
 *
 * @param control_law: An control law that is called at the frequency of current
 *        measurements. The function must return as quickly as possible
 *        such that the resulting PWM timings are available before the next
 *        timer update event.
 * @returns: True on success, false otherwise
 */
bool Motor::arm(PhaseControlLaw<3>* control_law) {
    axis_->mechanical_brake_.release();

    CRITICAL_SECTION() {
        control_law_ = control_law;

        // Reset controller states, integrators, setpoints, etc.
        axis_->controller_.reset();
        axis_->acim_estimator_.rotor_flux_ = 0.0f;
        if (control_law_) {
            control_law_->reset();
        }

        if (!odrv.config_.enable_brake_resistor || brake_resistor_armed) {
            armed_state_ = 1;
            is_armed_ = true;
        } else {
            error_ |= Motor::ERROR_BRAKE_RESISTOR_DISARMED;
        }
    }

    return true;
}

/**
 * @brief Updates the phase PWM timings unless the motor is disarmed.
 *
 * If the motor is armed, the PWM timings come into effect at the next update
 * event (and are enabled if they weren't already), unless the motor is disarmed
 * prior to that.
 * 
 * @param tentative: If true, the update is not counted as "refresh".
 */
void Motor::apply_pwm_timings(uint16_t timings[3], bool tentative) {
    CRITICAL_SECTION() {
        if (odrv.config_.enable_brake_resistor && !brake_resistor_armed) {
            disarm_with_error(ERROR_BRAKE_RESISTOR_DISARMED);
        }

        TIM_HandleTypeDef* htim = timer_;
        TIM_TypeDef* tim = htim->Instance;
        tim->CCR1 = timings[0];
        tim->CCR2 = timings[1];
        tim->CCR3 = timings[2];
        
        if (!tentative) {
            if (is_armed_) {
                // Set the Automatic Output Enable so that the Master Output Enable
                // bit will be automatically enabled on the next update event.
                tim->BDTR |= TIM_BDTR_AOE;
            }
        }
        
        // If a timer update event occurred just now while we were updating the
        // timings, we can't be sure what values the shadow registers now contain,
        // so we must disarm the motor.
        // (this also protects against the case where the update interrupt has too
        // low priority, but that should not happen)
        //if (__HAL_TIM_GET_FLAG(htim, TIM_FLAG_UPDATE)) {
        //    disarm_with_error(ERROR_CONTROL_DEADLINE_MISSED);
        //}
    }
}

/**
 * @brief Disarms the motor PWM.
 * 
 * After this function returns, it is guaranteed that all three
 * motor phases are floating and will not be enabled again until
 * arm() is called.
 */
bool Motor::disarm(bool* p_was_armed) {
    bool was_armed;
    
    CRITICAL_SECTION() {
        was_armed = is_armed_;
        if (is_armed_) {
            gate_driver_.set_enabled(false);
        }
        is_armed_ = false;
        armed_state_ = 0;
        TIM_HandleTypeDef* timer = timer_;
        timer->Instance->BDTR &= ~TIM_BDTR_AOE; // prevent the PWMs from automatically enabling at the next update
        __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(timer);
        control_law_ = nullptr;
    }

    // Check necessary to prevent infinite recursion
    if (was_armed) {
        update_brake_current();
    }

    if (p_was_armed) {
        *p_was_armed = was_armed;
    }

    return true;
}

// @brief Tune the current controller based on phase resistance and inductance
// This should be invoked whenever one of these values changes.
// TODO: allow update on user-request or update automatically via hooks
void Motor::update_current_controller_gains() {
    // Calculate current control gains
    float p_gain = config_.current_control_bandwidth * config_.phase_inductance;
    float plant_pole = config_.phase_resistance / config_.phase_inductance;
    current_control_.pi_gains_ = {p_gain, plant_pole * p_gain};
}

bool Motor::apply_config() {
    config_.parent = this;
    is_calibrated_ = config_.pre_calibrated;
    update_current_controller_gains();
    return true;
}

// @brief Set up the gate drivers
bool Motor::setup() {
    fet_thermistor_.update();
    motor_thermistor_.update();

    // Solve for exact gain, then snap down to have equal or larger range as requested
    // or largest possible range otherwise
    constexpr float kMargin = 0.90f;
    constexpr float max_output_swing = 1.35f; // [V] out of amplifier
    float max_unity_gain_current = kMargin * max_output_swing * shunt_conductance_; // [A]
    float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]
    
    float actual_gain;
    if (!gate_driver_.config(requested_gain, &actual_gain))
        return false;

    // Values for current controller
    phase_current_rev_gain_ = 1.0f / actual_gain;
    // Clip all current control to actual usable range
    max_allowed_current_ = max_unity_gain_current * phase_current_rev_gain_;

    max_dc_calib_ = 0.1f * max_allowed_current_;

    if (!gate_driver_.init())
        return false;

    return true;
}

void Motor::disarm_with_error(Motor::Error error){
    error_ |= error;
    last_error_time_ = odrv.n_evt_control_loop_ * current_meas_period;
    disarm();
}

bool Motor::do_checks(uint32_t timestamp) {
    gate_driver_.do_checks();

    if (!gate_driver_.is_ready()) {
        disarm_with_error(ERROR_DRV_FAULT);
        return false;
    }
    if (!motor_thermistor_.do_checks()) {
        disarm_with_error(ERROR_MOTOR_THERMISTOR_OVER_TEMP);
        return false;
    }
    if (!fet_thermistor_.do_checks()) {
        disarm_with_error(ERROR_FET_THERMISTOR_OVER_TEMP);
        return false;
    }
    return true;
}

float Motor::effective_current_lim() {
    // Configured limit
    float current_lim = config_.current_lim;
    // Hardware limit
    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
        current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control
    } else {
        current_lim = std::min(current_lim, axis_->motor_.max_allowed_current_);
    }

    // Apply thermistor current limiters
    current_lim = std::min(current_lim, motor_thermistor_.get_current_limit(config_.current_lim));
    current_lim = std::min(current_lim, fet_thermistor_.get_current_limit(config_.current_lim));
    effective_current_lim_ = current_lim;

    return effective_current_lim_;
}

//return the maximum available torque for the motor.
//Note - for ACIM motors, available torque is allowed to be 0.
float Motor::max_available_torque() {
    if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
        float max_torque = effective_current_lim_ * config_.torque_constant * axis_->acim_estimator_.rotor_flux_;
        max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
        return max_torque;
    } else {
        float max_torque = effective_current_lim_ * config_.torque_constant;
        max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
        return max_torque;
    }
}

std::optional<float> Motor::phase_current_from_adcval(uint32_t ADCValue) {
    // Make sure the measurements don't come too close to the current sensor's hardware limitations
    if (ADCValue < CURRENT_ADC_LOWER_BOUND || ADCValue > CURRENT_ADC_UPPER_BOUND) {
        error_ |= ERROR_CURRENT_SENSE_SATURATION;
        return std::nullopt;
    }

    int adcval_bal = (int)ADCValue - (1 << 11);
    float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
    float shunt_volt = amp_out_volt * phase_current_rev_gain_;
    float current = shunt_volt * shunt_conductance_;
    return current;
}

//--------------------------------
// Measurement and calibration
//--------------------------------

// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
    ResistanceMeasurementControlLaw control_law;
    control_law.target_current_ = test_current;
    control_law.max_voltage_ = max_voltage;

    arm(&control_law);

    for (size_t i = 0; i < 3000; ++i) {
        if (!((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_)) {
            break;
        }
        osDelay(1);
    }

    bool success = is_armed_;

    //// De-energize motor
    //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
    //    return false; // error set inside enqueue_voltage_timings

    disarm();

    config_.phase_resistance = control_law.get_resistance();
    if (is_nan(config_.phase_resistance)) {
        // TODO: the motor is already disarmed at this stage. This is an error
        // that only pretains to the measurement and its result so it should
        // just be a return value of this function.
        disarm_with_error(ERROR_PHASE_RESISTANCE_OUT_OF_RANGE);
        success = false;
    }

    float I_beta = control_law.get_Ibeta();
    if (is_nan(I_beta) || (abs(I_beta) / test_current) > 0.1f) {
        disarm_with_error(ERROR_UNBALANCED_PHASES);
        success = false;
    }

    return success;
}


bool Motor::measure_phase_inductance(float test_voltage) {
    InductanceMeasurementControlLaw control_law;
    control_law.test_voltage_ = test_voltage;

    arm(&control_law);

    for (size_t i = 0; i < 1250; ++i) {
        if (!((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_)) {
            break;
        }
        osDelay(1);
    }

    bool success = is_armed_;

    //// De-energize motor
    //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
    //    return false; // error set inside enqueue_voltage_timings

    disarm();

    config_.phase_inductance = control_law.get_inductance();
    
    // TODO arbitrary values set for now
    if (!(config_.phase_inductance >= 2e-6f && config_.phase_inductance <= 4000e-6f)) {
        error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE;
        success = false;
    }

    return success;
}


// TODO: motor calibration should only be a utility function that's called from
// the UI on explicit user request. It should take its parameters as input
// arguments and return the measured results without modifying any config values.
bool Motor::run_calibration() {
    float R_calib_max_voltage = config_.resistance_calib_max_voltage;
    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT
        || config_.motor_type == MOTOR_TYPE_ACIM) {
        if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
            return false;
        if (!measure_phase_inductance(R_calib_max_voltage))
            return false;
    } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
        // no calibration needed
    } else {
        return false;
    }

    update_current_controller_gains();
    
    is_calibrated_ = true;
    return true;
}

void Motor::update(uint32_t timestamp) {
    // Load torque setpoint, convert to motor direction
    std::optional<float> maybe_torque = torque_setpoint_src_.present();
    if (!maybe_torque.has_value()) {
        error_ |= ERROR_UNKNOWN_TORQUE;
        return;
    }
    float torque = direction_ * *maybe_torque;

    // Load setpoints from previous iteration.
    auto [id, iq] = Idq_setpoint_.previous()
                     .value_or(float2D{0.0f, 0.0f});
    // Load effective current limit
    float ilim = axis_->motor_.effective_current_lim_;

    // Autoflux tracks old Iq (that may be 2-norm clamped last cycle) to make sure we are chasing a feasable current.
    if ((axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) && config_.acim_autoflux_enable) {
        float abs_iq = std::abs(iq);
        float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
        id += gain * (abs_iq - id) * current_meas_period;
        id = std::clamp(id, config_.acim_autoflux_min_Id, 0.9f * ilim); // 10% space reserved for Iq
    } else {
        id = std::clamp(id, -ilim*0.99f, ilim*0.99f); // 1% space reserved for Iq to avoid numerical issues
    }

    // Convert requested torque to current
    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
        iq = torque / (axis_->motor_.config_.torque_constant * std::max(axis_->acim_estimator_.rotor_flux_, config_.acim_gain_min_flux));
    } else {
        iq = torque / axis_->motor_.config_.torque_constant;
    }

    // 2-norm clamping where Id takes priority
    float iq_lim_sqr = SQ(ilim) - SQ(id);
    float Iq_lim = (iq_lim_sqr <= 0.0f) ? 0.0f : sqrt(iq_lim_sqr);
    iq = std::clamp(iq, -Iq_lim, Iq_lim);

    if (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL) {
        Idq_setpoint_ = {id, iq};
    }

    // This update call is in bit a weird position because it depends on the
    // Id,q setpoint but outputs the phase velocity that we depend on later
    // in this function.
    // A cleaner fix would be to take the feedforward calculation out of here
    // and turn it into a separate component.
    MEASURE_TIME(axis_->task_times_.acim_estimator_update)
        axis_->acim_estimator_.update(timestamp);

    float vd = 0.0f;
    float vq = 0.0f;

    std::optional<float> phase_vel = phase_vel_src_.present();

    if (config_.R_wL_FF_enable) {
        if (!phase_vel.has_value()) {
            error_ |= ERROR_UNKNOWN_PHASE_VEL;
            return;
        }

        vd -= *phase_vel * config_.phase_inductance * iq;
        vq += *phase_vel * config_.phase_inductance * id;
        vd += config_.phase_resistance * id;
        vq += config_.phase_resistance * iq;
    }

    if (config_.bEMF_FF_enable) {
        if (!phase_vel.has_value()) {
            error_ |= ERROR_UNKNOWN_PHASE_VEL;
            return;
        }

        vq += *phase_vel * (2.0f/3.0f) * (config_.torque_constant / config_.pole_pairs);
    }
    
    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
        // reinterpret current as voltage
        Vdq_setpoint_ = {vd + id, vq + iq};
    } else {
        Vdq_setpoint_ = {vd, vq};
    }
}


/**
 * @brief Called when the underlying hardware timer triggers an update event.
 */
void Motor::current_meas_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current) {
    // TODO: this is platform specific
    //const float current_meas_period = static_cast<float>(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ;
    TaskTimerContext tmr{axis_->task_times_.current_sense};

    n_evt_current_measurement_++;

    bool dc_calib_valid = (dc_calib_running_since_ >= config_.dc_calib_tau * 7.5f)
                       && (abs(DC_calib_.phA) < max_dc_calib_)
                       && (abs(DC_calib_.phB) < max_dc_calib_)
                       && (abs(DC_calib_.phC) < max_dc_calib_);

    if (armed_state_ == 1 || armed_state_ == 2) {
        current_meas_ = {0.0f, 0.0f, 0.0f};
        armed_state_ += 1;
    } else if (current.has_value() && dc_calib_valid) {
        current_meas_ = {
            current->phA - DC_calib_.phA,
            current->phB - DC_calib_.phB,
            current->phC - DC_calib_.phC
        };
    } else {
        current_meas_ = std::nullopt;
    }

    // Run system-level checks (e.g. overvoltage/undervoltage condition)
    // The motor might be disarmed in this function. In this case the
    // handler will continue to run until the end but it won't have an
    // effect on the PWM.
    odrv.do_fast_checks();

    if (current_meas_.has_value()) {
        // Check for violation of current limit
        // If Ia + Ib + Ic == 0 holds then we have:
        // Inorm^2 = Id^2 + Iq^2 = Ialpha^2 + Ibeta^2 = 2/3 * (Ia^2 + Ib^2 + Ic^2)
        float Itrip = effective_current_lim_ + config_.current_lim_margin;
        float Inorm_sq = 2.0f / 3.0f * (SQ(current_meas_->phA)
                                      + SQ(current_meas_->phB)
                                      + SQ(current_meas_->phC));

        // Hack: we disable the current check during motor calibration because
        // it tends to briefly overshoot when the motor moves to align flux with I_alpha
        if (Inorm_sq > SQ(Itrip)) {
            disarm_with_error(ERROR_CURRENT_LIMIT_VIOLATION);
        }
    } else if (is_armed_) {
        // Since we can't check current limits, be safe for now and disarm.
        // Theoretically we could continue to operate if there is no active
        // current limit.
        disarm_with_error(ERROR_UNKNOWN_CURRENT_MEASUREMENT);
    }

    if (control_law_) {
        Error err = control_law_->on_measurement(vbus_voltage,
                            current_meas_.has_value() ?
                                std::make_optional(std::array<float, 3>{current_meas_->phA, current_meas_->phB, current_meas_->phC})
                                : std::nullopt,
                            timestamp);
        if (err != ERROR_NONE) {
            disarm_with_error(err);
        }
    }
}

/**
 * @brief Called when the underlying hardware timer triggers an update event.
 */
void Motor::dc_calib_cb(uint32_t timestamp, std::optional<Iph_ABC_t> current) {
    const float dc_calib_period = static_cast<float>(2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) / TIM_1_8_CLOCK_HZ;
    TaskTimerContext tmr{axis_->task_times_.dc_calib};

    if (current.has_value()) {
        const float calib_filter_k = std::min(dc_calib_period / config_.dc_calib_tau, 1.0f);
        DC_calib_.phA += (current->phA - DC_calib_.phA) * calib_filter_k;
        DC_calib_.phB += (current->phB - DC_calib_.phB) * calib_filter_k;
        DC_calib_.phC += (current->phC - DC_calib_.phC) * calib_filter_k;
        dc_calib_running_since_ += dc_calib_period;
    } else {
        DC_calib_.phA = 0.0f;
        DC_calib_.phB = 0.0f;
        DC_calib_.phC = 0.0f;
        dc_calib_running_since_ = 0.0f;
    }
}


void Motor::pwm_update_cb(uint32_t output_timestamp) {
    TaskTimerContext tmr{axis_->task_times_.pwm_update};
    n_evt_pwm_update_++;

    Error control_law_status = ERROR_CONTROLLER_FAILED;
    float pwm_timings[3] = {NAN, NAN, NAN};
    std::optional<float> i_bus;

    if (control_law_) {
        control_law_status = control_law_->get_output(
            output_timestamp, pwm_timings, &i_bus);
    }

    // Apply control law to calculate PWM duty cycles
    if (is_armed_ && control_law_status == ERROR_NONE) {
        uint16_t next_timings[] = {
            (uint16_t)(pwm_timings[0] * (float)TIM_1_8_PERIOD_CLOCKS),
            (uint16_t)(pwm_timings[1] * (float)TIM_1_8_PERIOD_CLOCKS),
            (uint16_t)(pwm_timings[2] * (float)TIM_1_8_PERIOD_CLOCKS)
        };
        apply_pwm_timings(next_timings, false);
    } else if (is_armed_) {
        if (!(timer_->Instance->BDTR & TIM_BDTR_MOE) && (control_law_status == ERROR_CONTROLLER_INITIALIZING)) {
            // If the PWM output is armed in software but not yet in
            // hardware we tolerate the "initializing" error.
            i_bus = 0.0f;
        } else {
            disarm_with_error(control_law_status);
        }
    }

    if (!is_armed_) {
        // If something above failed, reset I_bus to 0A.
        i_bus = 0.0f;
    } else if (is_armed_ && !i_bus.has_value()) {
        // If the motor is armed then i_bus must be known
        disarm_with_error(ERROR_UNKNOWN_CURRENT_MEASUREMENT);
        i_bus = 0.0f;
    }

    I_bus_ = *i_bus;

    if (*i_bus < config_.I_bus_hard_min || *i_bus > config_.I_bus_hard_max) {
        disarm_with_error(ERROR_I_BUS_OUT_OF_RANGE);
    }

    update_brake_current();
}