#include <Servo.h>

enum RFIDState {NoNewCard = 0, NoData, Match, NoMatch};

RFIDState rfidCheck()
{
  unsigned char fake = random(4);

  switch (fake) {
    case 0 :
      Serial.println("RFID check <NoNewCard>");
      return NoNewCard;

    case 1 :
      Serial.println("RFID check <NoData>");
      return NoData;

    case 2 :
      Serial.println("RFID check <Match>");
      return Match;

    case 3 :
      Serial.println("RFID check <NoMatch>");
      return NoMatch;
  }
}

Servo fingers;
Servo thumb;
Servo wrist;
int angle = 0;


typedef enum
{
  initialState = 0,
  wantCheckrfid = 1,
  want1 = 2,              //Fistclench,
  wantFistrelax = 3,
  want2 = 4,              //Thumbsupclench,
  wantThumbsuprelax = 5,
} states;

// state machine variables
states state = initialState;
unsigned long lastStateChange = 0;
unsigned long timeInThisState = 1000;

void setup() {

  Serial.begin(115200);
  Serial.println("with fake RFID. zeroing servos");

  fingers.attach(4);
  thumb.attach(3);
  wrist.attach(2);

  fingers.write(0);
  thumb.write(0);
  wrist.write(0);

  delay(777);

  //wrist2.attach(10)
  //pinktip.attach(9);
  //pinkybase.attach(8);
  //ringtip.attach(7);
  //ringbase.attach(6);
  //middletip.attach(5);
  //middlebase.attach(4);
  //indextip.attach(3);
  //indexbase.attach(2);

}

void doStateChange ()
{
  lastStateChange = millis ();
  timeInThisState = 1000;
  RFIDState rfidNow;

  Serial.print("           doStateChange state ");
  Serial.println(state);

  switch (state) {
  case initialState:
    state = wantCheckrfid;
    break;

  case wantCheckrfid:

    rfidNow = rfidCheck();  // this is the only connection to the RFID stuff

    if (rfidNow == NoNewCard) return;

    if (rfidNow == NoData) return;

    if (rfidNow == Match) {
      Serial.print("wantCheckrfid: fingers "); Serial.print(20);
      Serial.print(" thumb "); Serial.println(30);
      fingers.write(20);
      thumb.write(30);
      delay (500);
      Serial.print(" and fingers "); Serial.print(0);
      Serial.print(" thumb "); Serial.println(0);
      fingers.write(0);
      thumb.write(0);
    }

    // rfidNow is NoMatch or fell through from Match

    state = random(2) ? want1 : want2;
    timeInThisState = random (1000) + 2000;

    Serial.print(" state "); Serial.println(state);
    Serial.print(" for time "); Serial.println(timeInThisState);

    break;

  case want1:
    Serial.print("want1: fingers "); Serial.print(120);
    Serial.print(" thumb "); Serial.println(80);
    fingers.write(120);
    thumb.write(80);
    state = wantFistrelax;
    timeInThisState = 2000;
    break;

  case wantFistrelax:
    Serial.print("wantFistrelax: fingers "); Serial.print(0);
    Serial.print(" thumb "); Serial.println(0);
    fingers.write(0);
    thumb.write(0);
    state = wantCheckrfid;
    timeInThisState = 3000;
    break;

  case want2:
    Serial.print("want2: fingers "); Serial.print(120);
    Serial.print(" wrist "); Serial.println(90);
    fingers.write(120);
    wrist.write(90);
    state = wantThumbsuprelax;
    timeInThisState = 2000;
    break;

  case wantThumbsuprelax:
    Serial.print("wantThumbsuprelax: fingers "); Serial.print(0);
    Serial.print(" wrist "); Serial.println(0);
    fingers.write(0);
    wrist.write(0);
    state = wantCheckrfid;
    timeInThisState = 3000;
    break;
  }
}

void loop() {
  if (millis () - lastStateChange >= timeInThisState)
    doStateChange ();
}