/*
 * PositionControlProcess.cpp
 *
 *  Created on: 23 сент. 2019 г.
 *      Author: titov
 */

#include "PositionControlProcess.hh"

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

    position_stp = setpoint;

    if( output ) proc_enable = true;

}

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

    return control::PhaseSpaceValue( position_fdb, speed_fdb );


}

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

    if( output_interface ) proc_enable = false;

    output = output_interface;

}

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

    position_control.setTime(ts_in_second);

}

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

    if( not proc_enable )
        return;

    position_stp.update();

    speed_stp = position_control.update( position_stp, position_fdb, speed_fdb );

    output->set( speed_stp );

}

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

    proc_enable = false;

    position_stp = control::PhaseSpaceValue(0, 0);
    speed_stp = control::PhaseSpaceValue(0, 0);
    position_control.reset();

}

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

    return position_stp.value;

}

processing::acs::PositionControlProcess::PositionControlProcess(
        PositionRegulator & position_reg,
        systemic::ISignal & position_fdb, systemic::ISignal & speed_fdb ) :
                position_control(position_reg),
                position_fdb(position_fdb), speed_fdb(speed_fdb),
                position_stp(control::PhaseSpaceValue(0, 0)), speed_stp(
                control::PhaseSpaceValue(0, 0)), proc_enable(false),

        acc_limit(position_reg), speed_limit(position_reg) {}

vector::ITechValue & processing::acs::PositionControlProcess::getAccelerationLimit() {

    return acc_limit;

}

vector::ITechValue & processing::acs::PositionControlProcess::getSpeedLimit() {

    return speed_limit;

}