/*
 * MoveToPoint.hh
 *
 *  Created on: 8 июл. 2020 г.
 *      Author: LeonidTitov
 */

#ifndef UMLIBRARY_TECHNOLOGICAL_FUNCTION_MOVETOPOINT_HH_
#define UMLIBRARY_TECHNOLOGICAL_FUNCTION_MOVETOPOINT_HH_

#include "../ITechFunction.hh"

#include "../../processing/acs/VectorAsyncInterface.hh"

#include "../adapter/ControlSystemWrapper.hpp"

#include "../../systemic/IProcess.hh"
#include "../../systemic/Timer.hh"

namespace technological { namespace function {

class MoveToPoint : public ITechFunction, public systemic::IProcess {
public:
    struct Input {
        float position_stp;     //!<Заданное абсолютное положение, рад.
        float torque_lim;       //!<Ограничение момента, Н*м.
        float speed_lim;        //!<Ограничение скорости, рад/с.
        float acc_lim;          //!<Ограничение ускорения, рад/с^2.
    };

    struct Output {
        float position;         //!<Позиция, которую удалось достичь, рад.
        float setpoint;         //!<Целевая позиция, рад.
    };

    bool run( const char * value, std::size_t size );
    void stop();
    bool getResult( char * value, std::size_t size ) const;
    short getState() const;

    void process();

    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
                     );

    struct Setting {
        float retention_accuracy;   //!<Точность достижения позиции.
        float on_position_timeout;  //!<Время удержания заданного положения, с.

        Setting();
        bool isValid();
    };

    void configure( Setting & config );

    //Статусы ошибок причины незапуска:
    bool isSizeError() const;           //!<Ошибка размера входных данных.
    bool isInvalidInput() const;        //!<Некорретность параметров запуска.
    bool isControlSystemBusy() const;   //!<Ошибка формирования системы управления.

protected:
    virtual void on_start( Input & input, float start_point, systemic::time_t on_position_timeout ) {}
    virtual void in_progress() {}
    virtual void on_end() {}

    enum RunErrors {
        Empty,
        SizeError,
        InvalidInput,
        ControlSystemBusy,
    } run_result;

private:
    bool handle( Input & input );

    Locable< technological::adapter::TieInterface<vector::ITechValue> > wrapper;
    vector::ITechValue * control_system;

    Locable<vector::ITechValue> speed_limit;
    Locable<vector::ITechValue> torque_limit;
    Locable<vector::ITechValue> acc_limit;

    struct {
        float speed_limit = 0.0f;
        float torque_limit = 0.0f;
        float acceleration_limit = 0.0f;
    } stored;

    float setpoint;
    float retention_accuracy;           //!<Точность достижения позиции.

    static bool validate( Input & );
    void apply(Input &input);

    bool moving;

    Output result;

    systemic::Timer on_position_timer;
    systemic::time_t timeout;
};

}}

#endif /* UMLIBRARY_TECHNOLOGICAL_FUNCTION_MOVETOPOINT_HH_ */