/*
 * SpeedControlProcess.cpp
 *
 *  Created on: 18 ñåíò. 2019 ã.
 *      Author: titov
 */

#include "SpeedControlProcess.hh"

void processing::acs::SpeedControlProcess::set(
        control::PhaseSpacePoint<float> setpoint ) {

    speed_stp = setpoint;

    if( isConnected() ) proc_enable = true;
}

control::PhaseSpaceValue processing::acs::SpeedControlProcess::get() const {

//    return read_from_fast( &torque_stp );
    return control::PhaseSpaceValue( speed_fdb, 0.0f ); //todo: Óñêîðåíèå ñåòèøü? ÃÅÒÜ!

}

void processing::acs::SpeedControlProcess::set_output(
        SetInterface * output_interface ) {

    if( not output_interface ) proc_enable = false;

    output = output_interface;

}

void processing::acs::SpeedControlProcess::setSampleTime( float ts_in_second ) {

    speed_control.setTime(ts_in_second);

}

void processing::acs::SpeedControlProcess::process() {

    if( not proc_enable )
        return;

    speed_stp.update();
    feedforward.stp.update();

    torque_stp = speed_control.update( speed_stp, speed_fdb, feedforward.stp );

    output->set( torque_stp );

}

void processing::acs::SpeedControlProcess::reset() {

    proc_enable = false;

    speed_stp = control::PhaseSpaceValue(0, 0);
    torque_stp = control::PhaseSpaceValue(0, 0);
    speed_control.reset();

}

processing::acs::SpeedControlProcess::SpeedControlProcess(
        control::regulator::SpeedController & speed_reg, systemic::ISignal & speed_fdb ) :
            speed_control(speed_reg), speed_fdb(speed_fdb), output(nullptr),
            speed_stp(control::PhaseSpaceValue(0, 0)), torque_stp(control::PhaseSpaceValue(0, 0)),
            proc_enable(false), torque_up_limit(speed_reg), torque_down_limit(speed_reg), inertia_value(speed_reg) {}

vector::ITechValue & processing::acs::SpeedControlProcess::getTorqueUpLimit() {

    return torque_up_limit;

}

vector::ITechValue & processing::acs::SpeedControlProcess::getTorqueDownLimit() {

    return torque_down_limit;

}

vector::ITechValue & processing::acs::SpeedControlProcess::getInertiaValue() {

    return inertia_value;

}

bool processing::acs::SpeedControlProcess::isConnected() const {

    return output;

}

vector::IPhaseSpaceValue & processing::acs::SpeedControlProcess::getFeedforwardTract() {

    return feedforward;

}

const float & processing::acs::SpeedControlProcess::demand() const {

    return speed_stp.value;

}