// see https://forum.arduino.cc/t/state-change-random-feature/1023151


// remove / simulate all RFID stuff

#include <Servo.h>
//#include <MFRC522.h>
//#include <Wire.h>
//#include <SPI.h>

//MFRC522 mfrc522(10, 9); // MFRC522 mfrc522(SS_PIN, RST_PIN)
Servo fingers;
Servo thumb;
Servo wrist;
int angle = 0;
String tagUID = "9A 71 1C B0"; // String to store UID of tag. Change it with your tag's UID

unsigned char validCardSeen = 0;	// we haven't. seen a valid card.

typedef enum
{
  initialState,
  wantCheckrfid,
  want1,              //Fistclench,
  wantFistrelax,
  want2,              //Thumbsupclench,
  wantThumbsuprelax,
} states;

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

void setup() {

  Serial.begin(115200);
  Serial.println("stripped of everything! 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);
  //  SPI.begin(); // Init SPI bus
  //  mfrc522.PCD_Init(); // Init MFRC522
}

void doStateChange ()
{
  lastStateChange = millis ();
  timeInThisState = 1000;
  String tag = "";

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

    case wantCheckrfid:
      if (validCardSeen) {
        Serial.print(" rfid ");
        fingers.write(20);
        thumb.write(30);
        delay (400);
        fingers.write(0);
        thumb.write(0);
        Serial.print("endrfid ");

        state = random(2) ? want1 : want2;
        Serial.print(" random selection ");
        Serial.print(state);
        Serial.print(" ");

        timeInThisState = random (1000, 5000);

        validCardSeen = 0; // we acted on this card, so don't again.
      }
      else		// remove this clause when you are tired of the spam on the serial monitor
        Serial.println("waiting on seeing a valid card!");

      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;
      Serial.print(" end want1 ");
      break;

    case wantFistrelax:
      fingers.write(0);
      thumb.write(0);
      state = wantCheckrfid;
      timeInThisState = 3000;
      Serial.print("end wantfistrelax ");
      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;
      Serial.print(" end want2 ");
      break;

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

void loop() {
  String tag = "";

  delay(50);        // 20 times a second is fine for non-blocking or purposely blocking code

  if (!validCardSeen) { // only check RFID if we aren't waiting to act on a valid card

    if (random(1000) > 800) {  // new card 8/10 times
      Serial.println("no new card");
      return;
    }
    else Serial.print(" NEW CARD ");

    if (random(1000) > 950) {    // usually, able to read from the card
      Serial.println("can't / didn't read the card");
      return;
    }
    else Serial.print(" CAN BE READ AND ");

    //Read from the card - simulate just 9/10 chance card is valid

    if (random(1000) > 100) //Checking the card
    {
      Serial.println("IS A VALID CARD.");
      validCardSeen = 1;            // did. we saw it. inform the machine.
    }
    else {
      Serial.println(" is not valid ");
    }
  }

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