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

#include "ForceEstimation.hh"

processing::ForceEstimation::ForceEstimation(systemic::ISignal & speed,
                                             systemic::ISignal & force,
                                             systemic::ISignal & position ) :
                                             speed(speed), force(force), position(position), est() {}

const float & processing::ForceEstimation::getEstimatedForce() const {

    return force_est;

}

const float & processing::ForceEstimation::getEstimatedForceRate() const {

    return force_rate_est;

}

const float & processing::ForceEstimation::getEstimatedKhardness() const {

    return est.khardness_est;

}

vector::ITechValue & processing::ForceEstimation::getKhardness() {

    return est;

}

void processing::ForceEstimation::configure( Setting & config ) {

    delay = config.force_delay;
    min_force = config.min_force;

    est.L1N = config.L1N;
    est.L2N = config.L2N;

    est.L1P = config.L1P;
    est.L2P = config.L2P;

    est.permit_error = config.permit_error;

    est.transmission_ratio = config.transmission_ratio;
    est._1_transmission_ratio = 1.0f / config.transmission_ratio;

    est.set( config.khardness );

    const float minimal_force_delta = 2.0f;
    est.dead_displacement = minimal_force_delta / ( config.khardness * config.transmission_ratio );

}

void processing::ForceEstimation::process() {

    using namespace std;

    float force_value = force;

    if( isfinite(force_value) ) {

        if( force_value > min_force ) {

            if( force_prev != force_value )
                est.estimate( position, force_value );

            force_est = est.predict( position + speed * delay );


            force_prev = force_value;

        } else {

            force_prev = 0.0f;
            force_est = 0.0f;

            est.force_est = min_force;
            est.position_est = position;

            est.prev_displacement = 0.0f;
            est.prev_force_error = 0.0f;

        }

    }

    force_rate_est = speed * est.khardness_est;

}

processing::ForceEstimation::Setting::Setting() :
        min_force(-INFINITY), khardness(-INFINITY), force_delay(-INFINITY), transmission_ratio(0.0f),
        L1P(-INFINITY), L2P(-INFINITY), L1N(-INFINITY), L2N(-INFINITY), permit_error(-INFINITY) {}

bool processing::ForceEstimation::Setting::isValid() {
    using namespace std;

    using namespace std;

    const Setting non_valid;

    if( min_force < 0.0f or not isfinite(min_force) )
        min_force = non_valid.min_force;

    if( khardness < 0.0f or not isfinite(khardness) )
        khardness = non_valid.khardness;

    if( force_delay < 0.0f or not isfinite(force_delay) )
        force_delay = non_valid.force_delay;

    if( transmission_ratio <= 0.0f or not isfinite(transmission_ratio) )
        transmission_ratio = non_valid.transmission_ratio;

    if( L1P < 0.0f or not isfinite(L1P) )
        L1P = non_valid.L1P;

    if( L2P < 0.0f or not isfinite(L2P) )
        L2P = non_valid.L2P;

    if( L1N < 0.0f or not isfinite(L1N) )
        L1P = non_valid.L1N;

    if( L2N < 0.0f or not isfinite(L2N) )
        L2N = non_valid.L2N;

    if( permit_error <= 0.0f or not isfinite(permit_error) )
        permit_error = non_valid.permit_error;

    return min_force != non_valid.min_force and
            khardness != non_valid.khardness and
            force_delay != non_valid.force_delay and
            transmission_ratio != non_valid.transmission_ratio and
            L1P != non_valid.L1P and
            L2P != non_valid.L2P and
            L1N != non_valid.L1N and
            L2N != non_valid.L2N and
            permit_error != non_valid.permit_error;

}

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

    _Ts = ts_in_second;

}

void processing::ForceEstimation::Estimator::estimate(float value_position,
                                                      float value_force) {

    using namespace std;

    float displacement = value_position - position_est;
    float force_perdicted = force_est + khardness_est * displacement;
    float force_error = value_force - force_perdicted;

    float L1 = ( displacement > 0.0f ? L1P : L1N );
    float L2 = fabsf(displacement) > dead_displacement ? ( displacement > 0.0f ? L2P : L2N ) : 0.0f;

    //Update logic
    if( fabsf(force_error) < permit_error and
            fabsf(displacement) < intolerable_displacement ) {

        khardness_est = khardness_est + force_error * L2;
        force_est = force_perdicted + force_error * L1;
        position_est = value_position;

    }

    //Reset logic on big-error
    if( ( fabsf(displacement) >= intolerable_displacement
           and fabsf(prev_displacement) >= intolerable_displacement ) or
        (  fabsf(force_error) >= permit_error
           and fabsf(prev_force_error) >= permit_error ) ) {

        force_est = value_force;
        position_est = value_position;

    }

    prev_displacement = displacement;
    prev_force_error  = force_error ;

}

float processing::ForceEstimation::Estimator::predict(float value_position) {

    float displacement = value_position - position_est;
    return force_est + displacement * khardness_est;

}