/*
  https://forum.arduino.cc/t/freertos-hand-issue/1323658
*/


#define BTN_PIN 11
#define LED_PIN 13 //was 0
#define LO_SW 3 // was 1
#define PASS_SW 2
#define HI_SW 4
#define OUT_1 9
#define OUT_2 10

#define LO_BIT 2
#define HI_BIT 1
#define PASS_BIT 0


byte lastBtn = 0;
unsigned long lastBtnRead;
unsigned long btnDebounce = 100;
byte itr = 0;

byte clState = 0;
byte step = 0;
unsigned long lastPass;
unsigned long passInterval = 10;
unsigned long lastStep;
unsigned long stepInterval = 150;


void setup() {

  Serial.begin(115200);
  Serial.println("ready..");

  pinMode(LO_SW, INPUT); //low switch
  pinMode(PASS_SW, INPUT); //pass switch
  pinMode(HI_SW, INPUT); //high switch

  pinMode(OUT_1, OUTPUT);
  pinMode(OUT_2, OUTPUT);


  pinMode(BTN_PIN, INPUT);
  pinMode(LED_PIN,  OUTPUT);

}

void loop() {


  unsigned long now = millis();

  if (now - lastBtnRead >= btnDebounce) {
    byte b = digitalRead(BTN_PIN);
    if (b != lastBtn) {
      lastBtnRead = now;
      lastBtn = b;
      if (b == HIGH) {
        if (itr >= 2)
        {
          digitalWrite(LED_PIN, HIGH);
          itr = 0;
        }
        else
        {
          digitalWrite(LED_PIN, LOW);
          itr++;
        }
      }
    }
  }

  if (now - lastPass >= passInterval) {
    lastPass = now;
    switch (clState) {
      case 0: if (digitalRead(LO_SW)) bitSet(clState, LO_BIT);
        if (digitalRead(HI_SW)) bitSet(clState, HI_BIT);
        if (digitalRead(PASS_SW)) bitSet(clState, PASS_BIT);
        step = 0;
        break;
      case 1: switch (step) {
          case 0: digitalWrite(OUT_2, HIGH);
            lastStep = now;
            step++;
            break;
          case 1: if (now - lastStep >= stepInterval) {
              digitalWrite(OUT_2, LOW);
              lastStep = now;
              step++;
            }
            break;
          case 2: if (now - lastStep >= stepInterval) {
              digitalWrite(OUT_1, LOW);
              clState = 0;
              step = 0;
            }
            break;
        }
        break;
      case 2: digitalWrite(OUT_2, HIGH);
        digitalWrite(OUT_1, LOW);
        clState = 0;
        break;
      case 3: switch (step) {
          case 0: digitalWrite(OUT_2, LOW);
            digitalWrite(OUT_1, HIGH);
            lastStep = now;
            step++;
            break;
          case 1: if (now - lastStep >= stepInterval) {
              digitalWrite(OUT_1, LOW);
              digitalWrite(OUT_2, HIGH);
              lastStep = now;
              step++;
            }
            break;
          case 2: if (now - lastStep >= stepInterval) {
              clState = 0;
              step = 0;
            }
            break;
        }
        break;
      case 4: digitalWrite(OUT_2, LOW);
        digitalWrite(OUT_1, HIGH);
        clState = 0;
        break;
      case 5: switch (step) {
          case 0: digitalWrite(OUT_1, LOW);
            digitalWrite(OUT_2, HIGH);
            lastStep = now;
            step++;
            break;
          case 1: if (now - lastStep >= stepInterval) {
              digitalWrite(OUT_2, LOW);
              digitalWrite(OUT_1, HIGH);
              lastStep = now;
              step++;
            }
            break;
          case 2: if (now - lastStep >= stepInterval) {
              clState = 0;
              step = 0;
            }
            break;
        }
        break;
      case 6: digitalWrite(OUT_2, HIGH);
        digitalWrite(OUT_1, HIGH);
        clState = 0;
        break;
      case 7: digitalWrite(OUT_2, HIGH);//same as 6
        digitalWrite(OUT_1, HIGH);
        clState = 0;
        break;
      default: digitalWrite(OUT_2, LOW);
        digitalWrite(OUT_1, LOW);
        clState = 0;
        break;
    }
  }
}