/*
 * TorqueLimitation.cpp
 *
 *  Created on: 29 δεκ. 2020 γ.
 *      Author: titov
 */

#include "TorqueLimitation.hh"
#include "../math/math_inc.hh"

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

    _K = setting.overload_coefficient;
    _Kp1 = _K + 1;

    _OffsetUp_k = setting.positive_force_offset_k;
    _OffsetUp_b = setting.positive_force_offset_b;
    _OffsetDown_k = setting.negative_force_offset_k;
    _OffsetDown_b = setting.negative_force_offset_b;

    _max_torque = setting.max_torque;

    set(_max_torque);

}

void processing::TorqueLimitation::process() {

    using namespace std;

    float friction_smp = friction;
    float load_smp = load;
    float dynamic_torque_smp = dynamic_torque;

    float up_max_torque = math::function::saturation(
            lim_act_up - _K * ( load_smp - friction_smp ), torque_lim_act_up + fmaxf(dynamic_torque_smp + friction_smp, 0), torque_lim_act_up );
    float down_max_torque = math::function::saturation(
            -lim_act_down - _K * ( load_smp - friction_smp ), -torque_lim_act_down, -torque_lim_act_down + fminf(dynamic_torque_smp + friction_smp, 0) );

    torque_up_limit.set( actual_up_limit = up_max_torque );
    torque_down_limit.set( actual_down_limit = - down_max_torque );
}

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

    using namespace std;

    const Setting not_valid;

    if( overload_coefficient < 0.0f )
        overload_coefficient = not_valid.overload_coefficient;

    if( max_torque < 0.0f )
        max_torque = not_valid.max_torque;

    if( not isfinite( positive_force_offset_k ) )
        positive_force_offset_k = not_valid.positive_force_offset_k;

    if( not isfinite( negative_force_offset_k ) )
        negative_force_offset_k = not_valid.negative_force_offset_k;

    if( not isfinite( positive_force_offset_b ) )
        positive_force_offset_b = not_valid.positive_force_offset_b;

    if( not isfinite( negative_force_offset_b ) )
        negative_force_offset_b = not_valid.negative_force_offset_b;

    return overload_coefficient != not_valid.overload_coefficient
            and max_torque != not_valid.max_torque
            and positive_force_offset_k != not_valid.positive_force_offset_k
            and negative_force_offset_k != not_valid.negative_force_offset_k
            and positive_force_offset_b != not_valid.positive_force_offset_b
            and negative_force_offset_b != not_valid.negative_force_offset_b;

}

processing::TorqueLimitation::Setting::Setting() : overload_coefficient(NAN), max_torque(0.0f),
        positive_force_offset_k(0.0f), positive_force_offset_b(NAN),
        negative_force_offset_k(0.0f), negative_force_offset_b(NAN) {}

void processing::TorqueLimitation::setSampleTime( float ts_in_second ) {}

void processing::TorqueLimitation::set( control::Value limit ) {

    using namespace std;

    float torque_limited = fminf( fabsf( limit ), _max_torque );

    torque_lim_act_up = torque_limited * _OffsetUp_k + _OffsetUp_b;
    torque_lim_act_down = torque_limited * _OffsetDown_k + _OffsetDown_b;

    lim_act_up = torque_lim_act_up * _Kp1;
    lim_act_down = torque_lim_act_down * _Kp1;

    demand_limit = torque_limited;
}

control::Value processing::TorqueLimitation::get() const {

    return demand_limit;

}

processing::TorqueLimitation::TorqueLimitation(
        vector::ITechValue & up_limit, vector::ITechValue & down_limit,
        systemic::ISignal & friction, systemic::ISignal & dynamic_torque, systemic::ISignal & load ) :
                torque_up_limit(up_limit), torque_down_limit(down_limit),
                friction(friction), dynamic_torque(dynamic_torque), load(load),
                lim_act_up(0.0f), lim_act_down(0.0f), torque_lim_act_up(0.0f),
                torque_lim_act_down(0.0f) {}

const float & processing::TorqueLimitation::getActualUpLimit() const {

    return actual_up_limit;

}

const float & processing::TorqueLimitation::getActualDownLimit() const {

    return actual_down_limit;

}