mountcontrol/mcc/mcc_generic_mount.h

#pragma once

/*                          MOUNT CONTROL COMPONENTS LIBRARY                            */


/*                          GENERIC MOUNT REFERENCE IMPLEMENTATION                          */


#include "mcc_generics.h"


namespace mcc
{

enum class MccGenericMountErrorCode : int { ERROR_OK, ERROR_HW_INIT, ERROR_HW_STOP, ERROR_HW_GETSTATE };

}  // namespace mcc


namespace std
{

template <>
class is_error_code_enum<mcc::MccGenericMountErrorCode> : public true_type
{
};

}  // namespace std



namespace mcc
{

template <mcc_hardware_c HardwareT,
          mcc_telemetry_c TelemetryT,
          mcc_pzone_container_c PZoneContT,
          mcc_slewing_model_c SlewModelT,
          mcc_tracking_model_c TrackModelT,
          // mcc_guiding_model_c GuidingModelT,
          mcc_logger_c LoggerT = MccNullLogger>
class MccGenericMount : public HardwareT,
                        public TelemetryT,
                        public PZoneContT,
                        public SlewModelT,
                        public TrackModelT,
                        // public GuidingModelT,
                        public LoggerT
{
public:
    typedef std::error_code error_t;

    using LoggerT::logDebug;
    using LoggerT::logError;
    using LoggerT::logInfo;
    using LoggerT::logWarn;


    // using typename GuidingModelT::guiding_params_t;
    using typename SlewModelT::slewing_params_t;
    using typename TrackModelT::tracking_params_t;

    MccGenericMount(HardwareT hardware,
                    TelemetryT telemetry,
                    PZoneContT pzone_cont,
                    SlewModelT slew_model,
                    TrackModelT track_model,
                    LoggerT logger = MccNullLogger{})
        : HardwareT(std::move(hardware)),
          TelemetryT(std::move(telemetry)),
          PZoneContT(std::move(pzone_cont)),
          SlewModelT(std::move(slew_model)),
          TrackModelT(std::move(track_model)),
          LoggerT(std::move(logger))
    {
    }

    virtual ~MccGenericMount() = default;

    error_t stopMount()
    {
        logInfo("Stop any movements ...");

        this->stopTracking();
        this->stopSlewing();

        auto hw_err = this->hardwareStop();
        if (hw_err) {
            return mcc_deduce_error(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP);
        }

        logInfo("Stop command was sent");

        return MccGenericMountErrorCode::ERROR_OK;
    }

    error_t initMount()
    {
        logInfo("Start mount initialization ...");

        auto hw_err = this->hardwareInit();
        if (hw_err) {
            return mcc_deduce_error(hw_err, MccGenericMountErrorCode::ERROR_HW_STOP);
        }

        logInfo("Mount initialization was performed");

        return MccGenericMountErrorCode::ERROR_OK;
    }
};



/*                          GENERIC FINITE-STATE-MACHINE MOUNT REFERENCE IMPLEMENTATION                          */


template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountBaseEvent {
    // static constexpr std::string_view ID{""};
    virtual ~MccGenericFsmMountBaseEvent() = default;

    MOUNT_T& mount() const
    {
        return _mount;
    }

protected:
    MOUNT_T& _mount;

    MccGenericFsmMountBaseEvent(MOUNT_T& mount) : _mount(mount) {}
};


template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountInitEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountIdleEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountStopEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountErrorEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountSlewEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountTrackEvent;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountBaseState {
    // static constexpr std::string_view ID{""};
    virtual ~MccGenericFsmMountBaseState() = default;

protected:
    MccGenericFsmMountBaseState() = default;

    template <fsm::traits::fsm_event_c EvT>
    void exitLog(this auto&& self, EvT& event)
    {
        using self_t = std::remove_cvref_t<decltype(self)>;

        if constexpr (mcc_generic_log_mount_c<MOUNT_T> &&
                      std::derived_from<EvT, MccGenericFsmMountBaseEvent<MOUNT_T>>) {
            event.mount().logDebug(std::format("Exit from '{}' state due to '{}' event ...", self_t::ID, EvT::ID));
        }
    }

    template <fsm::traits::fsm_event_c EvT>
    void enterLog(this auto&& self, EvT& event)
    {
        using self_t = std::remove_cvref_t<decltype(self)>;

        if constexpr (mcc_generic_log_mount_c<MOUNT_T> &&
                      std::derived_from<EvT, MccGenericFsmMountBaseEvent<MOUNT_T>>) {
            event.mount().logDebug(std::format("Enter to '{}' state due to '{}' event ...", self_t::ID, EvT::ID));
        }
    }
};

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountStartState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountIdleState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountStopState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountErrorState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountSlewState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountTrackState;

template <mcc_generic_mount_c MOUNT_T>
struct MccGenericFsmMountInitState {
    static constexpr std::string_view ID{"GENERIC-MOUNT-INIT-STATE"};

    using transition_t = fsm::fsm_transition_table_t<
        std::pair<MccGenericFsmMountIdleEvent<MOUNT_T>, MccGenericFsmMountIdleState<MOUNT_T>>,
        std::pair<MccGenericFsmMountErrorEvent<MOUNT_T>, MccGenericFsmMountErrorState<MOUNT_T>>,
        std::pair<MccGenericFsmMountInitEvent<MOUNT_T>, MccGenericFsmMountInitState<MOUNT_T>>>;


    void exit(MccGenericFsmMountInitEvent<MOUNT_T>& event)
    {
        if constexpr (mcc_generic_log_mount_c<MOUNT_T>) {
            event.mount().logWarn("It seems a re-entering to the initializing state was asked! Ignore the event!");
        }
    }

    void enter(MccGenericFsmMountInitEvent<MOUNT_T>& event)
    {
        if constexpr (mcc_generic_log_mount_c<MOUNT_T>) {
            event.mount().logWarn(
                "It seems a re-entering to the initializing state was asked! Ignore the event and wait for the mount "
                "to initialize!");
        }
    }

    void exit(fsm::traits::fsm_event_c auto& event)
    {
        exitLog(event);
    }

    void enter(fsm::traits::fsm_event_c auto& event)
    {
        enterLog(event);
    }
};



template <mcc_generic_mount_c MOUNT_T,
          fsm::traits::fsm_state_c START_STATE_T = MccGenericFsmMountStartState<MOUNT_T>,
          fsm::traits::fsm_event_c INIT_EVENT_T = MccGenericFsmMountInitEvent<MOUNT_T>,
          fsm::traits::fsm_event_c IDLE_EVENT_T = MccGenericFsmMountIdleEvent<MOUNT_T>,
          fsm::traits::fsm_event_c STOP_EVENT_T = MccGenericFsmMountStopEvent<MOUNT_T>,
          fsm::traits::fsm_event_c ERROR_EVENT_T = MccGenericFsmMountErrorEvent<MOUNT_T>,
          fsm::traits::fsm_event_c SLEW_EVENT_T = MccGenericFsmMountSlewEvent<MOUNT_T>,
          fsm::traits::fsm_event_c TRACK_EVENT_T = MccGenericFsmMountTrackEvent<MOUNT_T>>
class MccGenericFsmMount : public MOUNT_T, protected fsm::MccFiniteStateMachine
{
public:
    using typename MOUNT_T::error_t;

    // reimplementation of base-class methods to adapt it to FSM-behavior

    auto initMount()
    {
        this->dispatchEvent(INIT_EVENT_T{*this});
    }

    auto stopMount()
    {
        this->dispatchEvent(STOP_EVENT_T{*this});
    }

    auto slewToTarget()
    {
        this->dispatchEvent(SLEW_EVENT_T{*this});
    }

    auto stopSlewing()
    {
        this->dispatchEvent(STOP_EVENT_T{*this});
    }

    auto trackTarget()
    {
        this->dispatchEvent(TRACK_EVENT_T{*this});
    }


    auto stopTracking()
    {
        this->dispatchEvent(STOP_EVENT_T{*this});
    }


protected:
    // wrappers

    auto _initMount()
    {
        return MOUNT_T::initMount();
    }


    auto _stopMount()
    {
        return MOUNT_T::stopMount();
    }

    auto _slewToTarget()
    {
        return MOUNT_T::slewToTarget();
    }


    auto _trackTarget()
    {
        return MOUNT_T::trackTarget();
    }


    auto _startGuidingTarget()
    {
        return MOUNT_T::startGuidingTarget();
    }

    auto _stopGuidingTarget()
    {
        return MOUNT_T::stopGuidingTarget();
    }
};


template <mcc_hardware_c HardwareT,
          mcc_telemetry_c TelemetryT,
          mcc_pzone_container_c PZoneContT,
          mcc_slewing_model_c SlewModelT,
          mcc_tracking_model_c TrackModelT,
          mcc_logger_c LoggerT = MccNullLogger>
class MccGenericMountFSM : public fsm::MccFiniteStateMachine,
                           public MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, LoggerT>
{
protected:
    typedef MccGenericMount<HardwareT, TelemetryT, PZoneContT, SlewModelT, TrackModelT, LoggerT> base_gmount_t;

public:
    using typename base_gmount_t::error_t;

    /*                          EVENTS AND STATES DEFINITION                            */


    /*  base class for events  */

    struct MccGenericMountBaseEvent {
        virtual ~MccGenericMountBaseEvent() = default;

        MccGenericMountFSM& mount() const
        {
            return _mount;
        }

    protected:
        MccGenericMountFSM& _mount;

        MccGenericMountBaseEvent(MccGenericMountFSM& mount) : _mount(mount) {}
    };

    // to IDLE state
    struct MccGenericMountEventIDLE : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-IDLE-EVENT";

        // CTAD does not work for clang++ (at least till v. 20 and -std=c++23)!
        // so, one must explicitly define constructor here
        MccGenericMountEventIDLE(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
    };


    // to error state
    struct MccGenericMountEventError : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-ERROR-EVENT";

        MccGenericMountEventError(MccGenericMountFSM& mount, const error_t& err)
            : MccGenericMountBaseEvent(mount), _error(err)
        {
        }


        error_t eventData() const
        {
            return _error;
        }

    protected:
        error_t _error;
    };


    // to INIT state
    struct MccGenericMountEventInit : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-INIT-EVENT";

        MccGenericMountEventInit(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
    };


    // to slewing state
    struct MccGenericMountEventSlew : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-SLEWING-EVENT";

        MccGenericMountEventSlew(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
    };


    // to tracking state
    struct MccGenericMountEventTrack : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-TRACKING-EVENT";

        MccGenericMountEventTrack(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
    };


    // to stoping state
    struct MccGenericMountEventStop : MccGenericMountBaseEvent {
        static constexpr std::string_view ID = "MCC-MOUNT-STOP-EVENT";

        MccGenericMountEventStop(MccGenericMountFSM& mount) : MccGenericMountBaseEvent(mount) {}
    };



    /*  base class for states  */

    struct MccGenericMountBaseState {
        static constexpr std::string_view ID{"BASE-STATE"};

        virtual ~MccGenericMountBaseState() = default;

    protected:
        MccGenericMountBaseState() = default;

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exitLog(this auto&& self, EvT& event)
        {
            using self_t = std::remove_cvref_t<decltype(self)>;

            event.mount().logDebug(std::format("Exit from '{}' state due to '{}' event ...", self_t::ID, EvT::ID));
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enterLog(this auto&& self, EvT& event)
        {
            using self_t = std::remove_cvref_t<decltype(self)>;
            event.mount().logDebug(std::format("Enter to '{}' state due to '{}' event ...", self_t::ID, EvT::ID));
        }
    };

    // forward declarations
    struct MccGenericMountStateInit;
    struct MccGenericMountStateIDLE;
    struct MccGenericMountStateError;  // must be implemented in derived class
    struct MccGenericMountStateSlew;
    struct MccGenericMountStateTrack;
    // struct MccGenericMountStateGuiding;
    struct MccGenericMountStateStopping;

    struct MccGenericMountStateUninit : MccGenericMountBaseState {
        static constexpr std::string_view ID{"UNINITIALIZED-STATE"};

        // from uninitialized state only INIT-event is permitted!
        using transition_t = fsm::fsm_transition_table_t<std::pair<MccGenericMountEventInit, MccGenericMountStateInit>>;

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);
        }
    };

    struct MccGenericMountStateInit : MccGenericMountBaseState {
        static constexpr std::string_view ID{"INITIALIZING-STATE"};

        using transition_t =
            fsm::fsm_transition_table_t<std::pair<MccGenericMountEventIDLE, MccGenericMountStateIDLE>,
                                        std::pair<MccGenericMountEventError, MccGenericMountStateError>,
                                        std::pair<MccGenericMountEventInit, MccGenericMountStateInit>>;

        void exit(MccGenericMountEventInit& event)
        {
            event.mount().logWarn("It seems re-entering to initializing state was asked! Just ignore this event!");
        }

        void enter(MccGenericMountEventInit& event)
        {
            event.mount().logWarn(
                "It seems re-entering to initializing state was asked! Ignore it and wait for mount to initialize!");
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);

            error_t err = event.mount()._initMount();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            } else {
                event.mount().dispatchEvent(MccGenericMountEventIDLE{event.mount()});
            }
        }
    };

    struct MccGenericMountStateIDLE : MccGenericMountBaseState {
        static constexpr std::string_view ID{"IDLE-STATE"};

        using transition_t =
            fsm::fsm_transition_table_t<std::pair<MccGenericMountEventInit, MccGenericMountStateInit>,
                                        std::pair<MccGenericMountEventError, MccGenericMountStateError>,
                                        std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
                                        std::pair<MccGenericMountEventSlew, MccGenericMountStateSlew>,
                                        std::pair<MccGenericMountEventTrack, MccGenericMountStateTrack>,
                                        std::pair<MccGenericMountEventIDLE, MccGenericMountStateIDLE>>;

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);
        }
    };


    struct MccGenericMountStateStopping : MccGenericMountBaseState {
        static constexpr std::string_view ID{"STOPPING-STATE"};

        using transition_t =
            fsm::fsm_transition_table_t<std::pair<MccGenericMountEventIDLE, MccGenericMountStateIDLE>,
                                        std::pair<MccGenericMountEventError, MccGenericMountStateError>,
                                        std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>>;

        void exit(MccGenericMountEventStop& event)
        {
            event.mount().logWarn("It seems re-entering to stopping state was asked! Just ignore it!");
        }

        void enter(MccGenericMountEventStop& event)
        {
            event.mount().logWarn(
                "It seems re-entering to stopping state was asked! Ignore it and wait for mount to stop!");
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);

            error_t err = event.mount()._stopMount();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            } else {
                event.mount().dispatchEvent(MccGenericMountEventIDLE{event.mount()});
            }
        }
    };


    struct MccGenericMountStateSlew : MccGenericMountBaseState {
        static constexpr std::string_view ID{"SLEWING-STATE"};

        using transition_t =
            fsm::fsm_transition_table_t<std::pair<MccGenericMountEventIDLE, MccGenericMountStateIDLE>,
                                        std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
                                        std::pair<MccGenericMountEventError, MccGenericMountStateError>,
                                        std::pair<MccGenericMountEventTrack, MccGenericMountStateTrack>,
                                        std::pair<MccGenericMountEventSlew, MccGenericMountStateSlew>>;

        void exit(MccGenericMountEventSlew& event)
        {
            event.mount().logWarn(
                "It seems re-entering to slewing state was asked! Do not stop the current slewing process, just ignore "
                "this event!");
        }

        void enter(MccGenericMountEventSlew& event)
        {
            event.mount().logWarn(
                "It seems re-entering to slewing state was asked! Do not start a new slewing process, just ignore this "
                "event!");
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            error_t err = event.mount()._stopSlewingMount();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            }

            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);

            error_t err = event.mount()._slewMount();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            } else {
                auto slew_pars = event.mount().getSlewingParams();

                if (slew_pars.slewAndStop) {
                    event.mount().dispatchEvent(MccGenericMountEventIDLE{event.mount()});
                } else {
                    event.mount().dispatchEvent(MccGenericMountEventTrack{event.mount()});
                }
            }
        }
    };


    struct MccGenericMountStateTrack : MccGenericMountBaseState {
        static constexpr std::string_view ID{"TRACKING-STATE"};

        using transition_t =
            fsm::fsm_transition_table_t<std::pair<MccGenericMountEventStop, MccGenericMountStateStopping>,
                                        std::pair<MccGenericMountEventError, MccGenericMountStateError>,
                                        std::pair<MccGenericMountEventTrack, MccGenericMountStateTrack>>;

        void exit(MccGenericMountEventTrack& event)
        {
            event.mount().logWarn(
                "It seems re-entering to tracking state was asked! Do not stop the current tracking process, just "
                "ignore this event!");
        }

        void enter(MccGenericMountEventTrack& event)
        {
            event.mount().logWarn(
                "It seems re-entering to tracking state was asked! Do not start a new tracking process, just ignore "
                "this event!");
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void exit(EvT& event)
        {
            // error_t err = event.mount()._stopTrackingMount();

            error_t err = static_cast<SlewModelT*>(&event.mount())->stopTracking();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            }

            this->exitLog(event);
        }

        template <std::derived_from<MccGenericMountBaseEvent> EvT>
        void enter(EvT& event)
        {
            this->enterLog(event);

            error_t err = event.mount()._trackMount();
            if (err) {
                event.mount().dispatchEvent(MccGenericMountEventError{event.mount(), err});
            }
        }
    };


    using LoggerT::logDebug;
    using LoggerT::logError;
    using LoggerT::logInfo;
    using LoggerT::logWarn;


    using typename SlewModelT::slewing_params_t;
    using typename TrackModelT::tracking_params_t;


    /*  CONSTRUCTORS AND DESTRUCTOR  */

    MccGenericMountFSM(HardwareT hardware,
                       TelemetryT telemetry,
                       PZoneContT pzone_cont,
                       SlewModelT slew_model,
                       TrackModelT track_model,
                       LoggerT logger = MccNullLogger{})
        : fsm::MccFiniteStateMachine(MccGenericMountStateUninit{}),
          base_gmount_t(std::move(hardware),
                        std::move(telemetry),
                        std::move(pzone_cont),
                        std::move(slew_model),
                        std::move(track_model),
                        std::move(logger))
    {
    }


    virtual ~MccGenericMountFSM() = default;

    // reimplementation of base-class methods to adapt it to FSM-behavior
    auto initMount()
    {
        if (_currentStateID == MccGenericMountStateInit::ID) {
            logWarn("Already in initializing state! Ignore!");
            return;
        }

        this->dispatchEvent(MccGenericMountEventInit{*this});
    }

    auto stopMount()
    {
        this->dispatchEvent(MccGenericMountEventStop{*this});
    }

    auto slewToTarget()
    {
        this->dispatchEvent(MccGenericMountEventSlew{*this});
    }

    auto stopSlewing()
    {
        this->dispatchEvent(MccGenericMountEventStop{*this});
    }

    auto trackTarget()
    {
        this->dispatchEvent(MccGenericMountEventTrack{*this});
    }


    auto stopTracking()
    {
        this->dispatchEvent(MccGenericMountEventIDLE{*this});
    }


protected:
    // wrappers

    auto _initMount()
    {
        return base_gmount_t::initMount();
    }


    auto _stopMount()
    {
        return base_gmount_t::stopMount();
    }

    auto _slewToTarget()
    {
        return base_gmount_t::slewToTarget();
    }


    auto _trackTarget()
    {
        return base_gmount_t::trackTarget();
    }


    auto _startGuidingTarget()
    {
        return base_gmount_t::startGuidingTarget();
    }

    auto _stopGuidingTarget()
    {
        return base_gmount_t::stopGuidingTarget();
    }
};


}  // namespace mcc