// example_enum_sm.cpp
// State machine with enum class — compiles and runs on PC
// No hardware dependencies

#include <cstdio>
#include <cstdint>
#include <cstring>

// ============================================================
// 1. LED State Machine
// ============================================================
enum class LedState : uint8_t {
    Off,
    On,
    BlinkSlow,
    BlinkFast,
    Count
};

const char* LedStateName(LedState s) {
    switch (s) {
        case LedState::Off:       return "Off";
        case LedState::On:        return "On";
        case LedState::BlinkSlow: return "BlinkSlow";
        case LedState::BlinkFast: return "BlinkFast";
        case LedState::Count:     return "Count";
        default:                  return "Unknown";
    }
}

class LedController {
public:
    LedController() : state_(LedState::Off), counter_(0) {}

    void SetState(LedState s) {
        state_ = s;
        counter_ = 0;
        printf("State → %s\n", LedStateName(state_));
    }

    LedState GetState() const { return state_; }

    // Call every "tick" (simulated ms)
    void Tick() {
        counter_++;
        switch (state_) {
        case LedState::Off:
            // Do nothing
            break;
        case LedState::On:
            printf("[LED ON]\n");
            break;
        case LedState::BlinkSlow:
            if (counter_ % 1000 == 0)
                printf("[LED BLINK SLOW] tick=%u\n", counter_);
            break;
        case LedState::BlinkFast:
            if (counter_ % 200 == 0)
                printf("[LED BLINK FAST] tick=%u\n", counter_);
            break;
        case LedState::Count:
            break;  // Sentinel, shouldn't be used
        }
    }

private:
    LedState state_;
    uint32_t counter_;
};

// ============================================================
// 2. Button Debounce State Machine
// ============================================================
enum class DebounceState : uint8_t {
    Idle,
    DebouncePress,
    Pressed,
    DebounceRelease
};

class Button {
public:
    Button() : state_(DebounceState::Idle), debounce_start_(0) {}

    // Simulate reading a pin
    void SetRaw(bool pressed) { raw_ = pressed; }

    // Call every 1ms
    void Tick(uint32_t now_ms) {
        switch (state_) {
        case DebounceState::Idle:
            if (raw_) {
                state_ = DebounceState::DebouncePress;
                debounce_start_ = now_ms;
                printf("[Button] Press detected, debouncing...\n");
            }
            break;

        case DebounceState::DebouncePress:
            if (now_ms - debounce_start_ >= 30) {
                if (raw_) {
                    state_ = DebounceState::Pressed;
                    printf("[Button] PRESSED! (%ums)\n", now_ms);
                } else {
                    state_ = DebounceState::Idle;
                    printf("[Button] False alarm\n");
                }
            }
            break;

        case DebounceState::Pressed:
            if (!raw_) {
                state_ = DebounceState::DebounceRelease;
                debounce_start_ = now_ms;
            }
            break;

        case DebounceState::DebounceRelease:
            if (now_ms - debounce_start_ >= 30) {
                if (!raw_) {
                    state_ = DebounceState::Idle;
                    printf("[Button] Released at %ums\n", now_ms);
                } else {
                    state_ = DebounceState::Pressed;
                }
            }
            break;
        }
    }

    bool IsPressed() const { return state_ == DebounceState::Pressed; }

private:
    DebounceState state_;
    uint32_t debounce_start_;
    bool raw_ = false;
};

// ============================================================
// 3. Flight Mode State Machine
// ============================================================
enum class FlightMode : uint8_t {
    Disarmed,
    Stabilize,
    AltHold,
    Loiter,
    RTL,
    Land,
    Failsafe
};

enum class FlightEvent : uint8_t {
    Arm,
    Disarm,
    ModeSwitch,
    RcLoss,
    RcRestore,
    BatteryLow,
    BatteryCritical,
    AltReached,
    LandComplete
};

const char* FlightModeName(FlightMode m) {
    switch (m) {
        case FlightMode::Disarmed:  return "DISARMED";
        case FlightMode::Stabilize: return "STABILIZE";
        case FlightMode::AltHold:   return "ALT_HOLD";
        case FlightMode::Loiter:    return "LOITER";
        case FlightMode::RTL:       return "RTL";
        case FlightMode::Land:      return "LAND";
        case FlightMode::Failsafe:  return "FAILSAFE";
        default:                    return "???";
    }
}

class FlightController {
public:
    FlightController() : mode_(FlightMode::Disarmed) {}

    FlightMode GetMode() const { return mode_; }

    bool HandleEvent(FlightEvent event) {
        FlightMode next = Evaluate(event, mode_);
        if (next != mode_) {
            printf("[FC] %s → %s (event=%d)\n",
                   FlightModeName(mode_), FlightModeName(next),
                   static_cast<int>(event));
            mode_ = next;
            return true;
        }
        return false;
    }

    void Tick() {
        // Mode-specific update (simplified)
        switch (mode_) {
        case FlightMode::Stabilize:
            printf("[FC] Stabilizing...\n");
            break;
        case FlightMode::AltHold:
            printf("[FC] Holding altitude...\n");
            break;
        case FlightMode::Loiter:
            printf("[FC] Holding position...\n");
            break;
        case FlightMode::RTL:
            printf("[FC] Returning to launch...\n");
            break;
        case FlightMode::Land:
            printf("[FC] Landing...\n");
            break;
        default:
            break;
        }
    }

private:
    FlightMode mode_;

    static FlightMode Evaluate(FlightEvent event, FlightMode current) {
        switch (current) {
        case FlightMode::Disarmed:
            if (event == FlightEvent::Arm) return FlightMode::Stabilize;
            return current;

        case FlightMode::Stabilize:
            switch (event) {
            case FlightEvent::ModeSwitch: return FlightMode::AltHold;
            case FlightEvent::RcLoss:     return FlightMode::Failsafe;
            case FlightEvent::Disarm:     return FlightMode::Disarmed;
            default: return current;
            }

        case FlightMode::AltHold:
            switch (event) {
            case FlightEvent::ModeSwitch: return FlightMode::Loiter;
            case FlightEvent::RcLoss:     return FlightMode::RTL;
            case FlightEvent::BatteryLow: return FlightMode::RTL;
            default: return current;
            }

        case FlightMode::Loiter:
            switch (event) {
            case FlightEvent::ModeSwitch:    return FlightMode::Stabilize;
            case FlightEvent::BatteryLow:    return FlightMode::RTL;
            case FlightEvent::BatteryCritical: return FlightMode::Land;
            case FlightEvent::RcLoss:        return FlightMode::RTL;
            default: return current;
            }

        case FlightMode::RTL:
            if (event == FlightEvent::AltReached) return FlightMode::Land;
            return current;

        case FlightMode::Land:
            if (event == FlightEvent::LandComplete) return FlightMode::Disarmed;
            return current;

        case FlightMode::Failsafe:
            if (event == FlightEvent::RcRestore) return FlightMode::Stabilize;
            return current;

        default:
            return current;
        }
    }
};

// ============================================================
// 4. RAII Timer (PC Simulation)
// ============================================================
class ScopedTimer {
public:
    ScopedTimer(const char* name)
        : name_(name), start_(GetMs()) {}

    ~ScopedTimer() {
        uint32_t elapsed = GetMs() - start_;
        printf("[Timer] %s took %ums\n", name_, elapsed);
    }

    ScopedTimer(const ScopedTimer&) = delete;

private:
    const char* name_;
    uint32_t start_;

    static uint32_t GetMs() {
        return static_cast<uint32_t>(
            std::clock() * 1000 / CLOCKS_PER_SEC);
    }
};

// ============================================================
// Main Demo
// ============================================================
int main() {
    printf("===== C++ Enum Class & RAII Demo =====\n\n");

    // === LED Demo ===
    printf("--- LED Controller ---\n");
    LedController led;
    led.SetState(LedState::BlinkSlow);
    for (int i = 0; i < 5; i++)
        led.Tick();
    printf("\n");

    // === Button Demo ===
    printf("--- Button Debounce ---\n");
    Button btn;

    // Simulate a button press with noise
    int t = 0;
    auto press = [&](bool state, int duration_ms) {
        btn.SetRaw(state);
        for (int i = 0; i < duration_ms; i++) {
            btn.Tick(t++);
            // Add noise: bounce during first 5ms of press
            if (state && i < 5) {
                btn.SetRaw((i % 2) == 0);
            }
        }
    };

    press(false, 50);    // Idle
    press(true, 200);    // Press (with bounce simulation)
    press(false, 50);    // Release
    printf("\n");

    // === Flight Controller Demo ===
    printf("--- Flight Controller ---\n");
    FlightController fc;

    // Sequence: Arm → Stabilize → AltHold → Loiter → Battery Low → RTL → Land
    fc.HandleEvent(FlightEvent::Arm);
    fc.HandleEvent(FlightEvent::ModeSwitch);
    fc.HandleEvent(FlightEvent::ModeSwitch);
    fc.HandleEvent(FlightEvent::BatteryLow);
    fc.Tick();  // RTL update
    fc.HandleEvent(FlightEvent::AltReached);  // Land
    fc.HandleEvent(FlightEvent::LandComplete); // Disarm
    printf("\n");

    // === RAII Timer ===
    printf("--- RAII Timer ---\n");
    {
        ScopedTimer timer("RAII Demo Section");
        // Simulate some work
        volatile uint32_t sum = 0;
        for (int i = 0; i < 1000000; i++)
            sum += i;
        // Timer destructor runs here → prints elapsed time
    }

    printf("\n===== Demo Complete =====\n");
    return 0;
}