D test FSM

fsm.d

import std.meta, std.traits;

version = DebugPrint;
version (DebugPrint)
{
    import std.experimental.logger;
    alias log = infof;

    extern(C) uint _currTime()
    {
        import std.datetime;
        static SysTime first;
        if (first == SysTime.init)
            first = Clock.currTime;
        return cast(uint)(Clock.currTime - first).total!"msecs";
    }
    extern(C) void _sideEffect() {}
}
else
{
    void log(Args...)(Args args)
    {
    }
    extern(C) uint _currTime();
    extern(C) void _sideEffect();
}

// https://github.com/Superstar64/tagged_union/blob/master/source/tagged_union.d
struct TaggedUnion(Types_...)
{
    alias Types = Types_;
    private size_t index = size_t.max;
    union
    {
        private Types data;
    }

    this(T)(T type) if (IndexOf!T != -1)
    {
        this = type;
    }

    auto ref opAssign(T)(T type) if (IndexOf!T != -1)
    {
        set!T(type);
    }

    auto opEquals(const typeof(this) other) const
    {
        if (id == other.id)
        {
            if (id == size_t.max)
                return true;
 
            foreach (c, Type; Types)
            {
                if (id == c)
                {
                    return getID!c == other.getID!c;
                }
            }
        }
        return false;
    }

    auto opEquals(T)(const T type) const if (IndexOf!T != -1)
    {
        if (id == IndexOf!T)
        {
            return type == get!T;
        }
        return false;
    }

@property:

    auto id() const
    {
        return index;
    }

    pure @trusted auto ref getID(size_t id)() inout if (id < Types.length)
    {
        // Create too much code...
        //assert(index == id);
        return data[id];
    }

    pure @trusted auto ref setID(size_t id)(Types[id] type) if (id < Types.length)
    {
        index = id;
        data[id] = type;
    }

    pure @safe bool isType(Type)() inout if (IndexOf!Type != -1)
    {
        return IndexOf!Type == id;
    }

    alias peek = isType;

    pure @safe auto ref get(Type)() inout if (IndexOf!Type != -1)
    {
        return getID!(IndexOf!Type);
    }

    alias get(size_t i) = getID!i;

    pure @safe auto ref set(Type)(Type type) if (IndexOf!Type != -1)
    {
        return setID!(IndexOf!Type)(type);
    }

    alias set(size_t i) = setID!i;
    string toString()
    {
        foreach (c, Type; Types)
        {
            if (id == c)
            {
                return Type.stringof;
            }
        }
        return "invalid";
    }

    template IndexOf(Type)
    {
        import std.typetuple : staticIndexOf;

        enum IndexOf = staticIndexOf!(Type, Types);
    }
}

void updateMembers(T)(ref T parent, uint now)
{
    // Any members with update functions?
    foreach(m; __traits(allMembers, T)) 
    {
        static if(__traits(compiles, __traits(getMember, parent, m).update(0)))
            __traits(getMember, parent, m).update(now);
    }
}

mixin template FSMLogic(FSM)
{
    alias State = TaggedUnion!(FSM.States);
    State _state, _nextState;
    size_t prevState;
    bool onEnter = false;
    uint now = 0;
    bool _events = false;
    
    void _enter()
    {
        log("State change: %s => %s", _state, _nextState);
        prevState = _state.id;
        _state = _nextState;
        _nextState = State.init;
        onEnter = true;
    }
    
    static void updateState(T)(ref T state, uint now)
    {
        // If State has an update function, call it
        static if (__traits(compiles, state.update(0)))
            state.update(now);

        // Any members with update functions?
        updateMembers(state, now);
    }
    
    void enterState(T)()
    {
        // Don't want any recursion here, so temporarily store state
        _nextState = T.init;
    }
    
    static size_t stateID(T)()
    {
        return State.IndexOf!T;
    }
    
    void begin()
    {
        enterState!(FSM.InitialState);
    }
    
    void update()
    {
        now = _currTime();
        
        onEnter = false;
        if (_nextState.id != State.init.id)
            _enter();

        // Call state handler
        sw: switch(_state.id)
        {
            foreach(Type; State.Types)
            {
                case stateID!(Type):
                    // Check if handler even exists
                    foreach (Overload; __traits(getOverloads, FSM, "onState"))
                    {
                        static if (is(Parameters!Overload == AliasSeq!(Type)))
                        {
                            onState(_state.get!Type);
                        }
                    }
                	// Update state
                	updateState(_state.get!Type, now);
                    break sw;
            }
            default:
            	break;
        }
    }
}

struct EdgeEvent
{
private:
    ubyte _state = 0;
    enum EventBit
    {
        event = 1,
        happened = 2
    }

public:
    void setEvent()
    {
        _state |= (EventBit.event | EventBit.happened);
    }

    @property bool event()
    {
        const result = _state & EventBit.event ? true : false;
        _state &= ~EventBit.event;
        return result;
    }

    // had event at least once
    @property bool happened()
    {
        return _state & EventBit.happened ? true : false;
    }
}

struct LevelEvent
{
private:
    ubyte _state = 0;
    enum EventBit
    {
        active = 1,
        rose = 2,
        fell = 4,
        happened = 8
    }

public:
    void setActive()
    {
        if (!active)
            _state |= (EventBit.rose | EventBit.happened | EventBit.active);
    }

    void setInActive()
    {
        if (active)
        {
            _state |= EventBit.fell;
            _state &= ~EventBit.active;
        }
    }

    @property bool rose()
    {
        const result = _state & EventBit.rose  ? true : false;
        _state &= ~EventBit.rose;
        return result;
    }

    @property bool fell()
    {
        const result = _state & EventBit.fell ? true : false;
        _state &= ~EventBit.fell;
        return result;
    }

    @property bool edge()
    {
        auto result = rose();
        result |= fell(); // No short-circuit || here, must always clear both flags!
        return result;
    }

    @property bool active()
    {
        return _state & EventBit.active ? true : false;
    }

    // was active at least once
    @property bool happened()
    {
        return _state & EventBit.happened ? true : false;
    }
}

// Real code starts here

struct Timer
{
    uint _target = uint.max;
    EdgeEvent alarm;
    alias alarm this;

    this(uint now, uint duration)
    {
        _target = now + duration;
    }
    
    void update(uint now)
    {
        log("%s (%s)", now, _target);
        if (now > _target)
        {
            _target = uint.max;
            alarm.setEvent();
        }
    }
}

struct Button(alias Addr)
{
    LevelEvent pressed;
    alias pressed this;

    void update(uint now)
    {
        import core.volatile;
        version (DebugPrint)
        {
            static bool last = false;
            auto val = !last;
            last = val;
        }
        else
            auto val = volatileLoad(Addr) & 0b1;
        if (val)
            pressed.setActive();
        else
            pressed.setInActive();
    }
}

struct Motor
{
private:
    ubyte _target, _current;
    uint _last;

    void setAngle()
    {
        import core.volatile;
        version (DebugPrint)
            log("set motor angle to %s", _current);
        else
            volatileStore(cast(ubyte*)0xDEADBEAF, _current);
    }

public:
    EdgeEvent moved;

    void update(uint now)
    {
        const angularV = 2048; // degree per sec
        const delta = ((now - _last) * angularV) / 1000;
        if (delta > 0)
        {
            if (_target > _current)
            {
                if (delta >= _target - _current)
                {
                    _current = _target;
                    moved.setEvent();
                }
                else
                {
                    _current += delta;
                }
                setAngle();
            }
            else if(_target < _current)
            {
                if (delta >= _current - _target)
                {
                    _current = _target;
                    moved.setEvent();
                }
                else
                {
                    _current -= delta;
                }
                setAngle();
            }
            _last = now;
        }
    }

    void moveAngle(ubyte target)
    {
        _target = target;
    }
}

enum ubyte* START_BUTTON = cast(ubyte*)0xDEADBEAF;
struct Phase1
{
    Timer timeout;
    Motor motor;
    Button!START_BUTTON startButton;
    ubyte buttonCount = 5;
}

struct Phase2
{
}

struct FSM
{
private:
	alias States = AliasSeq!(Phase1, Phase2);
    alias InitialState = Phase1;

	mixin FSMLogic!(typeof(this));
    
public:
    void onState(ref Phase1 phase1)
    {
        log("onState:Phase1");
        if (onEnter)
            phase1.timeout = Timer(now, 5);

        // Only executed once per event, even for level triggered. Cleared on check.
        if (phase1.timeout.event)
        {
            log("phase1 timout elapsed. Setting motor to 90 deg");
            phase1.motor.moveAngle(90);
        }

        // Executed if phase1.motor.moved.event happened at least once this state
        if (phase1.motor.moved.happened)
        {
            log("motor happened");
            // Only on rise
            if (phase1.startButton.rose)
            {
                phase1.buttonCount--;
                log("button count %s", phase1.buttonCount);
                if (phase1.buttonCount == 0)
                    enterState!Phase2;
            }
        }
    }
    
    void onState(ref Phase2 phase2)
    {
        log("onState:Phase2");
        if (onEnter)
        {
            log("Entered Phase2");
            _sideEffect();
            if (prevState == stateID!Phase1)
            {
                log("Previous phase was Phase1, goto Phase1");
                enterState!Phase1;
            }
        }
    }
}

void main()
{
    FSM fsm;
    fsm.begin();
    
    size_t iterations = 0;
    while(true)
    {
        fsm.update();

        version (DebugPrint)
        {
            // This is not an embedded system, we don't want 100% CPU...
            import core.thread : Thread;
            import std.datetime : msecs;
            Thread.sleep(1.msecs);
            if (++iterations == 100)
                return;
        }

    }
}