Machine

typedef void State;//statefunctions should be of this type
//macro to call runstate member fuction by runstate function pointer
#define EXEC(object) ((object).*(object.Exec))()
class SM;

//save som time by reading variable directly
extern volatile unsigned long timer0_millis;

typedef void State;//statefunctions should be of this type
typedef State (*Pstate)();//pointer to statefunction

class SM{
typedef void(SM::*PRun)();//defines runstate pointer
  public:
    SM(Pstate);//constructor simple SM
    SM(Pstate, Pstate);//constructor machine with head and body state
    PRun Exec;//runstate pointer
    void Set(Pstate);//simple statechange
    void Set(Pstate, Pstate);//statechange to head and body state
    void Finish();//suspend mschine execution
    boolean Finished;//flag suspended machine
  void *data;//generic data pointer
    void Restart();//resume from suspension with last known states
    unsigned long Statetime(){//return ms since entry, inline for efficiency
      return timer0_millis - Mark;
    };//Statetime();
    boolean Timeout(unsigned long T){//returns true if Statetime > T
      return (timer0_millis - Mark)>T;
    };//Timeout()
  private:
    Pstate Head;//pointer to head state function
    Pstate Body;//pointer to body state function
    void Run(){//runstate running
      Body();
    };//Run()
    void FRun(){//runstate entry
      Mark = timer0_millis;//reset timer on entry
      Head();//call head state function
      Exec = &SM::Run;//set runstate to running
    };//FRun()
    void Idle();//runstate finished
    unsigned long Mark;//timer mark
};

//
//
//
//
//
//
//
//
//
//

SM::SM(Pstate Initial){//create simple SM
  Set(Initial);//set initial state
};//constructor

//create SM with initial head and body states
SM::SM(Pstate First, Pstate Rest){
  Set(First, Rest);//set initial head and body state functions
}//constructor

void SM::Set(Pstate Current){
  Finished = false;//clear flag, just in case
  Body = Current;//common head-
  Head = Current;//-and body state function
  Exec = &SM::FRun;//set runstate to entry
};//Set()

void SM::Set(Pstate First, Pstate Rest){
  Finished = false;//clear flag just in case
  Head = First;//separate head-
  Body = Rest;//-and body state functions
  Exec =&SM::FRun;//set runstate to entry
};//set()

void SM::Finish(){
  Exec = &SM::Idle;//set runstate to finished
  Finished = true;//set flag
};//Finish()

void SM::Restart(){
  Exec = &SM::FRun;//set runstate to entry
  Finished = false;//clear flag
}//restart

void SM::Idle(){
  return;//do abolutely nothing
};//Idle()

//
//
//
//
//
//
//
//
//
//
//
//
//
//
//

#include <LiquidCrystal.h>

#define fiveButton 2
#define tenButton 3
// TODO do this 4 instead for soda
#define sodaButton 12
#define beerButton A5
#define wineButton A4
#define resetButton A3

LiquidCrystal lcd(4, 5, 8, 9, 10, 11); // Parameters: (rs, enable, d4, d5, d6, d7)
//LiquidCrystal lcd(6, 7, 8, 9, 10, 11); // Parameters: (rs, enable, d4, d5, d6, d7)

volatile long money;
State Input();
SM Machine(Input);

State FiveCrowns() {
  money += 5;
  updateMoney();
  Machine.Set(Input);
}

State TenCrowns() {
  money += 10;
  updateMoney();
  Machine.Set(Input);
}

State Soda() {
  lcd.clear();
  if (money < 5){
    noMoney(5);
  } else {
    lcd.print("Giving Soda");
    loadingSequence();
    money -= 5;
    lcd.setCursor(0,1);
    lcd.clear();
    lcd.print("Enjoy your Soda");
    delay(1000);
  }
  Machine.Set(Input);
}

State Beer() {
  lcd.clear();
  if (money < 10){
    noMoney(10);
  } else {
    lcd.print("Giving Beer");
    loadingSequence();
    money -= 10;
    lcd.setCursor(0,1);
    lcd.clear();
    lcd.print("Enjoy your Beer");
    delay(1000);
  }
  Machine.Set(Input);
}

State Wine() {
  lcd.clear();
  if (money < 15){
    noMoney(15);
  } else {
    lcd.print("Giving Wine");
    loadingSequence();
    money -= 15;
    lcd.setCursor(0,1);
    lcd.clear();
    lcd.print("Enjoy your Beer");
    delay(1000);
  }
  Machine.Set(Input);
}

State Reset() {
  lcd.clear();
  lcd.print("Now returning");
  lcd.setCursor(0, 1);
  lcd.print(money);
  lcd.print(" SEK");
  loadingSequence();
  lcd.setCursor(1,12);
  lcd.print("DONE");
  delay(500);
  lcd.clear();
  lcd.print("Money returned");
  lcd.setCursor(0, 1);
  lcd.print("Have a nice day!");
  delay(1000);
  Machine.Set(Input);
}

State Input() {
  lcd.clear();
  lcd.print("Welcome!");
  updateMoney();
  if (digitalRead(fiveButton)) {
    Machine.Set(FiveCrowns);
  } else if (digitalRead(tenButton)) {
    Machine.Set(TenCrowns);
  } else if (digitalRead(sodaButton)) {
    Machine.Set(Soda);
  } else if (digitalRead(beerButton)) {
    Machine.Set(Beer);
  } else if (digitalRead(wineButton)) {
    Machine.Set(Wine);
  } else if (digitalRead(resetButton)) {
    Machine.Set(Reset);
  }
}

void updateMoney() {
  lcd.setCursor(0, 1);
  lcd.print("Money: ");
  lcd.print(money);
  lcd.print(" SEK");
  lcd.setCursor(0, 0);
}

void noMoney(int no) {
  lcd.print("No money.");
  lcd.setCursor(0,1);
  lcd.print("Have/Need");
  lcd.print(" ");
  lcd.print(money);
  lcd.print("/");
  lcd.print(no);
}

void loadingSequence(){
  lcd.setCursor(1, 15);
  lcd.print("%");
  for (int i = 0; i < 101; i++){
    lcd.setCursor(1, 13);
    lcd.print(i);
  }
}

void setup() {
  pinMode(6, OUTPUT); // Contrast pwm pin
  pinMode(buzzPin, OUTPUT); //TODO use buzzPin for objects too close, also make sound play.
  //TCCR2B = TCCR2B & 0b11111000 | 0b110;
  analogWrite(6, 100);

  money = 0;
  lcd.begin(16, 2);
  Serial.begin(9600);
}

void loop() {
  EXEC(Machine);
}