/*
 * TorqueEstimation.cpp
 *
 *  Created on: 29 дек. 2020 г.
 *      Author: titov
 */

#include "LoadEstimation.hh"

bool processing::LoadEstimation::Setting::isValid() {

    const Setting non_valid;

    if( Jp <= 0.0f )
        Jp = non_valid.Jp;

    if( L1 <= 0.0f )
        L1 = non_valid.L1;

    if( L2 <= 0.0f )
        L2 = non_valid.L2;

    return Jp != non_valid.Jp
            and L1 != non_valid.L1
            and L2 != non_valid.L2;

}

processing::LoadEstimation::Setting::Setting() : Jp(0.0f), L1(0.0f), L2(0.0f) {}

void processing::LoadEstimation::configure( const Setting & setting ) {

    _L1Base = setting.L1;
    _L2Base = setting.L2;
    _Base_J = 1.0f / setting.Jp;

    _L1Ts = _L1Base * _Ts;
    _L2Ts = _L2Base * _Ts;
    _Ts_J = _Base_J * _Ts;

}

void processing::LoadEstimation::process() {

    const float error = ( speed - est_speed );

    est_speed   += error * _L1Ts + ( torque - est_load ) * _Ts_J;
    est_load    -= error * _L2Ts;

}

void processing::LoadEstimation::setSampleTime( float ts_in_second ) {

    _Ts = ts_in_second;

    _L1Ts = _L1Base * _Ts;
    _L2Ts = _L2Base * _Ts;
    _Ts_J = _Base_J * _Ts;

}

const float & processing::LoadEstimation::getEstimatedLoad() const {

    return est_load;

}

const float & processing::LoadEstimation::getEstimatedSpeed() const {

    return est_speed;

}

processing::LoadEstimation::LoadEstimation( systemic::ISignal & speed,
                                                systemic::ISignal & torque ) : speed(speed), torque(torque) {}