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

#include "MoveToPointWithTimeout.hh"

#include "../../math/math_inc.hh"

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

    MoveToPoint::Setting basic_setting;

    basic_setting.on_position_timeout = config.on_position_timeout;
    basic_setting.retention_accuracy = config.retention_accuracy;

    MoveToPoint::configure( basic_setting );

    permit_speed_factor = config.permit_speed_factor;
    permit_time = config.permit_time;

}

void technological::function::MoveToPointWithTimeout::in_progress() {

    if( compite_timer.delayFinished() ) {

        move_failure = true;

        MoveToPoint::stop();

    }

}

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

    return move_failure ? FAILURE : MoveToPoint::getState();

}

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

    compite_timer.stop();

    MoveToPoint::stop();

    move_failure = false;

}

void technological::function::MoveToPointWithTimeout::on_start( Input & input, float start_point, systemic::time_t on_position_timeout ) {

    float move_time = math::function::get_time(
                fabsf( input.position_stp - start_point ),
                input.speed_lim * permit_speed_factor,
                input.acc_lim
            );

    systemic::time_t complite_timeout = systemic::seconds2time( move_time + permit_time ) + on_position_timeout;
    compite_timer.start( complite_timeout );

}

technological::function::MoveToPointWithTimeout::Setting::Setting() : permit_speed_factor(0.0f), permit_time(0.0f) {}

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

    const Setting non_valid;

    if( permit_speed_factor <= 0.0f || permit_speed_factor > 1.0f )
        permit_speed_factor = non_valid.permit_speed_factor;

    if( permit_time <= 0.0f )
        permit_time = non_valid.permit_time;

    MoveToPoint::Setting basic_setting;

    basic_setting.on_position_timeout = on_position_timeout;
    basic_setting.retention_accuracy = retention_accuracy;

    bool result = basic_setting.isValid();

    on_position_timeout = basic_setting.on_position_timeout;
    retention_accuracy = basic_setting.retention_accuracy;

    return result and permit_time != non_valid.permit_time and permit_speed_factor != non_valid.permit_speed_factor;

}

technological::function::MoveToPointWithTimeout::MoveToPointWithTimeout(
        ResourceKeeper< technological::adapter::TieInterface<vector::ITechValue> > & control_system_wrapper,
        ResourceKeeper<vector::ITechValue> & speed_limit,
        ResourceKeeper<vector::ITechValue> & torque_limit,
        ResourceKeeper<vector::ITechValue> & acc_limit) : MoveToPoint(control_system_wrapper, speed_limit, torque_limit, acc_limit ),
                move_failure(false), permit_speed_factor(1.0f), permit_time(0.0f), compite_timer() {}