#define Power_Led D1
#define EPS_Led D2
#define Solenoid D3
#define Buzzer D4
#define DIGITAL_PIN5_NUM D5
#define Pressure_Low A7
#define Pressure_High A6
#define PressureSensor D10
#define Solenoid_Ok D5
#define initialOnDuration 1000UL

static bool digitalPin5State=false;
static bool hasSensorInput5BeenLow = false;
static int initialSequenceStage=0;
static int initialSequenceStartTime=0;
static int startMillis = 0;
void setup() {
  pinMode(Power_Led,OUTPUT);
  pinMode(EPS_Led,OUTPUT);
  pinMode(Solenoid,OUTPUT);
  pinMode(Buzzer,OUTPUT);
  pinMode(Pressure_Low,INPUT_PULLDOWN);
  pinMode(Pressure_High,INPUT_PULLDOWN);
  pinMode(PressureSensor,INPUT_PULLDOWN);
  pinMode(Solenoid_Ok,INPUT_PULLDOWN);
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, STM32!");
}

void loop() {
  // put your main code here, to run repeatedly:
  unsigned long currentMillis = millis();
  handleInitialSequence(currentMillis);
   // this speeds up the simulation
}
void handleInitialSequence(unsigned long currentMillis) {
	switch (initialSequenceStage) {
	case 0:
		// Pressure sensor => HIGH
		digitalWrite(PressureSensor,HIGH);
    digitalWrite(Power_Led,HIGH);
#ifdef DEBUG
		UART_SendString(&huart2, "Stage 0: Pressure sensor output = HIGH\r\n");
#endif
		if ((currentMillis - initialSequenceStartTime) < initialOnDuration) {
			// EPS LED => HIGH
			digitalWrite(EPS_Led, HIGH);
		} else {
			// EPS LED => LOW
			digitalWrite(EPS_Led, LOW);
			initialSequenceStage = 1;
			initialSequenceStartTime = currentMillis;
#ifdef DEBUG
			UART_SendString(&huart2, "Moving to Stage 1\r\n");
#endif
		}
		break;

	case 1:
		if ((currentMillis - initialSequenceStartTime) < initialOnDuration) {
			// Buzzer => HIGH
			digitalWrite(Buzzer, HIGH);
		} else {
			// Buzzer => LOW
			digitalWrite(Buzzer,LOW);
			initialSequenceStage = 2;
			initialSequenceStartTime = currentMillis;
#ifdef DEBUG
			UART_SendString(&huart2, "Moving to Stage 2\r\n");
#endif
		}
		break;

	case 2:
		// Solenoid LED => HIGH
		digitalWrite(Solenoid, HIGH);

		// Check digitalPin5
		handleDigitalPin5Check();

		// Solenoid LED => LOW
		digitalWrite(Solenoid, LOW);

		initialSequenceStage = 3; // done
#ifdef DEBUG
		UART_SendString(&huart2, "Stage 2 done, moving to Stage 3\r\n");
#endif
		break;

	default:
		// Stage >= 3 => do nothing
		break;
	}
}

void handleDigitalPin5Check(void) {
	unsigned long startMillis = millis();
	while ((millis() - startMillis) < 1000UL) {
		digitalPin5State = digitalRead(DIGITAL_PIN5_NUM);
		if (!digitalPin5State) // LOW
		{
			hasSensorInput5BeenLow = true;
			break;
		}
	}
}