#pragma once

#include <fstream>

#include "mcc_defaults.h"
#include "mcc_generics.h"
#include "mcc_moving_model_common.h"

namespace mcc
{

enum class MccSimpleMovingControlsErrorCode : int {
    ERROR_OK,
    ERROR_HW_GETSTATE,
    ERROR_HW_SETSTATE,
    ERROR_PCM_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


namespace std
{

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

}  // namespace std



namespace mcc
{

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

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

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

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

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


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



class MccSimpleMovingControls
{
    static constexpr auto DEG90INRADS = std::numbers::pi / 2.0;

    class PathFile
    {
    public:
        PathFile(const std::string& filename = "") : _filename(filename), _st() {}

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

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

        ~PathFile()
        {
            save();
        }

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

            fst.open(_filename);

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

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

            _filename.clear();

            return true;
        }

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

public:
    typedef std::error_code error_t;

    enum Mode { MOVING_MODE_SLEW, MOVING_MODE_TRACK, MOVING_MODE_ERROR };

    //     typedef std::CallbackFuncTion<void(Mode mode)> mode_switch_callback_t;

    // protected:
    //     constexpr static auto defaultModeSwitchCallback = [](Mode) {};

    // public:

    template <mcc_generic_mount_c MountT,
              std::invocable<typename MountT::mount_status_t> CallbackFuncT =
                  decltype([](typename MountT::mount_status_t) {})>
    MccSimpleMovingControls(
        MountT* mount,
        CallbackFuncT&& mode_switch_calback = [](typename MountT::mount_status_t) {})
        : _stopMoving(new std::atomic_bool), _currentParamsMutex(new std::mutex), _lastError(new std::atomic<error_t>)
    {
        auto send_to_hardware = [mount](typename MountT::hardware_state_t const& hw_state) {
            mount->logDebug(std::format("Send to hardware: X = {} degs, Y = {} degs",
                                        mcc::MccAngle{hw_state.X}.degrees(), mcc::MccAngle{hw_state.Y}.degrees()));

            auto hw_err = mount->hardwareSetState(hw_state);
            if (hw_err) {
                return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
            }

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

            return MccSimpleMovingControlsErrorCode::ERROR_OK;
        };


        auto check_pzones = [mount, this](MccTelemetryData const& tdata, double min_time_to_pzone_in_secs,
                                          double braking_accelX, double braking_accelY) {
            bool in_zone;
            std::vector<bool> in_zone_vec;
            MccCelestialPoint cpt;

            auto distXY = mcc_compute_distance(tdata, min_time_to_pzone_in_secs, braking_accelX, braking_accelY);

            mount->logTrace(
                std::format("    the distance that will be covered in the next {} seconds: X-axis: {}, Y-axis: {}",
                            min_time_to_pzone_in_secs, mcc::MccAngleFancyString(distXY.first),
                            mcc::MccAngleFancyString(distXY.second)));

            // calculate coordinates at current speed '_currentParams.minTimeToPZone' seconds ahead
            // and check them for getting into the prohibited zones
            if constexpr (mccIsEquatorialMount(MountT::mountType)) {
                cpt.X = tdata.HA + distXY.first;
                cpt.Y = tdata.DEC_APP + distXY.second;

                if (cpt.Y > DEG90INRADS) {
                    cpt.Y = DEG90INRADS;
                }
                if (cpt.Y < -DEG90INRADS) {
                    cpt.Y = -DEG90INRADS;
                }

            } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
                cpt.X = tdata.AZ + distXY.first;
                cpt.Y = tdata.ZD + distXY.second;
                if (cpt.Y < 0.0) {
                    cpt.Y = 0.0;
                }
                if (cpt.Y > std::numbers::pi) {
                    cpt.Y = std::numbers::pi;
                }
            }
            mcc_tp2tp(tdata.time_point, cpt.time_point);

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

            in_zone_vec.clear();
            auto pz_err = mount->inPZone(cpt, &in_zone, &in_zone_vec);
            if (pz_err) {
                return mcc_deduce_error_code(pz_err, MccSimpleMovingControlsErrorCode::ERROR_PZONE_CONTAINER_COMP);
            }

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

                mount->logError("target point is near prohibited zone (zone index: {})! Entered target coordinates:",
                                i);
                mount->logError(std::format(
                    "    RA-APP, DEC-APP, HA, LST: {}, {}, {}, {}", mcc::MccAngle{tdata.RA_APP}.sexagesimal(true),
                    mcc::MccAngle{tdata.DEC_APP}.sexagesimal(), mcc::MccAngle{tdata.HA}.sexagesimal(true),
                    mcc::MccAngle{tdata.LST}.sexagesimal(true)));
                mount->logError(std::format("    AZ, ZD, ALT: {}, {}, {}", mcc::MccAngle{tdata.AZ}.sexagesimal(),
                                            mcc::MccAngle{tdata.ZD}.sexagesimal(),
                                            mcc::MccAngle{tdata.ALT}.sexagesimal()));

                mount->logError(std::format("    hardware X, Y: {}, {}", mcc::MccAngle{tdata.X}.sexagesimal(),
                                            mcc::MccAngle{tdata.Y}.sexagesimal()));

                return MccSimpleMovingControlsErrorCode::ERROR_NEAR_PZONE;
            }

            return MccSimpleMovingControlsErrorCode::ERROR_OK;
        };

        auto log_pos = [mount, this](typename MountT::hardware_state_t const& hw_state, MccTelemetryData const& tdata) {
            if constexpr (mccIsEquatorialMount(MountT::mountType)) {
                mount->logTrace(std::format("    current target: HA = {}, DEC = {}",
                                            mcc::MccAngle(tdata.target.HA).sexagesimal(true),
                                            mcc::MccAngle(tdata.target.DEC_APP).sexagesimal()));
                mount->logTrace(std::format("    current mount:  HA = {}, DEC = {}",
                                            mcc::MccAngle(tdata.HA).sexagesimal(true),
                                            mcc::MccAngle(tdata.DEC_APP).sexagesimal()));

                _pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.HA << " "
                          << tdata.target.DEC_APP << " " << tdata.HA << " " << tdata.DEC_APP << " "
                          << (tdata.target.HA - tdata.HA) << " " << (tdata.target.DEC_APP - tdata.DEC_APP) << " "
                          << (int)hw_state.moving_state << "\n";

            } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
                mount->logTrace(std::format("    target: AZ = {}, ZD = {}",
                                            mcc::MccAngle(tdata.target.AZ).sexagesimal(),
                                            mcc::MccAngle(tdata.target.ZD).sexagesimal()));
                mount->logTrace(std::format("    mount:  AZ = {}, ZD = {}", mcc::MccAngle(tdata.AZ).sexagesimal(),
                                            mcc::MccAngle(tdata.ZD).sexagesimal()));

                _pathFile << tdata.time_point.time_since_epoch().count() << " " << tdata.target.AZ << " "
                          << tdata.target.ZD << " " << tdata.AZ << " " << tdata.ZD << " "
                          << (tdata.target.AZ - tdata.AZ) << " " << (tdata.target.ZD - tdata.ZD) << " "
                          << (int)hw_state.moving_state << "\n";
            }
        };

        *_stopMoving = true;
        *_lastError = MccSimpleMovingControlsErrorCode::ERROR_OK;

        using cb_func_t = std::function<void(typename MountT::mount_status_t)>;
        auto cb_sptr = std::shared_ptr<cb_func_t>(new cb_func_t(std::forward<CallbackFuncT>(mode_switch_calback)));


        /*  stop moving function */

        _stopMovingFunc = [mount, this]() {
            typename MountT::hardware_state_t hw_state;
            hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED;

            *_stopMoving = true;

            *_lastError = send_to_hardware(hw_state);
        };


        /*  slewing function */

        _slewingFunc = [mount, cb_sptr, send_to_hardware, check_pzones, log_pos, 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
                    _pathFile.setFilename(_currentParams.slewingPathFilename);
                } else {
                    mount->logError("Slewing path filename is empty! Do not save it!");
                }
            }

            mount->logInfo(
                std::format("Start slewing in mode '{}'", (slew_and_stop ? "SLEW-AND-STOP" : "SLEW-AND-TRACK")));
            mount->logInfo(std::format("    slewing process timeout: {} secs", _currentParams.slewTimeout.count()));
            if (!slew_and_stop) {
                mount->logInfo(std::format("    slewing tolerance radius: {} arcsecs",
                                           mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()));
            }
            mount->logInfo(std::format("    braking acceleration X: {} degs/s^2 (in config: {} rads/s^2)",
                                       mcc::MccAngle(braking_accelX).degrees(), _currentParams.brakingAccelX));
            mount->logInfo(std::format("    braking acceleration Y: {} degs/s^2 (in config: {} rads/s^2)",
                                       mcc::MccAngle(braking_accelY).degrees(), _currentParams.brakingAccelY));
            mount->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: "
                      << mcc::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}> <moving state>\n";


            typename MountT::error_t t_err;
            MccTelemetryData tdata;

            {
                std::lock_guard lock{*_currentParamsMutex};
                t_err = mount->telemetryData(&tdata);

                if (t_err) {
                    *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
                    return;
                }
            }

            auto last_hw_time = tdata.time_point;

            // bool in_zone;
            // std::vector<bool> in_zone_vec;

            // MccCelestialPoint cpt;

            // if constexpr (mccIsEquatorialMount(MountT::mountType)) {
            //     cpt.pair_kind = MccCoordPairKind::COORDS_KIND_HADEC_APP;
            // } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
            //     cpt.pair_kind = MccCoordPairKind::COORDS_KIND_AZZD;
            // } else {
            //     static_assert(false, "UNKNOWN MOUNT TYPE!");
            // }

            typename MountT::hardware_state_t hw_state;

            auto hw_err = mount->hardwareGetState(&hw_state);
            if (hw_err) {
                *_stopMoving = true;
                *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
                return;
            }


            hw_state.X = (double)tdata.target.X;
            hw_state.Y = (double)tdata.target.Y;

            {
                std::lock_guard lock{*_currentParamsMutex};

                hw_state.speedX = _currentParams.slewRateX;
                hw_state.speedY = _currentParams.slewRateY;
            }
            hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;

            // start slewing ...
            error_t err = send_to_hardware(hw_state);
            if (err) {
                *_lastError = err;
                mount->logError(std::format("start slewing: an error occured while sending hardware state: {} {} {}",
                                            err.value(), err.category().name(), err.message()));
                return;
            }

            *cb_sptr(MountT::mount_status_t::SLEWING);  // send the status to the mount


            double dist;


            std::chrono::steady_clock::time_point start_slewing_tp, last_adjust_tp;
            start_slewing_tp = std::chrono::steady_clock::now();
            last_adjust_tp = start_slewing_tp;

            std::pair<double, double> distXY;
            bool tag_var_coord = true;

            if (tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZALT ||
                tdata.target.pair_kind == MccCoordPairKind::COORDS_KIND_AZZD) {
                tag_var_coord = false;
            }

            auto start_point = tdata.time_point;  // needed for trajectory file

            // main loop (simply monitors the current position taking into account the prohibited zones, as well as the
            // timeout of the entire process)
            while (!*_stopMoving) {
                // wait for updated telemetry data
                {
                    std::lock_guard lock{*_currentParamsMutex};

                    t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                    if (t_err) {
                        *_lastError =
                            mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
                        break;
                    }

                    last_hw_time = tdata.time_point;
                }

                hw_err = mount->hardwareGetState(&hw_state);
                if (hw_err) {
                    *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
                    break;
                }

                log_pos(hw_state, tdata);

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

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

                {
                    std::lock_guard lock{*_currentParamsMutex};

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

                if (slew_and_stop && !tag_var_coord) {  // just wait for mount to be stopped
                    if (hw_state.moving_state == MountT::hardware_moving_state_t::HW_MOVE_STOPPED) {
                        mount->logInfo("mount moving state is STOPPED - exit!");
                        break;
                    }
                } else {
                    if (last_hw_time == tdata.time_point) {
                        mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
                        continue;
                    }

                    last_hw_time = tdata.time_point;

                    t_err = mount->targetToMountDist(&dist);
                    if (t_err) {
                        *_lastError =
                            mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
                        break;
                    }

                    mount->logTrace(std::format("    target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));

                    if ((dist <= _currentParams.slewToleranceRadius) &&
                        (hw_state.moving_state ==
                         MountT::hardware_moving_state_t::HW_MOVE_GUIDING)) {  // stop slewing and exit from
                        // cycle
                        mount->logInfo("target-to-mount distance is lesser than slew tolerance radius - exit!");

                        if (slew_and_stop) {
                            stopMount();
                        }

                        break;
                    }


                    if (*_stopMoving) {
                        *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
                        break;
                        // return MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
                    }


                    // resend new position since target coordinates are changed in time
                    hw_state.X = (double)tdata.target.X;
                    hw_state.Y = (double)tdata.target.Y;

                    err = send_to_hardware(hw_state);
                    if (err) {
                        *_lastError = err;
                        break;
                    }
                }

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

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

            *_stopMoving = true;

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

            _pathFile.save();

            // get final position

            if (!err) {
                // wait for updated telemetry data
                {
                    std::lock_guard lock{*_currentParamsMutex};

                    t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                    if (t_err) {
                        *_lastError =
                            mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
                        return;
                    }
                }

                t_err = mount->targetToMountDist(&dist);
                if (t_err) {
                    *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
                    return;
                }

                log_pos(hw_state, tdata);

                mount->logDebug(std::format("    target-to-mount distance {}", mcc::MccAngleFancyString(dist)));

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


        /*  tracking function  */

        _trackingFunc = [mount, cb_sptr, check_pzones, send_to_hardware, log_pos, this]() {
            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.trackingPathFilename.empty()) {  // open slewing trajectory file
                    _pathFile.setFilename(_currentParams.trackingPathFilename);
                } else {
                    mount->logError("Tracking path filename is empty! Do not save it!");
                }
            }

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


            _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}> <moving state>\n";


            typename MountT::hardware_state_t hw_state;

            error_t err;
            MccTelemetryData tdata;
            double dist;

            auto last_hw_time = tdata.time_point;

            *cb_sptr(MountT::mount_status_t::TRACKING);  // send the status to the mount

            while (!*_stopMoving) {
                // wait for updated telemetry data
                {
                    std::lock_guard lock{*_currentParamsMutex};

                    auto t_err = mount->waitForTelemetryData(&tdata, _currentParams.telemetryTimeout);

                    if (t_err) {
                        *_lastError =
                            mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
                        break;
                    }

                    last_hw_time = tdata.time_point;
                }

                auto hw_err = mount->hardwareGetState(&hw_state);
                if (hw_err) {
                    *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
                    break;
                }

                log_pos(hw_state, tdata);

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

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

                if (last_hw_time == tdata.time_point) {
                    mount->logTrace("Same hardware timepoint! Just continue to polling!\n\n\n\n");
                    continue;
                }

                last_hw_time = tdata.time_point;

                auto t_err = mount->targetToMountDist(&dist);
                if (t_err) {
                    *_lastError = mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_DIST_TELEMETRY);
                    break;
                }

                mount->logTrace(std::format("    target-to-mount distance: {}", mcc::MccAngleFancyString(dist)));

                // resend new position since target coordinates are changed in time
                hw_state.X = (double)tdata.target.X;
                hw_state.Y = (double)tdata.target.Y;

                err = send_to_hardware(hw_state);
                if (err) {
                    *_lastError = err;
                    break;
                }

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

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

            *_stopMoving = true;

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

            _pathFile.save();
        };
    }

    virtual ~MccSimpleMovingControls()
    {
        *_stopMoving = true;
    }

    error_t slewToTarget(bool slew_and_stop = false)
    {
        return *_lastError;
    }

    error_t trackTarget()
    {
        return *_lastError;
    }


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

        return *_lastError;
    }

    error_t setMovingParams(MccSimpleMovingModelParams params)
    {
        std::lock_guard lock{*_currentParamsMutex};

        _currentParams = std::move(params);

        return MccSimpleMovingControlsErrorCode::ERROR_OK;
    }

    MccSimpleMovingModelParams getMovingParams() const
    {
        std::lock_guard lock{*_currentParamsMutex};

        return _currentParams;
    }

    error_t mountMovingLastError() const
    {
        return *_lastError;
    }

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

    std::unique_ptr<std::atomic_bool> _stopMoving;

    std::unique_ptr<std::mutex> _currentParamsMutex;
    MccSimpleMovingModelParams _currentParams{};

    std::unique_ptr<std::atomic<error_t>> _lastError;

    PathFile _pathFile{};
};

}  // namespace mcc