/*
 * MoveToPoint.cpp
 *
 *  Created on: 8 èþë. 2020 ã.
 *      Author: LeonidTitov
 */

#include "MoveToPoint.hh"

#include <cmath>
#include <cstring>

bool technological::function::MoveToPoint::run( const char * value,
                                                std::size_t size    ) {

    if( sizeof(Input) != size )
        return not(run_result = SizeError);

    Input input;
    std::memcpy( &input, value, size );

    if( not validate(input) )
        return not(run_result = InvalidInput);

    if( not handle(input) )
        return not(run_result = ControlSystemBusy);

    return not(run_result = Empty);

}

void technological::function::MoveToPoint::stop() {

    if( not control_system )
        return;

    moving = false;

    setpoint = result.position = result.setpoint = NAN;

    control_system = nullptr;
    wrapper->disconnect();

    torque_limit->set( stored.torque_limit );
    torque_limit.unlock();

    acc_limit->set( stored.acceleration_limit );
    acc_limit.unlock();

    speed_limit->set( stored.speed_limit );
    speed_limit.unlock();

    on_end();

    wrapper.unlock();

}

bool technological::function::MoveToPoint::getResult( char * value,
                                                      std::size_t size ) const {

    if( size != sizeof(Output) )
        return false;

    Output output;

    output = result;

    std::memcpy( value, &output, sizeof(Output) );

    return true;

}

short technological::function::MoveToPoint::getState() const {

    using namespace std;

    if( control_system and not moving )
        return ITechFunction::FINISHED;

    if( control_system and moving )
        return ITechFunction::EXECUTE;

    if( not control_system )
        return ITechFunction::DISABLE;

}

void technological::function::MoveToPoint::process() {

    using namespace std;

    if( moving and control_system ) {

        if( fabsf( control_system->get() - setpoint ) < retention_accuracy ) {
            if( not on_position_timer.isActive() )
                on_position_timer.start(timeout);
        } else
            on_position_timer.stop();

        if( on_position_timer.delayFinished() ) {

            result.position = control_system->get();
            result.setpoint = setpoint;

            moving = false;

        } else in_progress();
    }
}

technological::function::MoveToPoint::MoveToPoint(
        ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
        ResourceKeeper<vector::ITechValue> & speed_limit,
        ResourceKeeper<vector::ITechValue> & torque_limit,
        ResourceKeeper<vector::ITechValue> & acc_limit ) :
                wrapper(control_system_wrapper),
                speed_limit(speed_limit), torque_limit(torque_limit), acc_limit(acc_limit),
                setpoint(NAN), moving(false), retention_accuracy(0.0f),
                stored(), run_result(Empty) {

    result.position = NAN;
    result.setpoint = NAN;

}

technological::function::MoveToPoint::Setting::Setting() : retention_accuracy(0.0f), on_position_timeout(0.0f) {}

bool technological::function::MoveToPoint::Setting::isValid() {

    using namespace std;

    const Setting not_valid;

    if( retention_accuracy <= 0.0f or not isfinite(retention_accuracy) )
        retention_accuracy = not_valid.retention_accuracy;

    if( on_position_timeout <= 0.0f or not isfinite(retention_accuracy) )
        on_position_timeout = not_valid.on_position_timeout;

    return retention_accuracy != not_valid.retention_accuracy
            and on_position_timeout != not_valid.on_position_timeout;

}

void technological::function::MoveToPoint::configure( Setting & config ) {

    retention_accuracy = config.retention_accuracy;
    timeout = systemic::seconds2time( config.on_position_timeout );

}

void technological::function::MoveToPoint::apply( Input & input ) {

    acc_limit->set(input.acc_lim);
    torque_limit->set(input.torque_lim);
    speed_limit->set(input.speed_lim);

    control_system->set(setpoint = input.position_stp);

    moving = true;

    on_position_timer.stop();

    on_start(input, control_system->get(), timeout);

}

bool technological::function::MoveToPoint::handle( Input & input ) {

    if( control_system ) {

        apply(input);

    } else {

        ScopeLock<
            Locable<vector::ITechValue>,
            Locable<vector::ITechValue>,
            Locable<vector::ITechValue>,
            Locable< technological::adapter::TieInterface<vector::ITechValue> >
        > lock( acc_limit, speed_limit, torque_limit, wrapper );

        if( lock and (control_system = wrapper->try_connect()) ) {

            stored.acceleration_limit = acc_limit->get();
            stored.speed_limit = speed_limit->get();
            stored.torque_limit = torque_limit->get();

            apply(input);

            lock.freeze();

        }

    }

    return bool(control_system);

}

bool technological::function::MoveToPoint::validate( Input & input ) {

    using namespace std;

    if( input.acc_lim <= 0.0f or input.speed_lim <= 0.0f or input.torque_lim <= 0.0f )
        return false;

    if( not isfinite( input.position_stp ) )
        return false;

    return true;
}

bool technological::function::MoveToPoint::isSizeError() const {

    return run_result == SizeError;

}

bool technological::function::MoveToPoint::isInvalidInput() const {

    return run_result == InvalidInput;

}

bool technological::function::MoveToPoint::isControlSystemBusy() const {

    return run_result == ControlSystemBusy;

}