#pragma once



/****************************************************************************************
 *                                                                                      *
 *                          MOUNT CONTROL COMPONENTS LIBRARY                            *
 *                                                                                      *
 *                                                                                      *
 *             IMPLEMENTATION OF VERY SIMPLE MOUNT MOVEMENT CONTROLS CLASS              *
 *                                                                                      *
 ****************************************************************************************/



/*
    The user needs to accept some assumptions implemented in this class:

        1) It is assumed that the implementation of the HARDWARE_T class must return movement state
           as "HW_MOVE_STOPPED" to indicate that underlying hardware is at required position
        2) In the case of "slew_and_stop == false" the slewing cycle waits for "HW_MOVE_STOPPED" to switch
           to state "tracking", but the implementation of the HARDWARE_T class may return movement state
           as "HW_MOVE_GUIDING" to indicate that underlying hardware is near required position, i.e., its
           inner condition "hardware is at position" is true. The latter is usefull to avoid full hardware stop
           and allows to switch to state "tracking" as soon as possible

 */


#include <atomic>
#include <fstream>
#include <thread>

#include "mcc/mcc_coordinate.h"
#include "mcc_concepts.h"
#include "mcc_error.h"

namespace mcc::impl
{

namespace details
{
// just auxiliary class instance
static MccNullLogger NullLogger{};
}  // namespace details


enum class MccSimpleMovementControlsErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_HW_ERROR,
    ERROR_PCM_COMP,
    ERROR_CCTE_COMP,
    ERROR_GET_TELEMETRY,
    ERROR_DIST_TELEMETRY,
    ERROR_PZONE_CONTAINER_COMP,
    ERROR_TARGET_IN_PZONE,
    ERROR_NEAR_PZONE,
    ERROR_TIMEOUT,
    ERROR_ALREADY_SLEW,
    ERROR_ALREADY_STOPPED,
    ERROR_STOPPED
};

}  // namespace mcc::impl


namespace std
{

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

}  // namespace std



namespace mcc::impl
{

// error category
struct MccSimpleMovementControlsCategory : public std::error_category {
    MccSimpleMovementControlsCategory() : std::error_category() {}

    const char* name() const noexcept
    {
        return "SIMPLE-SLEWING-MODEL";
    }

    std::string message(int ec) const
    {
        MccSimpleMovementControlsErrorCode err = static_cast<MccSimpleMovementControlsErrorCode>(ec);

        switch (err) {
            case MccSimpleMovementControlsErrorCode::ERROR_OK:
                return "OK";
            case MccSimpleMovementControlsErrorCode::ERROR_HW_GETSTATE:
                return "cannot get hardware state";
            case MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE:
                return "cannot set hardware state";
            case MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR:
                return "hardware error occured";
            case MccSimpleMovementControlsErrorCode::ERROR_PCM_COMP:
                return "PCM computation error";
            case MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP:
                return "celestial coordinate transformation error";
            case MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY:
                return "cannot get telemetry";
            case MccSimpleMovementControlsErrorCode::ERROR_DIST_TELEMETRY:
                return "cannot get target-to-mount-position distance";
            case MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP:
                return "pzone container computation error";
            case MccSimpleMovementControlsErrorCode::ERROR_TARGET_IN_PZONE:
                return "target is in prohibited zone";
            case MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE:
                return "near prohibited zone";
            case MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT:
                return "a timeout occured while slewing";
            case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_SLEW:
                return "already slewing";
            case MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED:
                return "slewing is already stopped";
            case MccSimpleMovementControlsErrorCode::ERROR_STOPPED:
                return "slewing was stopped";
            default:
                return "UNKNOWN";
        }
    }

    static const MccSimpleMovementControlsCategory& get()
    {
        static const MccSimpleMovementControlsCategory constInst;
        return constInst;
    }
};


inline std::error_code make_error_code(MccSimpleMovementControlsErrorCode ec)
{
    return std::error_code(static_cast<int>(ec), MccSimpleMovementControlsCategory::get());
}



struct MccSimpleMovementControlsParameters {
    // *******    common for all modes    *******

    // mean celestial rate
    static constexpr double sideralRate = 15.0410686_arcsecs;  // in radians per second

    // timeout to telemetry updating
    std::chrono::milliseconds telemetryTimeout{3000};

    // minimal time to prohibited zone (at current speed in slewing mode). if it is lesser then exit with error
    std::chrono::seconds minTimeToPZone{10};

    // time interval to update prohibited zones related quantities (e.g. intersection points)
    std::chrono::milliseconds updatingPZoneInterval{5000};


    // *******    slewing mode    *******

    bool slewAndStop{false};  // slew to target and stop mount

    // coordinates difference to stop slewing (in radians)
    double slewToleranceRadius{5.0_arcsecs};

    // telemetry request interval
    std::chrono::milliseconds slewingTelemetryInterval{100};

    // target-mount coordinate difference to start adjusting of slewing (in radians)
    double adjustCoordDiff{slewToleranceRadius * 10.0};

    // slew process timeout
    std::chrono::seconds slewTimeout{3600};

    double slewRateX{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)
    double slewRateY{0.0};  // maximal slewing rate (0 means move with maximal allowed rate????!!!!!)

    std::chrono::milliseconds adjustCycleInterval{500};  // minimum time between two successive adjustments

    double adjustRateX{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)
    double adjustRateY{5.0_arcmins};  // maximal adjusting rate (a rate at the final slewing stage)

    // braking acceleration after execution of mount stopping command (in rads/s^2)
    // it must be given as non-negative value!!!
    double brakingAccelX{0.0};
    double brakingAccelY{0.0};

    // slewing trajectory file. if empty - just skip saving
    std::string slewingPathFilename{};
    // *******    tracking mode    *******

    // telemetry request interval
    std::chrono::milliseconds trackingTelemetryInterval{100};

    double trackSpeedX{};
    double trackSpeedY{};
    std::chrono::milliseconds trackingCycleInterval{500};  // minimum time between two successive tracking corrections
    bool dualAxisTracking{true};  // mount must be of an equatorial type: false means guiding along only HA-axis

    // time shift into future to compute target position in future (UT1-scale time duration)
    std::chrono::milliseconds timeShiftToTargetPoint{10000};
    // maximal target-to-mount difference for tracking process (in arcsecs)
    // it it is greater then the current mount coordinates are used as target one
    double trackingMaxCoordDiff{20.0};

    // tracking trajectory file. if empty - just skip saving
    std::string trackingPathFilename{};
};



class MccSimpleMovementControls
{
protected:
    // a simple class for movement path saving
    class PathFile
    {
    public:
        PathFile(const std::string& filename = "") : _filename(filename), _st() {}

        PathFile(const PathFile&) = delete;
        PathFile(PathFile&&) = default;

        ~PathFile()
        {
            save();
        }

        void setFilename(const std::string& filename)
        {
            _filename = filename;
        }

        std::string getFilename() const
        {
            return _filename;
        }

        friend PathFile& operator<<(PathFile& pf, auto&& v)
        {
            pf._st << std::forward<decltype(v)>(v);

            return pf;
        }

        bool save()
        {
            std::fstream fst;

            if (_filename.empty()) {
                return false;
            }

            if (_st.str().empty()) {  // nothing to save
                return true;
            }

            fst.open(_filename, std::ios::app);

            if (!fst.is_open()) {
                return false;
            }

            fst << _st.str();
            _st.str("");
            _filename.clear();

            return true;
        }

    private:
        std::string _filename;
        std::ostringstream _st;
    };


public:
    typedef MccError error_t;

    typedef MccSimpleMovementControlsParameters movement_params_t;

    template <mcc_hardware_c HARDWARE_T,
              mcc_generic_mount_c MOUNT_T,  // MOUNT_T is mcc_telemetry_c, mcc_pzone_container_c and mcc_logger_c
                                            // (see mcc_concepts.h)
              std::invocable<typename MOUNT_T::mount_status_t const&> CallbackFuncT =
                  decltype([](typename MOUNT_T::mount_status_t const&) {})>
    MccSimpleMovementControls(
        HARDWARE_T* hardware,
        MOUNT_T* mount,
        CallbackFuncT&& mode_switch_callback = [](typename MOUNT_T::mount_status_t const&) {})
        : MccSimpleMovementControls(hardware, mount, mount, std::forward<CallbackFuncT>(mode_switch_callback), mount)
    {
    }

    template <mcc_hardware_c HARDWARE_T,
              mcc_telemetry_c TELEMETRY_T,
              mcc_pzone_container_c PZONE_CONT_T,
              typename CallbackFuncT,
              mcc_logger_c LOGGER_T = MccNullLogger>
    MccSimpleMovementControls(HARDWARE_T* hardware,
                              TELEMETRY_T* telemetry,
                              PZONE_CONT_T* pzone_cont,
                              CallbackFuncT&& mode_switch_callback,
                              LOGGER_T* logger = &details::NullLogger)
    {
        static_assert(traits::mcc_func_traits<CallbackFuncT>::arity == 1 &&
                          mcc_mount_status_c<std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>>,
                      "INVALID 'CallbackFuncT' CALLABLE SIGNATURE!");

        using STATUS_T = std::decay_t<traits::mcc_func_arg1_t<CallbackFuncT>>;

        auto send_to_hardware = [hardware, logger](typename HARDWARE_T::hardware_state_t const& hw_state) -> error_t {
            auto tp =
                std::chrono::duration_cast<std::chrono::milliseconds>(hw_state.XY.epoch().UTC().time_since_epoch());

            if constexpr (std::derived_from<decltype(hw_state.XY), MccCoordPair<MccAngleX, MccAngleY>>) {
                logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
                                             hw_state.XY.x().degrees(), hw_state.XY.y().degrees(), tp));
            } else {  // user defined mcc_coord_pair_c
                logger->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs (timepoint: {})",
                                             MccAngle((double)hw_state.XY.x()).degrees(),
                                             MccAngle((double)hw_state.XY.y()).degrees(), tp));
            }

            auto hw_err = hardware->hardwareSetState(hw_state);
            if (hw_err) {
                mcc_deduced_err(hw_err, MccSimpleMovementControlsErrorCode::ERROR_HW_SETSTATE);
            }

            logger->logDebug("    the 'hardwareSetState' method performed successfully!");


            return MccSimpleMovementControlsErrorCode::ERROR_OK;
        };


        auto check_pzones = [pzone_cont, logger](typename TELEMETRY_T::telemetry_data_t const& tdata,
                                                 double min_time_to_pzone_in_secs, double braking_accelX,
                                                 double braking_accelY) -> error_t {
            bool in_zone;
            std::vector<bool> in_zone_vec;

            // calculate the distances along the X and Y axes that the mount will travel at the current speed in a given
            // time, taking into account the braking acceleration
            double speedX = tdata.hwState.speedXY.x();
            double speedY = tdata.hwState.speedXY.y();

            // time to stop mount with given current speed and constant braking acceleration
            double tx_stop = std::abs(speedX) / braking_accelX;
            double ty_stop = std::abs(speedY) / braking_accelY;

            double tx = min_time_to_pzone_in_secs;
            double ty = min_time_to_pzone_in_secs;

            if (std::isfinite(tx_stop) && (min_time_to_pzone_in_secs > tx_stop)) {
                tx = tx_stop;
            }
            if (std::isfinite(ty_stop) && (min_time_to_pzone_in_secs > ty_stop)) {
                ty = ty_stop;
            }

            // the distance:
            // here, one must take into account the sign of the speed!!!
            double dx = speedX * tx - std::copysign(braking_accelX, speedX) * tx * tx / 2.0;
            double dy = speedY * ty - std::copysign(braking_accelY, speedY) * ty * ty / 2.0;

            logger->logTrace(
                std::format("    the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
                            min_time_to_pzone_in_secs, MccAngleFancyString(dx), MccAngleFancyString(dy)));


            // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
            // and check them for getting into the prohibited zones

            std::conditional_t<mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
                               std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZZD, std::nullptr_t>>
                mount_pos;

            static_assert(!std::is_null_pointer_v<decltype(mount_pos)>, "UNKNOWn MOUNT TYPE");

            mount_pos.setX((double)tdata.hwState.XY.x() + tdata.pcmCorrection.pcmX + dx);
            mount_pos.setY((double)tdata.hwState.XY.y() + tdata.pcmCorrection.pcmY + dy);
            mount_pos.setEpoch(tdata.hwState.XY.epoch());

            logger->logTrace(std::format("    mount:  speedX = {}/s, speedY = {}/s", MccAngleFancyString(speedX),
                                         MccAngleFancyString(speedY)));

            auto pz_err = pzone_cont->inPZone(MccSkyPoint(mount_pos), &in_zone, &in_zone_vec);
            if (pz_err) {
                return mcc_deduced_err(pz_err, MccSimpleMovementControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

            if (in_zone) {
                size_t i = 0;
                for (; i < in_zone_vec.size(); ++i) {
                    if (in_zone_vec[i]) {
                        break;
                    }
                }

                auto names = pzone_cont->pzoneNames();
                auto it = names.begin();
                std::ranges::advance(it, i);

                logger->logError(
                    "target point is near prohibited zone (zone index: {}, zone name: {})! Current mount position:", i,
                    *it);

                MccSkyHADEC_OBS hadec;
                MccSkyRADEC_OBS radec;
                MccSkyAZZD azzd;
                MccSkyAZALT azalt;
                MccAngle lst;

                auto ccte_err = tdata.mountPos.appSideralTime(&lst, true);
                if (ccte_err) {
                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
                }

                ccte_err = tdata.mountPos.toAtSameEpoch(radec, hadec, azzd, azalt);
                if (ccte_err) {
                    return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
                }

                logger->logError(std::format("    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}",
                                             MccAngle{radec.x()}.sexagesimal(true), MccAngle{radec.y()}.sexagesimal(),
                                             MccAngle{hadec.x()}.sexagesimal(true), lst.sexagesimal(true)));
                logger->logError(std::format("    AZ, ZD, ALT: {}, {}, {}", MccAngle{azzd.x()}.sexagesimal(),
                                             MccAngle{azzd.y()}.sexagesimal(), MccAngle{azalt.y()}.sexagesimal()));

                logger->logError(std::format("    hardware X, Y: {}, {}", MccAngle{tdata.hwState.XY.x()}.sexagesimal(),
                                             MccAngle{tdata.hwState.XY.y()}.sexagesimal()));

                return MccSimpleMovementControlsErrorCode::ERROR_NEAR_PZONE;
            }


            return MccSimpleMovementControlsErrorCode::ERROR_OK;
        };


        auto log_pos = [logger, this](typename TELEMETRY_T::telemetry_data_t const& tdata,
                                      std::tuple<double, double, double>& dist) -> error_t {
            double x_mnt, y_mnt, x_tag, y_tag;

            std::conditional_t<
                mccIsEquatorialMount(HARDWARE_T::hwMountType), MccSkyHADEC_OBS,
                std::conditional_t<mccIsAltAzMount(HARDWARE_T::hwMountType), MccSkyAZALT, std::nullptr_t>>
                coord_pair;

            static_assert(!std::is_null_pointer_v<decltype(coord_pair)>, "UNKNOWN MOUNT TYPE!");

            const std::string_view x_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "HA"
                                           : mccIsAltAzMount(HARDWARE_T::hwMountType)    ? "AZ"
                                                                                         : "GEN_X";
            const std::string_view y_str = mccIsEquatorialMount(HARDWARE_T::hwMountType) ? "DEC"
                                           : mccIsAltAzMount(HARDWARE_T::hwMountType)    ? "ALT"
                                                                                         : "GEN_Y";
            ;

            auto ccte_err = tdata.targetPos.toAtSameEpoch(coord_pair);
            if (ccte_err) {
                return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
            }


            x_tag = coord_pair.x();
            y_tag = coord_pair.y();

            logger->logTrace(std::format("    current target: {} = {}, {} = {} (encoders: {} {})", x_str,
                                         coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
                                         tdata.targetXY.x().sexagesimal(), tdata.targetXY.y().sexagesimal()));

            ccte_err = tdata.mountPos.toAtSameEpoch(coord_pair);
            if (ccte_err) {
                return mcc_deduced_err(ccte_err, MccSimpleMovementControlsErrorCode::ERROR_CCTE_COMP);
            }

            logger->logTrace(std::format("    current mount: {} = {}, {} = {} (encoders: {} {})", x_str,
                                         coord_pair.x().sexagesimal(true), y_str, coord_pair.y().sexagesimal(),
                                         tdata.hwState.XY.x().sexagesimal(), tdata.hwState.XY.y().sexagesimal()));

            x_mnt = coord_pair.x();
            y_mnt = coord_pair.y();

            dist = utils::distanceOnSphere(x_tag, y_tag, x_mnt, y_mnt);

            _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << " "
                      << x_mnt << " " << y_mnt << " " << std::get<0>(dist) << " " << std::get<1>(dist) << " "
                      << std::get<2>(dist) << " " << (int)tdata.hwState.movementState << "\n";
            // _pathFile << tdata.mountPos.epoch().UTC().time_since_epoch().count() << " " << x_tag << " " << y_tag << "
            // "
            //           << x_mnt << " " << y_mnt << " " << (x_tag - x_mnt) << " " << (y_tag - y_mnt) << " "
            //           << (int)tdata.hwState.movementState << "\n";

            logger->logTrace(std::format("    target-to-mount distance: {} (dx = {}, dy = {})",
                                         MccAngleFancyString(std::get<2>(dist)), MccAngleFancyString(std::get<0>(dist)),
                                         MccAngleFancyString(std::get<1>(dist))));

            return MccSimpleMovementControlsErrorCode::ERROR_OK;
        };

        auto cb_sptr =
            std::make_shared<std::function<void(STATUS_T const&)>>(std::forward<CallbackFuncT>(mode_switch_callback));


        /*  stop moving function */

        _stopMovingFunc = [send_to_hardware, hardware, cb_sptr, this]() {
            typename HARDWARE_T::hardware_state_t hw_state;
            hw_state.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPING;

            *_stopMoving = true;

            *_lastError = send_to_hardware(hw_state);

            if (_lastError->load()) {
                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);
            }
        };


        /*    slewing function    */

        _slewingFunc = [log_pos, send_to_hardware, check_pzones, cb_sptr, hardware, telemetry, logger,
                        this](bool slew_and_stop) {
            double braking_accelX, braking_accelY;
            double min_time_to_pzone_in_secs;


            {
                // std::lock_guard lock{*_currentParamsMutex};
                if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
                    braking_accelX = std::numeric_limits<double>::min();
                } else {
                    braking_accelX = std::abs(_currentParams.brakingAccelX);
                }

                if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
                    braking_accelY = std::numeric_limits<double>::min();
                } else {
                    braking_accelY = std::abs(_currentParams.brakingAccelY);
                }

                min_time_to_pzone_in_secs =
                    std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();

                if (!_currentParams.slewingPathFilename.empty()) {  // open slewing trajectory file
                    logger->logInfo(std::format("Set slewing path filename to {}", _currentParams.slewingPathFilename));
                    _pathFile.setFilename(_currentParams.slewingPathFilename);
                } else {
                    logger->logWarn("Slewing path filename is empty! Do not save it!");
                }
            }

            logger->logInfo(
                std::format("Start slewing in mode '{}'", slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK"));
            logger->logInfo(std::format("    slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
            if (!slew_and_stop) {
                logger->logInfo(std::format("    slewing tolerance radius: {} arcsecs",
                                            MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
            }
            logger->logInfo(std::format("    braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
                                        MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
            logger->logInfo(std::format("    braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
                                        MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
            logger->logInfo(std::format("    min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));


            _pathFile << "# \n";
            _pathFile << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
            _pathFile << "# Config:\n";
            _pathFile << "#     slewing tolerance radius: " << MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
                      << " arcsecs\n";
            _pathFile << "#     slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
            _pathFile << "# \n";
            _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
            _pathFile << "#     <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
                         "<dY_{target-mount}> <taget-mount-distance> <moving state>\n";


            typename TELEMETRY_T::telemetry_data_t tdata;
            typename HARDWARE_T::hardware_state_t hw_state;


            auto t_err = telemetry->telemetryData(&tdata);
            if (t_err) {
                *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                _pathFile.save();

                return;
            }


            *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
            if (_lastError->load()) {
                _pathFile.save();

                return;
            }

            using ep_t = decltype(hw_state.XY.epoch());

            hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
            hw_state.XY.setX(tdata.targetXY.x());
            hw_state.XY.setY(tdata.targetXY.y());
            hw_state.XY.setEpoch(ep_t::now());

            // start slewing ...

            *_lastError = send_to_hardware(hw_state);
            if (_lastError->load()) {
                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                _pathFile.save();

                return;
            }


            (*cb_sptr)(STATUS_T::MOUNT_STATUS_SLEWING);

            auto start_point = std::chrono::steady_clock::now();
            auto last_hw_time = tdata.hwState.XY.epoch().UTC();

            mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;

            std::tuple<double, double, double> dist;

            *_stopMoving = false;

            hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_ADJUSTING;

            while (!*_stopMoving) {
                t_err = telemetry->telemetryData(&tdata);
                if (t_err) {
                    *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                    (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                    return;
                }

                hw_state.XY.setX(tdata.targetXY.x());
                hw_state.XY.setY(tdata.targetXY.y());
                hw_state.XY.setEpoch(ep_t::now());

                log_pos(tdata, dist);

                // logger->logInfo("\tMNT.XY {} {}; MNT.HADEC {} {}", (double)tdata.hwState.XY.x(),
                //                 (double)tdata.hwState.XY.y(), tdata.mountPos.co_lon(), tdata.mountPos.co_lat());
                // logger->logInfo("\tTAG.XY {} {}; TAG.HADEC {} {}", (double)tdata.targetXY.x(),
                //                 (double)tdata.targetXY.y(), tdata.targetPos.co_lon(), tdata.targetPos.co_lat());

                if (*_stopMoving) {
                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
                    break;
                }

                *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
                if (_lastError->load()) {
                    break;
                }

                {
                    std::lock_guard lock{*_currentParamsMutex};

                    if ((std::chrono::steady_clock::now() - start_point) > _currentParams.slewTimeout) {
                        logger->logError("slewing process timeout!");
                        *_lastError = MccSimpleMovementControlsErrorCode::ERROR_TIMEOUT;
                        break;
                    }

                    if (std::get<2>(dist) <= _currentParams.slewToleranceRadius) {
                        logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");

                        break;
                    }


                    // start adjusting (precise pointing) the slewing
                    if (std::get<2>(dist) <= _currentParams.adjustCoordDiff) {
                        (*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);
                        *_lastError = send_to_hardware(hw_state);
                        if (_lastError->load()) {
                            break;
                        }
                    }

                    if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
                        // the mount stopped but still to far from target position!!!
                        if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
                            (*cb_sptr)(STATUS_T::MOUNT_STATUS_ADJUSTING);

                            *_lastError = send_to_hardware(hw_state);
                            if (_lastError->load()) {
                                break;
                            }
                        } else {
                            break;
                        }
                    }
                }

                if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_ERROR) {
                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_HW_ERROR;
                    break;
                }


                /*
                if (slew_and_stop) {  // just wait until the mount stops
                    if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED) {
                        // if (std::get<2>(dist) > _currentParams.slewToleranceRadius) {
                        //     // resend new position since target coordinates are changed in time
                        //     hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
                        //     hw_state.XY.setX(tdata.targetXY.x());
                        //     hw_state.XY.setY(tdata.targetXY.y());
                        //     hw_state.XY.setEpoch(ep_t::now());

                        //     *_lastError = send_to_hardware(hw_state);
                        //     if (_lastError->load()) {
                        //         break;
                        //     }
                        // } else {
                        logger->logInfo("mount movement state is STOPPED! Exit from slewing process!");
                        break;
                        // }
                    }
                } else {
                    if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
                        logger->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
                        continue;
                    }

                    last_hw_time = tdata.hwState.XY.epoch().UTC();

                    // stop slewing and exit from the cycle?
                    if (std::get<2>(dist) <= _currentParams.slewToleranceRadius) {
                        if (tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_STOPPED ||
                            tdata.hwState.movementState == HARDWARE_T::hardware_movement_state_t::HW_MOVE_GUIDING) {
                            logger->logInfo("target-to-mount distance is lesser than slew acceptable radius - exit!");

                            break;
                        } else {
                            logger->logDebug(
                                "target-to-mount distance is lesser than slew acceptable radius but hardware is still "
                                "not ready!");
                        }
                    }


                    if (*_stopMoving) {
                        break;
                    }


                    // resend new position since target coordinates are changed in time
                    hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_SLEWING;
                    hw_state.XY.setX(tdata.targetXY.x());
                    hw_state.XY.setY(tdata.targetXY.y());
                    hw_state.XY.setEpoch(ep_t::now());

                    *_lastError = send_to_hardware(hw_state);
                    if (_lastError->load()) {
                        break;
                    }
                }
                */

                // sleep here
                std::this_thread::sleep_for(_currentParams.slewingTelemetryInterval);
            }

            if (_stopMoving->load()) {  // external stop
                logger->logWarn("Slewing was stopped!");
                *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
                _pathFile.save();

                return;
            }

            *_stopMoving = true;

            logger->logInfo("Slewing finished");
            auto err = _lastError->load();
            logger->logInfo(std::format("    exit code: {} {} {}", err.value(), err.category().name(), err.message()));

            _pathFile.save();

            // get final position

            if (!err) {
                t_err = telemetry->telemetryData(&tdata);
                if (t_err) {
                    *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                    (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                    return;
                }

                log_pos(tdata, dist);

                if (!slew_and_stop) {  // start tracking
                    _trackingFunc();
                } else {
                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
                }
            }
        };


        /*  tracking function  */

        _trackingFunc = [log_pos, check_pzones, send_to_hardware, telemetry, cb_sptr, logger, this]() {
            double braking_accelX, braking_accelY;
            double min_time_to_pzone_in_secs;


            logger->logInfo("Start tracking");
            {
                // std::lock_guard lock{*_currentParamsMutex};
                if (mcc::utils::isEqual(_currentParams.brakingAccelX, 0.0)) {
                    braking_accelX = std::numeric_limits<double>::min();
                } else {
                    braking_accelX = std::abs(_currentParams.brakingAccelX);
                }

                if (mcc::utils::isEqual(_currentParams.brakingAccelY, 0.0)) {
                    braking_accelY = std::numeric_limits<double>::min();
                } else {
                    braking_accelY = std::abs(_currentParams.brakingAccelY);
                }

                min_time_to_pzone_in_secs =
                    std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();

                if (!_currentParams.trackingPathFilename.empty()) {  // open slewing trajectory file
                    logger->logInfo(
                        std::format("    Set tracking filename to {}", _currentParams.trackingPathFilename));
                    _pathFile.setFilename(_currentParams.trackingPathFilename);
                } else {
                    logger->logWarn("    Tracking path filename is empty! Do not save it!");
                }
            }

            logger->logInfo(std::format("    braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
                                        MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
            logger->logInfo(std::format("    braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
                                        MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
            logger->logInfo(std::format("    min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
            logger->logInfo(
                std::format("    max target-to-mount distance: {} arcseconds", _currentParams.trackingMaxCoordDiff));


            _pathFile << "# \n";
            _pathFile << "# Tracking trajectory, " << std::chrono::system_clock::now() << "\n";
            _pathFile << "# \n";
            _pathFile << "# Format (time is in nanoseconds, coordinates are in radians): \n";
            _pathFile << "#     <UNIXTIME> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
                         "<dY_{target-mount}> <taget-mount-distance> <moving state>\n";


            typename TELEMETRY_T::telemetry_data_t tdata;
            typename HARDWARE_T::hardware_state_t hw_state;
            std::tuple<double, double, double> dist;


            auto t_err = telemetry->telemetryData(&tdata);
            if (t_err) {
                *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                _pathFile.save();

                return;
            }

            log_pos(tdata, dist);

            {
                std::lock_guard lock{*_currentParamsMutex};
                if (_currentParams.trackingMaxCoordDiff < std::get<2>(dist)) {
                    logger->logInfo(
                        std::format("The target-to-mount distance {} is greater than allowed one ({})! Track current "
                                    "mount position!",
                                    MccAngleFancyString(std::get<2>(dist)),
                                    MccAngleFancyString(_currentParams.trackingMaxCoordDiff)));

                    telemetry->setPointingTarget(tdata.mountPos);
                }
            }

            t_err = telemetry->telemetryData(&tdata);
            if (t_err) {
                *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                _pathFile.save();

                return;
            }


            *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
            if (_lastError->load()) {
                _pathFile.save();

                return;
            }

            hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING;

            auto last_hw_time = tdata.hwState.XY.epoch().UTC();

            mcc_deduced_coord_pair_t<HARDWARE_T::hwMountType> tag_cp, mnt_cp;

            *_stopMoving = false;

            while (!_stopMoving->load()) {
                t_err = telemetry->telemetryData(&tdata);
                if (t_err) {
                    *_lastError = mcc_deduced_err(t_err, MccSimpleMovementControlsErrorCode::ERROR_GET_TELEMETRY);
                    (*cb_sptr)(STATUS_T::MOUNT_STATUS_ERROR);

                    return;
                }

                log_pos(tdata, dist);

                if (*_stopMoving) {
                    *_lastError = MccSimpleMovementControlsErrorCode::ERROR_STOPPED;
                    break;
                }

                *_lastError = check_pzones(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);
                if (_lastError->load()) {
                    break;
                }


                if (last_hw_time == tdata.hwState.XY.epoch().UTC()) {
                    logger->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
                    continue;
                }

                last_hw_time = tdata.hwState.XY.epoch().UTC();


                // resend new position since target coordinates are changed in time
                hw_state.movementState = HARDWARE_T::hardware_movement_state_t::HW_MOVE_TRACKING;
                hw_state.XY.setX(tdata.targetXY.x());
                hw_state.XY.setY(tdata.targetXY.y());
                hw_state.XY.setEpoch(tdata.hwState.XY.epoch());

                *_lastError = send_to_hardware(hw_state);
                if (_lastError->load()) {
                    break;
                }

                if (*_stopMoving) {
                    break;
                }

                // sleep here
                std::this_thread::sleep_for(_currentParams.trackingTelemetryInterval);
            }

            if (_stopMoving->load()) {  // external stop
                logger->logWarn("Tracking was stopped!");
                *_lastError = MccSimpleMovementControlsErrorCode::ERROR_OK;
            } else {
                *_stopMoving = true;

                logger->logInfo("Tracking finished");
                auto err = _lastError->load();
                logger->logInfo(
                    std::format("    exit code: {} {} {}", err.value(), err.category().name(), err.message()));
            }

            _pathFile.save();
        };
    }

    virtual ~MccSimpleMovementControls() = default;

    MccSimpleMovementControls(const MccSimpleMovementControls&) = delete;
    MccSimpleMovementControls(MccSimpleMovementControls&&) = default;

    error_t slewToTarget(bool slew_and_stop = false)
    {
        _slewingFunc(slew_and_stop);

        return *_lastError;
    }

    error_t trackTarget()
    {
        _trackingFunc();

        return *_lastError;
    }


    error_t stopMount()
    {
        if (*_stopMoving) {
            *_lastError = MccSimpleMovementControlsErrorCode::ERROR_ALREADY_STOPPED;
        } else {
            _stopMovingFunc();
        }

        return *_lastError;
    }

    error_t setMovementParams(movement_params_t const& params)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = params;

        return MccSimpleMovementControlsErrorCode::ERROR_OK;
    }

    movement_params_t getMovementParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }

    error_t mountMovementLastError() const
    {
        return _lastError->load();
    }

protected:
    std::function<void(bool)> _slewingFunc{};
    std::function<void()> _trackingFunc{};
    std::function<void()> _stopMovingFunc{};

    std::unique_ptr<std::atomic_bool> _stopMoving{new std::atomic_bool{false}};

    std::unique_ptr<std::mutex> _currentParamsMutex{new std::mutex{}};
    movement_params_t _currentParams{};

    std::unique_ptr<std::atomic<error_t>> _lastError{
        new std::atomic<error_t>{MccSimpleMovementControlsErrorCode::ERROR_OK}};

    PathFile _pathFile{};
};

}  // namespace mcc::impl