1  #include "StateMachine.h"
 2  
 3  #include <Arduino.h>
 4  
 5  #include "Core.h"
 6  
 7  namespace signalos {
 8  
 9  void StateMachine::init(Core& core, StateId initialState) {
10    // Exit current state if one exists (e.g., when triggering sleep from any state)
11    if (current_) {
12      current_->exit(core);
13    }
14  
15    currentId_ = initialState;
16    current_ = getState(initialState);
17  
18    if (current_) {
19      Serial.printf("[SM] Initial state: %d\n", static_cast<int>(initialState));
20      current_->enter(core);
21    } else {
22      Serial.printf("[SM] ERROR: No state registered for id %d\n", static_cast<int>(initialState));
23    }
24  }
25  
26  void StateMachine::update(Core& core) {
27    if (!current_) {
28      return;
29    }
30  
31    StateTransition trans = current_->update(core);
32  
33    if (trans.next != currentId_) {
34      transition(trans.next, core, trans.immediate);
35    }
36  
37    current_->render(core);
38  }
39  
40  void StateMachine::registerState(State* state) {
41    if (!state) return;
42  
43    if (stateCount_ >= MAX_STATES) {
44      Serial.println("[SM] ERROR: Too many states registered");
45      return;
46    }
47  
48    states_[stateCount_++] = state;
49    Serial.printf("[SM] Registered state: %d\n", static_cast<int>(state->id()));
50  }
51  
52  State* StateMachine::getState(StateId id) {
53    for (size_t i = 0; i < stateCount_; ++i) {
54      if (states_[i] && states_[i]->id() == id) {
55        return states_[i];
56      }
57    }
58    return nullptr;
59  }
60  
61  void StateMachine::transition(StateId next, Core& core, bool immediate) {
62    State* nextState = getState(next);
63  
64    if (!nextState) {
65      Serial.printf("[SM] ERROR: No state for id %d\n", static_cast<int>(next));
66      return;
67    }
68  
69    Serial.printf("[SM] Transition: %d -> %d%s\n", static_cast<int>(currentId_), static_cast<int>(next),
70                  immediate ? " (immediate)" : "");
71  
72    if (current_) {
73      current_->exit(core);
74    }
75  
76    currentId_ = next;
77    current_ = nextState;
78    current_->enter(core);
79  }
80  
81  }  // namespace signalos