// RGB and R, G, B LED demo
const uint8_t Relay = 4;
const uint8_t pinB = 5;
const uint8_t pinG = 6;
const uint8_t pinR = 7;
const uint8_t PushButton = 3;
const uint8_t InterlockSw = 2;
const uint16_t timeout = 1200; //1.2s
uint32_t oldTime, run_oldTime;
uint16_t runTimes[4] = {0,2000,4000,6000};
uint8_t i = 0, currentState, preState;

void setRGBled(uint8_t r, uint8_t g, uint8_t b) {
  digitalWrite(pinR, r);
  digitalWrite(pinG, g);
  digitalWrite(pinB, b);
}

void resetAll(){
  digitalWrite(Relay, LOW);
  setRGBled(LOW, LOW, LOW); //OFF
  oldTime = millis();
  i = 0;
}

void setup() {
  pinMode(pinR, OUTPUT);
  pinMode(pinG, OUTPUT);
  pinMode(pinB, OUTPUT);
  pinMode(Relay, OUTPUT);
  pinMode(PushButton, INPUT_PULLUP);
  pinMode(InterlockSw, INPUT);
  currentState = digitalRead(PushButton);
  preState = currentState;
}

void loop() {
  currentState = digitalRead(PushButton);
  delay(20);
  if (digitalRead(PushButton) != currentState){
    currentState = preState;
    //oldTime = millis();
  } 
  else {
    if (digitalRead(InterlockSw)) {
      if (currentState == LOW && preState == HIGH) {
        if (millis() - oldTime < timeout) {
          i = (i + 1) % 4;

          if (i == 0 || digitalRead(Relay)==HIGH) resetAll();
          else if (i == 1) setRGBled(LOW, HIGH, LOW); //Green
          else if (i == 2) setRGBled(LOW, LOW, HIGH); //Blue
          else if (i == 3) setRGBled(HIGH, HIGH, LOW); //Yellow

          preState = LOW;
          oldTime = millis();
        }
      }
      else if (millis() - oldTime > timeout) {
        if (digitalRead(PushButton)==LOW){ //manual (pulse) mode
          setRGBled(HIGH, HIGH, HIGH); //White
          digitalWrite(Relay, HIGH);
          while (digitalRead(PushButton)==LOW) delay(20); 
          resetAll();      
        }
        else if (digitalRead(Relay)==LOW && i>0) { //auto mode
          setRGBled(HIGH, LOW, LOW); //Red
          digitalWrite(Relay, HIGH);
          run_oldTime = millis();
        }
        else if (millis() - run_oldTime > runTimes[i]){
          resetAll();
        }
      }
    }
    preState = currentState;
  }
}