//V4_FULL_FITUR - Without MARK Relay
#define PIN_RETAIN 2      // Input untuk switch retain
#define PIN_WP_OK 3       // Input untuk sensor WP OK
#define PIN_POINT_WP 4    // Output untuk point CHECK WP
#define PIN_RELAY_SOL 5   // Output untuk relay solenoid soket
#define PIN_RELAY_VIB 6   // Output untuk relay vibrator 
#define PIN_INDIKATOR 8   // Output untuk indikator LED blink
#define PIN_KEY1 9        // Output untuk key scan 1
#define PIN_KEY2 10       // Input untuk key scan 2
#define PIN_KEY3 11       // Input untuk key scan 3
#define PIN_KEY4 12       // Output untuk key scan 4
#define PIN_RELAY_NG 13   // Output untuk LED NG (rotary)

const unsigned long vibrateInterval = 4000;
const unsigned long retainDelay = 1500;
const unsigned long solToVibDelay = 500;
const unsigned long wpOkToVibDelay = 200;
const unsigned long systemResetDelay = 500;
const unsigned long blinkInterval = 400;
const unsigned long vibToSolOffDelay = 3000;
const unsigned long delayResetSemula = 200; 
const unsigned long deteksiNG = 300; 

unsigned long previousMillis = 0;
unsigned long retainActivatedTime = 0;
unsigned long vibrationEndTime = 0;
unsigned long systemResetStartTime = 0;
unsigned long wpOkLowTime = 0;
unsigned long vibOffTime = 0;
unsigned long blinkPreviousMillis = 0;
unsigned long finalResetStartTime = 0; 
unsigned long wpOkHighStartTime = 0; 

int currentWpOkState = HIGH;
int lastWpOkState = HIGH;
int currentRetainState = HIGH;
int lastRetainState = HIGH;

bool retainPressed = false;
bool wpOkActivated = false;
bool solActivated = false;
bool isVibrating = false;
bool vibrateActivated = false;
bool vibrationCompleted = false;
bool systemLocked = false;
bool cycleCompleted = false;
bool vibOffDone = false;
bool systemResetActive = false;
bool shouldBlink = false;
bool indicatorState = false;
bool wpOkLowDetected = false;
bool waitingSolOff = false;
bool solDeactivatedAfterVibration = false;
bool systemNeedsReset = false;
bool resetConditionMet = false;
bool ngCondition = false;
bool systemUsedOnce = false;
bool wpOkWasActivated = false;
bool keyLockedSystem = false; 
bool finalResetActive = false; 
bool readyForManualReset = false; 
bool wpOkHighDetected = false; 
bool keyResetPending = false; 
bool lastKeyState = false; 

void setup() {
  pinMode(PIN_RETAIN, INPUT_PULLUP);
  pinMode(PIN_WP_OK, INPUT_PULLUP);
  pinMode(PIN_KEY1, OUTPUT);
  pinMode(PIN_KEY2, INPUT_PULLUP);
  pinMode(PIN_KEY3, INPUT_PULLUP);
  pinMode(PIN_KEY4, OUTPUT);

  pinMode(PIN_RELAY_SOL, OUTPUT);
  pinMode(PIN_POINT_WP, OUTPUT);
  pinMode(PIN_RELAY_VIB, OUTPUT);
  pinMode(PIN_INDIKATOR, OUTPUT);
  pinMode(PIN_RELAY_NG, OUTPUT);

  digitalWrite(PIN_RELAY_SOL, HIGH);
  digitalWrite(PIN_POINT_WP, HIGH);
  digitalWrite(PIN_RELAY_VIB, HIGH);
  digitalWrite(PIN_INDIKATOR, LOW);
  digitalWrite(PIN_RELAY_NG, HIGH); 

  digitalWrite(PIN_KEY1, HIGH);
  digitalWrite(PIN_KEY4, LOW);
}

void resetSystem() {
  digitalWrite(PIN_RELAY_VIB, HIGH);
  digitalWrite(PIN_INDIKATOR, LOW);

  if (checkKeyStatus() && (currentRetainState == HIGH || ngCondition)) {
    digitalWrite(PIN_RELAY_SOL, HIGH);
    digitalWrite(PIN_RELAY_NG, HIGH); 
    digitalWrite(PIN_POINT_WP, HIGH); 

    ngCondition = false;
    systemLocked = false;
    keyLockedSystem = false; 
    systemUsedOnce = false;
    wpOkWasActivated = false;
    vibrateActivated = false;
    isVibrating = false;
    solActivated = false;
    retainPressed = false;
    vibrationEndTime = 0;
    vibrationCompleted = false;
    systemResetActive = false;
    cycleCompleted = false;
    wpOkActivated = false;
    vibOffDone = false;
    wpOkLowDetected = false;
    waitingSolOff = false;
    solDeactivatedAfterVibration = false;
    systemNeedsReset = false;
    resetConditionMet = false;
    finalResetActive = false;
    readyForManualReset = false;
    wpOkHighDetected = false; 
    keyResetPending = false; 
  
    previousMillis = 0;
    retainActivatedTime = 0;
    vibrationEndTime = 0;
    systemResetStartTime = 0;
    wpOkLowTime = 0;
    vibOffTime = 0;
    blinkPreviousMillis = 0;
    finalResetStartTime = 0;
    wpOkHighStartTime = 0;
  }
}

void trackPinStates() {
  currentWpOkState = digitalRead(PIN_WP_OK);
  currentRetainState = digitalRead(PIN_RETAIN);
}

void handleKeyResetLogic() {
  bool currentKeyState = checkKeyStatus();

  if (lastKeyState == true && currentKeyState == false) {
    if (currentRetainState == LOW) {
      keyResetPending = true;
    } else {
      resetSystem();
    }
  }
  
  if (keyResetPending && currentRetainState == HIGH) {
    resetSystem();
    keyResetPending = false;
  }
  
  lastKeyState = currentKeyState;
}

void handleEdgeDetection() {
  unsigned long currentMillis = millis();
  
  if (lastWpOkState == HIGH && currentWpOkState == LOW) {
    if (!systemUsedOnce && !ngCondition) {
      wpOkActivated = true;
      wpOkWasActivated = true;
      systemUsedOnce = true;
    }
    wpOkHighDetected = false;
    wpOkHighStartTime = 0;
  }
  
  if (lastWpOkState == LOW && currentWpOkState == HIGH && wpOkWasActivated && !cycleCompleted) {
    if (!wpOkHighDetected) {
      wpOkHighDetected = true;
      wpOkHighStartTime = currentMillis;
    }
  }
  
  if (wpOkHighDetected && currentWpOkState == HIGH && wpOkWasActivated && !cycleCompleted && !ngCondition) {
    if (currentMillis - wpOkHighStartTime >= deteksiNG) {
      ngCondition = true;
      systemLocked = true;
      digitalWrite(PIN_RELAY_NG, LOW);
      digitalWrite(PIN_RELAY_VIB, HIGH);
      digitalWrite(PIN_POINT_WP, HIGH);
      wpOkHighDetected = false;
    }
  }
  
  if (wpOkHighDetected && currentWpOkState == LOW) {
    wpOkHighDetected = false;
    wpOkHighStartTime = 0;
  }
  
  if (systemNeedsReset && !resetConditionMet) {
    if (currentRetainState == HIGH && currentWpOkState == HIGH) {
      resetConditionMet = true;
    }
  }
  
  if (currentRetainState == HIGH && currentWpOkState == HIGH && readyForManualReset && !ngCondition) {
    resetSystemToInitial();
  }
  
  lastRetainState = currentRetainState;
  lastWpOkState = currentWpOkState;
}

void resetSystemToInitial() {
  ngCondition = false;
  systemLocked = false;
  keyLockedSystem = false;
  systemUsedOnce = false;
  wpOkWasActivated = false;
  vibrateActivated = false;
  isVibrating = false;
  solActivated = false;
  retainPressed = false;
  vibrationEndTime = 0;
  vibrationCompleted = false;
  systemResetActive = false;
  cycleCompleted = false;
  wpOkActivated = false;
  vibOffDone = false;
  wpOkLowDetected = false;
  waitingSolOff = false;
  solDeactivatedAfterVibration = false;
  systemNeedsReset = false;
  resetConditionMet = false;
  finalResetActive = false;
  readyForManualReset = false;
  wpOkHighDetected = false; 
  keyResetPending = false;
  
  previousMillis = 0;
  retainActivatedTime = 0;
  vibrationEndTime = 0;
  systemResetStartTime = 0;
  wpOkLowTime = 0;
  vibOffTime = 0;
  blinkPreviousMillis = 0;
  finalResetStartTime = 0;
  wpOkHighStartTime = 0; 
  
  digitalWrite(PIN_RELAY_SOL, HIGH);
  digitalWrite(PIN_RELAY_VIB, HIGH);
  digitalWrite(PIN_INDIKATOR, LOW);
  digitalWrite(PIN_RELAY_NG, HIGH);
  digitalWrite(PIN_POINT_WP, HIGH);
}

bool checkKeyStatus() {
  digitalWrite(PIN_KEY1, LOW);
  delayMicroseconds(100);
  bool key1_connected = (digitalRead(PIN_KEY2) == LOW);
  digitalWrite(PIN_KEY1, HIGH);
  delayMicroseconds(100);
  bool key3_connected = (digitalRead(PIN_KEY3) == LOW);
  return (key1_connected && key3_connected);
}

void handleIndicatorBlink() {
  if (currentRetainState == HIGH && currentWpOkState == HIGH && !ngCondition && (!cycleCompleted || keyLockedSystem)) {
    shouldBlink = true;
  } else {
    shouldBlink = false;
  }
  
  if (shouldBlink) {
    unsigned long currentMillis = millis();
    if (currentMillis - blinkPreviousMillis >= blinkInterval) {
      indicatorState = !indicatorState;
      digitalWrite(PIN_INDIKATOR, indicatorState ? HIGH : LOW);
      blinkPreviousMillis = currentMillis;
    }
  } else {
    digitalWrite(PIN_INDIKATOR, LOW);
    indicatorState = false;
  }
}

void handleSolRelay() {
  unsigned long currentMillis = millis();
  if (ngCondition) {
    if (checkKeyStatus()) {
      digitalWrite(PIN_RELAY_SOL, HIGH);
      resetSystem();
    }
    return;
  }

  if (solDeactivatedAfterVibration) {
    digitalWrite(PIN_RELAY_SOL, HIGH);
    return;
  }
  
  if (systemNeedsReset && !resetConditionMet) {
    digitalWrite(PIN_RELAY_SOL, HIGH);
    retainPressed = false;
    solActivated = false;
    return;
  }
  
  if (systemUsedOnce && !wpOkActivated) {
    digitalWrite(PIN_RELAY_SOL, HIGH);
    retainPressed = false;
    solActivated = false;
    return;
  }
  
  if (currentRetainState == LOW && !solDeactivatedAfterVibration && !ngCondition) {
    if (!retainPressed) {
      retainPressed = true;
      retainActivatedTime = currentMillis;
    }
    if (currentMillis - retainActivatedTime >= retainDelay) {
      digitalWrite(PIN_RELAY_SOL, LOW);
      solActivated = true;
    }
  } else if (!solDeactivatedAfterVibration) {
    digitalWrite(PIN_RELAY_SOL, HIGH);
    retainPressed = false;
    solActivated = false;
  }
}

void handleSystemReset() {
  unsigned long currentMillis = millis();
  
  if (waitingSolOff && !cycleCompleted) {
    if (currentMillis - vibOffTime >= vibToSolOffDelay) {
      digitalWrite(PIN_RELAY_VIB, HIGH);
      digitalWrite(PIN_RELAY_SOL, HIGH);
      solDeactivatedAfterVibration = true;
      systemNeedsReset = true;
      vibOffDone = true;
      
      waitingSolOff = false;
      cycleCompleted = true;
      systemResetActive = true;
      systemResetStartTime = currentMillis;
    }
    return;
  }
  
  if (systemResetActive && cycleCompleted && !finalResetActive && (currentMillis - systemResetStartTime >= systemResetDelay)) {
    digitalWrite(PIN_RELAY_VIB, HIGH);
    systemLocked = false;
    vibrateActivated = false;
    isVibrating = false;
    retainPressed = false;
    vibrationEndTime = 0;
    vibrationCompleted = false;
    systemResetActive = false;
    wpOkActivated = false;
    vibOffDone = false;
    wpOkLowDetected = false;
    waitingSolOff = false;
    finalResetActive = true;
    finalResetStartTime = currentMillis;
  }
  
  if (finalResetActive && (currentMillis - finalResetStartTime >= delayResetSemula)) {
    finalResetActive = false;
    readyForManualReset = true; 
  }
}

void handleVibration() {
  unsigned long currentMillis = millis();
  
  if (ngCondition) {
    digitalWrite(PIN_RELAY_VIB, HIGH);
    isVibrating = false;
    return;
  }
  
  if (currentRetainState == HIGH && isVibrating) {
    digitalWrite(PIN_RELAY_VIB, HIGH);
    vibrationEndTime = currentMillis;
    vibrationCompleted = false;
    isVibrating = false;
    vibrateActivated = false;
    vibrationEndTime = 0;
    return;
  }
  
  if (systemNeedsReset && !resetConditionMet && isVibrating) {
    digitalWrite(PIN_RELAY_VIB, HIGH);
    vibrationEndTime = currentMillis;
    vibrationCompleted = false;
    isVibrating = false;
    vibrateActivated = false;
    vibrationEndTime = 0;
    return;
  }
  
  if (solActivated && !vibrationCompleted && wpOkActivated && !vibOffDone && !systemLocked && !cycleCompleted && currentRetainState == LOW) {
    if (currentWpOkState == LOW) {
      if (!wpOkLowDetected) {
        wpOkLowTime = currentMillis;
        wpOkLowDetected = true;
      }
      if (currentMillis - wpOkLowTime >= wpOkToVibDelay) {
        if (!isVibrating) {
          digitalWrite(PIN_RELAY_VIB, LOW);
          previousMillis = currentMillis;
          isVibrating = true;
          vibrateActivated = true;
        }
      }
    } else {
      wpOkLowDetected = false;
      if (isVibrating) {
        digitalWrite(PIN_RELAY_VIB, HIGH);
        isVibrating = false;
        vibrateActivated = false;
      }
    }
  }
  
  if (isVibrating) {
    if (currentWpOkState == HIGH) {
      digitalWrite(PIN_RELAY_VIB, HIGH);
      vibrationEndTime = currentMillis;
      vibrationCompleted = true;
      isVibrating = false;
      vibrateActivated = false;
      vibOffTime = currentMillis;
      waitingSolOff = true;
    } else if (currentMillis - previousMillis >= vibrateInterval) {
      digitalWrite(PIN_RELAY_VIB, HIGH);
      vibrationEndTime = currentMillis;
      vibrationCompleted = true;
      isVibrating = false;
      vibOffTime = currentMillis;
      waitingSolOff = true;
    }
  }
}

void loop() {
  trackPinStates();
  handleKeyResetLogic(); 
  handleEdgeDetection();
  handleIndicatorBlink();
  
  if (checkKeyStatus()) {
    keyLockedSystem = true; 
    
    if (!ngCondition) {
      digitalWrite(PIN_RELAY_SOL, HIGH);
    }
    digitalWrite(PIN_RELAY_VIB, HIGH);
    digitalWrite(PIN_INDIKATOR, LOW);
    
    if (ngCondition) {
      resetSystem();
      return;
    }
    
    systemLocked = true;
    isVibrating = false;
    vibrateActivated = false;
    solActivated = false;
    systemResetActive = false;
    cycleCompleted = false;
    retainPressed = false;
    wpOkActivated = false;
    vibOffDone = false;
    wpOkLowDetected = false;
    waitingSolOff = false;
    solDeactivatedAfterVibration = false;
    systemNeedsReset = false;
    resetConditionMet = false;
    finalResetActive = false;
    readyForManualReset = false;
    wpOkHighDetected = false;
    return;
  } else {
    if (keyLockedSystem) {
      keyLockedSystem = false;
      systemLocked = false; 
    }
  }
  
  handleSolRelay();
  handleVibration();
  handleSystemReset();

  if (checkKeyStatus()) {
    digitalWrite(PIN_POINT_WP, HIGH);
  } else if (ngCondition) {
    digitalWrite(PIN_POINT_WP, HIGH);
  } else if (currentRetainState == LOW) {
    digitalWrite(PIN_POINT_WP, LOW);
  } else {
    digitalWrite(PIN_POINT_WP, HIGH);
  }
}