...

mcc/mcc_moving_controls.h

@@ -105,6 +105,62 @@ inline std::error_code make_error_code(MccSimpleMovingControlsErrorCode ec)
 
 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;
 
@@ -125,30 +181,10 @@ public:
         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,
-                                        MccTelemetryData const& tdata) {
+        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 start_point = tdata.time_point;  // needed for trajectory file
-
-            if constexpr (mccIsEquatorialMount(MountT::mountType)) {
-                mount->logDebug(std::format("    entered target: HA = {}, DEC = {}",
-                                            mcc::MccAngle{tdata.target.HA}.sexagesimal(true),
-                                            mcc::MccAngle{tdata.target.DEC_APP}.sexagesimal()));
-                mount->logDebug(std::format("    current mount:  HA = {}, DEC = {}",
-                                            mcc::MccAngle{tdata.HA}.sexagesimal(true),
-                                            mcc::MccAngle{tdata.DEC_APP}.sexagesimal()));
-
-            } else if constexpr (mccIsAltAzMount(MountT::mountType)) {
-                mount->logDebug(std::format("    entered target: AZ = {}, ZD = {}",
-                                            mcc::MccAngle{tdata.target.AZ}.sexagesimal(),
-                                            mcc::MccAngle{tdata.target.ZD}.sexagesimal()));
-                mount->logDebug(std::format("    current mount:  AZ = {}, ZD = {}",
-                                            mcc::MccAngle{tdata.AZ}.sexagesimal(),
-                                            mcc::MccAngle{tdata.ZD}.sexagesimal()));
-            }
-
             auto hw_err = mount->hardwareSetState(hw_state);
             if (hw_err) {
                 return mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_SETSTATE);
@@ -160,19 +196,134 @@ public:
         };
 
 
+        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)));
 
-        _slewingFunc = [mount, cb_sptr, this](bool slew_and_stop) {
+
+        /*  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;
 
-            bool store_path = false;
-            std::ofstream fst;
-            using path_tp_t = std::chrono::duration<double>;  // seconds represented as double
 
             {
                 // std::lock_guard lock{*_currentParamsMutex};
@@ -192,13 +343,9 @@ public:
                     std::chrono::duration_cast<std::chrono::duration<double>>(_currentParams.minTimeToPZone).count();
 
                 if (!_currentParams.slewingPathFilename.empty()) {  // open slewing trajectory file
-                    fst.open(_currentParams.slewingPathFilename);
-                    if (fst.is_open()) {
-                        store_path = true;
-                    } else {
-                        mount->logError(std::format("Cannot open slewing path file: {}! Do not save it!",
-                                                    _currentParams.slewingPathFilename));
-                    }
+                    _pathFile.setFilename(_currentParams.slewingPathFilename);
+                } else {
+                    mount->logError("Slewing path filename is empty! Do not save it!");
                 }
             }
 
@@ -216,18 +363,16 @@ public:
             mount->logInfo(std::format("    min time to prohibited zone: {} seconds", min_time_to_pzone_in_secs));
 
 
-            if (store_path) {
-                fst << "# \n";
-                fst << "# Slewing trajectory, " << std::chrono::system_clock::now() << "\n";
-                fst << "# Config:\n";
-                fst << "#     slewing tolerance radius: " << mcc::MccAngle{_currentParams.slewToleranceRadius}.arcsecs()
-                    << " arcsecs\n";
-                fst << "#     slewing process timeout: " << _currentParams.slewTimeout.count() << " secs\n";
-                fst << "# \n";
-                fst << "# Format (time is in seconds, coordinates are in radians): \n";
-                fst << "#     <time-since-start> <target X> <target Y> <mount X> <mount Y> <dX_{target-mount}> "
-                       "<dY_{target-mount}> <moving state>\n";
-            }
+            _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;
@@ -238,32 +383,33 @@ public:
                 t_err = mount->telemetryData(&tdata);
 
                 if (t_err) {
-                    return *_lastError =
-                               mcc_deduce_error_code(t_err, MccSimpleMovingControlsErrorCode::ERROR_GET_TELEMETRY);
+                    *_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;
+            // bool in_zone;
+            // std::vector<bool> in_zone_vec;
 
-            MccCelestialPoint cpt;
+            // 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!");
-            }
+            // 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;
-                return *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
+                *_lastError = mcc_deduce_error_code(hw_err, MccSimpleMovingControlsErrorCode::ERROR_HW_GETSTATE);
+                return;
             }
 
 
@@ -278,14 +424,321 @@ public:
             }
             hw_state.moving_state = MountT::hardware_moving_state_t::HW_MOVE_SLEWING;
 
-            if (*_stopMoving) {
-                mount->logDebug("slewing was stopped!");
-                return *_lastError = MccSimpleMovingControlsErrorCode::ERROR_STOPPED;
+            // 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;
+                }
             }
         };
 
-        _trackingFunc = [mount, cb_sptr, this]() {
 
+        /*  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();
         };
     }
 
@@ -305,12 +758,12 @@ public:
     }
 
 
-    error_t stopMountMoving()
+    error_t stopMount()
     {
         if (*_stopMoving) {
             *_lastError = MccSimpleMovingControlsErrorCode::ERROR_ALREADY_STOPPED;
         } else {
-            *_stopMoving = true;
+            _stopMovingFunc();
         }
 
         return *_lastError;
@@ -340,6 +793,7 @@ public:
 protected:
     std::function<void(bool)> _slewingFunc{};
     std::function<void()> _trackingFunc{};
+    std::function<void()> _stopMovingFunc{};
 
     std::unique_ptr<std::atomic_bool> _stopMoving;
 
@@ -347,6 +801,8 @@ protected:
     MccSimpleMovingModelParams _currentParams{};
 
     std::unique_ptr<std::atomic<error_t>> _lastError;
+
+    PathFile _pathFile{};
 };
 
 }  // namespace mcc