/*
 * ForceControllerAutotuningFeatureEstimate.cpp
 *
 *  Created on: 4 мая 2022 г.
 *      Author: titov
 */

#include "ForceControllerAutotuningFeatureEstimate.hh"

processing::ForceControllerAutotuningFeatureEstimate::ForceControllerAutotuningFeatureEstimate(
        systemic::ISignal & force, systemic::ISignal & dforce,
        const control::regulator::ForceController & fc ) : force(force), dforce(dforce), fc(fc),
        dynamic_error(0.0f), aggregated_dynamic_error(0.0f) {}

const float & processing::ForceControllerAutotuningFeatureEstimate::getDynamicError() const {

    return dynamic_error;

}

const float & processing::ForceControllerAutotuningFeatureEstimate::getEndMismatch() const {

    return fc.getEndMismatch();

}

void processing::ForceControllerAutotuningFeatureEstimate::reset() {

    dynamic_error = 0.0f;
    aggregated_dynamic_error = 0.0f;

}

void processing::ForceControllerAutotuningFeatureEstimate::process() {

    dynamic_error = fc.getDynamicError(dforce);
    aggregated_dynamic_error += dynamic_error * _Ts;

}

const float & processing::ForceControllerAutotuningFeatureEstimate::getAggregatedDynamicError() const {

    return aggregated_dynamic_error;

}

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

    _Ts = ts_in_second;

}