...

include/mcc/mcc_generic_movecontrols.h

@@ -19,6 +19,7 @@
 
 
 #include <print>
+#include <typeinfo>
 
 #include "mcc_coordinate.h"
 #include "mcc_error.h"
@@ -240,8 +241,6 @@ template <std::movable PARAMS_T,
           MccGenericMovementControlsPolicy EXEC_POLICY = MccGenericMovementControlsPolicy::POLICY_ASYNC>
 class MccGenericMovementControls
 {
-    enum { STATE_SLEW, STATE_TRACK, STATE_IDLE } _currentState;
-
 public:
     static constexpr MccGenericMovementControlsPolicy executePolicy = EXEC_POLICY;
 
@@ -258,51 +257,45 @@ public:
           _stopFunc(std::forward<STOP_FUNC_T>(stop_func))
     {
         if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
-            *_doStop = false;
-            *_doSlew = false;
-            *_doTrack = false;
             *_stopMovementRequest = false;
-            _currentState = STATE_IDLE;
 
+            *_fsmState = STATE_IDLE;
+
+            // start thread of movements
             _fstFuture = std::async(
-                [&, this](std::stop_token stoken) {
+                [this](std::stop_token stoken) {
+                    auto do_state = _fsmState->load();
+
                     while (!stoken.stop_requested()) {
-                        if (_doStop->load()) {
+                        std::println("\n{:*^80}\n", "  WAIT LOCK  ");
-                            std::println("******* DO-STOP ({} {} {}) *******", _doStop->load(), _doSlew->load(),
-                                         _doTrack->load());
-                            *_stopMovementRequest = true;
-                            // _currentState = STATE_IDLE;
-                            _stopFunc();
-                            // *_doStop = false;
-                        }
-
-                        if (_doSlew->load()) {
-                            std::println("******* DO-SLEW ({} {} {}) *******", _doStop->load(), _doSlew->load(),
-                                         _doTrack->load());
-                            *_stopMovementRequest = false;
-                            _currentState = STATE_SLEW;
-                            _slewFunc(_slewAndStop->load());
-                            // _currentState = STATE_IDLE;
-                            // *_doSlew = false;
-                            continue;  // to guarantee execute doStop if tracking is requested!!!
-                        }
-
-                        if (_doTrack->load()) {
-                            std::println("******* DO-TRACK ({} {} {}) *******", _doStop->load(), _doSlew->load(),
-                                         _doTrack->load());
-                            *_stopMovementRequest = false;
-                            _currentState = STATE_TRACK;
-                            _trackFunc();
-                            // _currentState = STATE_IDLE;
-                            // *_doTrack = false;
-                        }
-
-                        _currentState = STATE_IDLE;
-
                         // wait here ...
                         _wakeupRequest->wait(false, std::memory_order_relaxed);
 
                         _wakeupRequest->clear();
+
+                        std::println("\n{:*^80}\n", "  UNLOCKED  ");
+
+                        // if (stoken.stop_requested()) {
+                        //     break;
+                        // }
+
+                        do_state = _fsmState->load();
+
+                        if (do_state & STATE_STOP) {
+                            // if (_fsmState->load() & STATE_STOP) {
+                            *_stopMovementRequest = true;
+                            _stopFunc();
+                        }
+
+                        if (do_state & STATE_SLEW) {
+                            // if (_fsmState->load() & STATE_SLEW) {
+                            *_stopMovementRequest = false;
+                            _slewFunc(_slewAndStop->load());
+                        } else if (do_state & STATE_TRACK) {
+                            // } else if (_fsmState->load() & STATE_TRACK) {
+                            *_stopMovementRequest = false;
+                            _trackFunc();
+                        }
                     }
                 },
                 _fstStopSource.get_token());
@@ -318,24 +311,9 @@ public:
     virtual ~MccGenericMovementControls()
     {
         if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
-            // if (_slewFuncFuture.valid()) {
+            *_fsmState = STATE_IDLE;
-            //     auto status = _slewFuncFuture.wait_for(_waitTimeout->load());
-            // }
-
-            // if (_trackFuncFuture.valid()) {
-            //     auto status = _trackFuncFuture.wait_for(_waitTimeout->load());
-            // }
-
-            // if (_stopFuncFuture.valid()) {
-            //     auto status = _stopFuncFuture.wait_for(_waitTimeout->load());
-            // }
-
-
-            *_doSlew = false;
-            *_doTrack = false;
-            // *_stopMovementRequest = false;
-
             _fstStopSource.request_stop();
+
             _wakeupRequest->test_and_set();
             _wakeupRequest->notify_one();
 
@@ -352,34 +330,18 @@ public:
         // *_stopMovementRequest = false;
 
         if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
-            // if (_slewFuncFuture.valid()) {  // already slewing
+            auto prev_state = _fsmState->load();
-            //     _slewFuncFuture = std::async(
-            //         std::launch::async,
-            //         [this](bool st) {
-            //             auto err = _stopFunc();  // first, stop mount
-            //             if (!err) {
-            //                 *_stopMovementRequest = false;
 
-            //                 return _slewFunc(st);
+            *_fsmState = STATE_SLEW;
-            //             } else {
-            //                 return err;
-            //             }
-            //         },
-            //         slew_and_stop);
-            // } else {
-            //     _slewFuncFuture = std::async(std::launch::async, _slewFunc, slew_and_stop);
-            // }
 
+            if (prev_state & STATE_SLEW || prev_state & STATE_TRACK) {
+                *_fsmState |= STATE_STOP;
 
-            if (_currentState == STATE_SLEW || _currentState == STATE_TRACK) {  // first, stop mount
-                *_doStop = true;
                 *_stopMovementRequest = true;  // to exit from slewing or tracking
-            } else {
-                *_doStop = false;
             }
-            *_doTrack = false;
+
-            *_doSlew = true;
             *_slewAndStop = slew_and_stop;
+
             _wakeupRequest->test_and_set();
             _wakeupRequest->notify_one();
 
@@ -396,19 +358,16 @@ public:
         // *_stopMovementRequest = false;
 
         if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
-            // if (!_trackFuncFuture.valid()) {
+            auto prev_state = _fsmState->load();
-            //     _trackFuncFuture = std::async(std::launch::async, _trackFunc);
+
-            // }  // already tracking, just ignore
+            if (!(prev_state & STATE_TRACK)) {
+                *_fsmState = STATE_TRACK;
+
+                if (prev_state & STATE_SLEW) {
+                    *_fsmState |= STATE_STOP;
 
-            if (_currentState != STATE_TRACK) {
-                if (_currentState == STATE_SLEW) {  // first, stop mount
-                    *_doStop = true;
                     *_stopMovementRequest = true;  // to exit from slewing
-                } else {
-                    *_doStop = false;
                 }
-                *_doSlew = false;
-                *_doTrack = true;
 
                 _wakeupRequest->test_and_set();
                 _wakeupRequest->notify_one();
@@ -427,14 +386,7 @@ public:
         // *_stopMovementRequest = true;
 
         if constexpr (executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC) {
-            // if future is valid then stop is already called
+            *_fsmState = STATE_STOP;
-            // if (!_stopFuncFuture.valid()) {
-            //     _stopFuncFuture = std::async(std::launch::async, _stopFunc);
-            // }
-
-            *_doStop = true;
-            *_doSlew = false;
-            *_doTrack = false;
 
             *_stopMovementRequest = true;  // to exit from slewing or tracking
 
@@ -475,22 +427,6 @@ protected:
     std::function<error_t()> _trackFunc{};
     std::function<error_t()> _stopFunc{};
 
-    std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
-                       std::future<error_t>,
-                       std::nullptr_t>
-        _slewFuncFuture{};
-
-    std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
-                       std::future<error_t>,
-                       std::nullptr_t>
-        _trackFuncFuture{};
-
-    std::conditional_t<executePolicy == MccGenericMovementControlsPolicy::POLICY_ASYNC,
-                       std::future<error_t>,
-                       std::nullptr_t>
-        _stopFuncFuture{};
-
-
     std::unique_ptr<std::atomic<std::chrono::nanoseconds>> _waitTimeout{
         new std::atomic<std::chrono::nanoseconds>{defaultWaitTimeout}};
 
@@ -504,11 +440,12 @@ protected:
     std::stop_source _fstStopSource{};
 
     std::unique_ptr<std::atomic_flag> _wakeupRequest{new std::atomic_flag};
-    std::unique_ptr<std::atomic_bool> _doStop{new std::atomic_bool{false}};
-    std::unique_ptr<std::atomic_bool> _doSlew{new std::atomic_bool{false}};
-    std::unique_ptr<std::atomic_bool> _doTrack{new std::atomic_bool{false}};
     std::unique_ptr<std::atomic_bool> _slewAndStop{new std::atomic_bool{false}};
 
+    enum { STATE_IDLE = 0x00, STATE_SLEW = 0x01, STATE_TRACK = 0x02, STATE_STOP = 0x04 };
+
+    std::unique_ptr<std::atomic_int> _fsmState{new std::atomic_int};
+
 
     // utility methods