#define Button_Pin 2
#define SafeSwitch_Pin 3

#define LedR_Pin 13
#define LedG_Pin 12
#define LedB_Pin 11

#define Relay_Pin 4

bool InCycle = false;
unsigned long CycleDuration = 6000UL;
unsigned long CurrentCycleT = 0;

int LedR_State = LOW;
int LedG_State = LOW;
int LedB_State = LOW;

unsigned long BlueLedBlinkInterval = 300UL;
unsigned long BlueLedBlinkPreviousMillis = 0;

unsigned long RedLedBlinkInterval = 200UL;
unsigned long RedLedBlinkPreviousMillis = 0;

int CrateState = 0;
// Crate States (LED COLOR, MEANING)
// 0 = RED = Door open
// 1 = GREEN = Ready 
// 2 = BLUE = In Cycle
// 3 = BLINKING BLUE = Cycle Over
// 4 = BLINKING RED = Cycle Broken

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(Button_Pin, INPUT_PULLUP);
  pinMode(SafeSwitch_Pin, INPUT_PULLUP);
  pinMode(LedR_Pin, OUTPUT);
  pinMode(LedG_Pin, OUTPUT);
  pinMode(LedB_Pin, OUTPUT);
  pinMode(Relay_Pin, OUTPUT);
  digitalWrite(Relay_Pin, LOW);
}

void loop(){

  // DOOR SAFETY CHECK
  if (isDoorClosed()) {
    if (CrateState == 0) {
      CrateState = 1;
    }
  }
  else {
    if (CrateState == 2) {
      breakCycle();
    }
    else if (CrateState != 4) {
      CrateState = 0;
    }
  }

  // HANDLE CYCLES
  if ((digitalRead(Button_Pin) == LOW) && CrateState == 1){
    initCycle();
  }
  if (CrateState == 2){
    if (millis() > CurrentCycleT + CycleDuration) {
      endCycle();
    }
  }

  // CONTROL LED
  ledControl(CrateState);
}

bool isDoorClosed(){
  if(digitalRead(SafeSwitch_Pin) == LOW) {
    return true;
  }
  else {
    return false;
  }

}

void initCycle() {
  CrateState = 2;
  CurrentCycleT = millis();
  digitalWrite(Relay_Pin, HIGH);
}

void endCycle() {
  CrateState = 3;
  CurrentCycleT = 0;
  digitalWrite(Relay_Pin, LOW);
}

void breakCycle() {
  CrateState = 4;
  CurrentCycleT = 0;
  digitalWrite(Relay_Pin, LOW);
}

void ledControl(int state){
  switch (state) {
    case 0: // RED
      LedR_State = HIGH;
      LedG_State = LOW;
      LedB_State = LOW;
      break;
    case 1: // GREEN
      LedR_State = LOW;
      LedG_State = HIGH;
      LedB_State = LOW;
      break;
    case 2: // BLUE
      LedR_State = LOW;
      LedG_State = LOW;
      LedB_State = HIGH;
      break;
    case 3: // BLUE BLINK
      LedR_State = LOW;
      LedG_State = LOW;
      if (millis() - BlueLedBlinkPreviousMillis >= BlueLedBlinkInterval) {
        BlueLedBlinkPreviousMillis += BlueLedBlinkInterval;
        if (LedB_State == HIGH) {
          LedB_State = LOW;
        }
        else {
          LedB_State = HIGH;
        }
      }
      break;
    case 4: // RED BLINK
      LedB_State = LOW;
      LedG_State = LOW;
      if (millis() - RedLedBlinkPreviousMillis >= RedLedBlinkInterval) {
        RedLedBlinkPreviousMillis += RedLedBlinkInterval;
        if (LedR_State == HIGH) {
          LedR_State = LOW;
        }
        else {
          LedR_State = HIGH;
        }
      }
      break;
  }
  digitalWrite(LedR_Pin, LedR_State);
  digitalWrite(LedG_Pin, LedG_State);
  digitalWrite(LedB_Pin, LedB_State);
}