// Simple FSM using mbed-events with RGB class
#include "mbed.h"

class RGBLed
{
public:
    RGBLed(PinName redpin, PinName greenpin, PinName bluepin);
    void write(float red,float green, float blue);
private:
    PwmOut _redpin;
    PwmOut _greenpin;
    PwmOut _bluepin;
};
 
RGBLed::RGBLed (PinName redpin, PinName greenpin, PinName bluepin)
    : _redpin(redpin), _greenpin(greenpin), _bluepin(bluepin)
{
    //50Hz PWM clock default a bit too low, go to 2000Hz (less flicker)
    _redpin.period(0.0005);
}
 
void RGBLed::write(float red,float green, float blue)
{
    _redpin = red;
    _greenpin = green;
    _bluepin = blue;
}

RGBLed rgb(p23, p24, p25);

class StopLight
{
private:
    EventQueue _queue;
    int _pending;

    enum state { RED, GREEN, YELLOW };

    // The _step function is a traditional FSM which enqueues transitions onto the event queue
    void _step(state transition) {
        switch (transition) {
            case RED:
                set_red();
                _pending = _queue.call_in(2000, this, &StopLight::_step, GREEN);
                break;

            case GREEN:
                set_green();
                _pending = _queue.call_in(1000, this, &StopLight::_step, YELLOW);
                break;

            case YELLOW:
                set_yellow();
                _pending = _queue.call_in(500, this, &StopLight::_step, RED);
                break;
        }
    }
    void set_red(){
        rgb.write(0.0,1.0,1.0);
    }
    void set_green(){
        rgb.write(1.0,0.0,1.0);
    }
    void set_yellow() {         // red plus green
        rgb.write(0.0,0.0,1.0);
    }
public:
    // The fire function can immediately change states in the FSM
    void fire() {
        _queue.cancel(_pending);
        _queue.call(this, &StopLight::_step, GREEN);
        _queue.dispatch();
    }
};

StopLight sl;

int main (void) {
    sl.fire();
    wait(osWaitForever);
}