/*
 * SearchCondition.cpp
 *
 *  Created on: 5 èþë. 2020 ã.
 *      Author: LeonidTitov
 */

#include "SearchCondition.hh"

#include <cmath>
#include <cstring>

void technological::function::SearchCondition::apply(const 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.delta_stp + control_system->get())));
    searching = true;
}

bool technological::function::SearchCondition::hadnle( const 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 && (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::SearchCondition::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 hadnle(input) )
        return not(run_result = ControlSystemBusy);

    return not(run_result = Empty);
}

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

    run_result = Empty;

    if( not control_system )
        return;

    searching = false;
    setpoint = result.position = result.value = 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();

    wrapper.unlock();

}

bool technological::function::SearchCondition::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::SearchCondition::getState() const {

    using namespace std;

    if(control_system) {

        if( searching )
            return ITechFunction::EXECUTE;

        else if( isfinite(result.value) )
            return ITechFunction::FINISHED;

        else
            return ITechFunction::FAILURE;

    } else
        return ITechFunction::DISABLE;

}

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

    using namespace std;

    if( searching and control_system ) {

        if( condition ) {
            result.value = value;
            result.position = control_system->get();

            searching = false;

            control_system->set( control_system->get() );

        } else if( fabsf( control_system->get() - setpoint ) < retention_accuracy ) {

            result.value = NAN;
            result.position = NAN;

            searching = false;

        }
    }
}

technological::function::SearchCondition::SearchCondition(
        ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
        ResourceKeeper<vector::ITechValue> & speed_limit,
        ResourceKeeper<vector::ITechValue> & torque_limit,
        ResourceKeeper<vector::ITechValue> & acc_limit,
        systemic::IStatus &condition, systemic::ISignal & value) : wrapper(control_system_wrapper),
                speed_limit(speed_limit), torque_limit(torque_limit), acc_limit(acc_limit),
                condition(condition), value(value),
                setpoint(NAN), searching(false), retention_accuracy(0.0f),
                stored(), run_result(Empty) {

    result.value = NAN;
    result.position = NAN;

}

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

    retention_accuracy = config.retention_accuracy;

}

bool technological::function::SearchCondition::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( input.delta_stp == 0.0f or not isfinite( input.delta_stp ) )
        return false;

    return true;
}

technological::function::SearchCondition::Setting::Setting() : retention_accuracy(0.0f) {}

bool technological::function::SearchCondition::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;

    return retention_accuracy != not_valid.retention_accuracy;

}

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

    return run_result == SizeError;

}

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

    return run_result == InvalidInput;

}

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

    return run_result == ControlSystemBusy;

}