/*
 * ForceEstimation.hh
 *
 *  Created on: 4 апр. 2022 г.
 *      Author: titov
 */

#ifndef UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_
#define UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_

#include "../systemic/ISignal.hh"
#include "../systemic/IProcess.hh"
#include "acs/VectorAsyncInterface.hh"

namespace processing {

//!Модуль осуществляет обработку датчика усилия.
class ForceEstimation : public systemic::IProcess {

    systemic::ISignal & speed;
    systemic::ISignal & position;
    systemic::ISignal & force;

    float force_prev = 0.0f;

    struct Estimator : vector::ITechValue  {

        float force_est = 0.0f;
        float khardness_est = 0.0f;
        float position_est = 0.0f;
        float touch_est = 0.0f;

        float L1P = 0.0f;
        float L2P = 0.0f;

        float L1N = 0.0f;
        float L2N = 0.0f;

        float permit_error = 0.0f;
        float intolerable_displacement = 0.0f;

        float dead_displacement = 0.0f;

        float prev_displacement = 0.0f;
        float prev_force_error = 0.0f;

        void estimate( float value_position, float value_force );
        float predict( float value_position );
        void reset();

        float transmission_ratio;
        float _1_transmission_ratio;

        void set( float khardness ) {
            khardness_est = khardness * transmission_ratio;
            intolerable_displacement = permit_error / khardness_est;
        }

        float get() const {
            return khardness_est * _1_transmission_ratio;
        }

    } est;

    float force_est;
    float force_rate_est;

    float _Ts = 0.0f;
    float delay;
    float min_force;

public:
    struct Setting {

        float min_force;                //!<Минимальное усилие, с которого начинается работа наблюдателя (Н).
        float khardness;                //!<Коэффициент жесткости (Н/м).
        float force_delay;              //!<Задержка сигнала усилия (с).

        float transmission_ratio;       //!<Передаточный коэффициент (м/рад).

        float L1P;                       //!<Коэффициет следящей системы оценки усилия. Коррекция оценки усилия при положительном смещении
        float L2P;                       //!<Коэффициет следящей системы оценки усилия. Коррекция коэффициента жесткости при положительном смещении.

        float L1N;                       //!<Коэффициет следящей системы оценки усилия. Коррекция оценки усилия при отрицательном смещении
        float L2N;                       //!<Коэффициет следящей системы оценки усилия. Коррекция коэффициента жесткости при отрицательном смещении.

        float permit_error;             //!<Допустимая ошибка следящей системы.

        Setting();
        bool isValid();
    };

    void configure( Setting & config );

public:
    ForceEstimation( systemic::ISignal & speed, systemic::ISignal & force, systemic::ISignal & position );

    const float & getEstimatedForce() const;
    const float & getEstimatedForceRate() const;
    const float & getEstimatedKhardness() const;

    vector::ITechValue & getKhardness();

    void process();
    void setSampleTime( float ts_in_second );

};

}



#endif /* UMLIBRARY_PROCESSING_FORCEESTIMATION_HH_ */