#include <Arduino.h>

#define CENTER_PIN 0        // GPIO0 (Interrupt Input)
#define MOTOR_LEFT_PIN 12   // GPIO12
#define MOTOR_RIGHT_PIN 13  // GPIO13
#define TO_CENTER_PIN 14    // GPIO14
#define OUT_1_PIN 15         // GPIO1 (Output)
#define OUT_2_PIN 32         // GPIO3 (Output)

volatile bool pass_left = 0;
volatile bool pass_right = 0;
volatile bool center_triggered = false;

void IRAM_ATTR centerISR() {
    center_triggered = true;
}

void setup() {
    pinMode(CENTER_PIN, INPUT_PULLUP);
    pinMode(MOTOR_LEFT_PIN, INPUT_PULLUP);
    pinMode(MOTOR_RIGHT_PIN, INPUT_PULLUP);
    pinMode(TO_CENTER_PIN, INPUT_PULLUP);
    pinMode(OUT_1_PIN, OUTPUT);
    pinMode(OUT_2_PIN, OUTPUT);
    
    digitalWrite(OUT_1_PIN, LOW);
    digitalWrite(OUT_2_PIN, LOW);
    
    attachInterrupt(digitalPinToInterrupt(CENTER_PIN), centerISR, FALLING);
}

void loop() {
    // ตรวจสอบ MOTOR_LEFT_PIN
    if (digitalRead(MOTOR_LEFT_PIN) == LOW) {
        digitalWrite(OUT_1_PIN, HIGH);
        pass_left = 1;
    } else {
        digitalWrite(OUT_1_PIN, LOW);
       
    }

    // ตรวจสอบ MOTOR_RIGHT_PIN
    if (digitalRead(MOTOR_RIGHT_PIN) == LOW) {
        digitalWrite(OUT_2_PIN, HIGH);
        pass_right = 1;
    } else {
        digitalWrite(OUT_2_PIN, LOW);
        
    }

    // ตรวจสอบ TO_CENTER_PIN
    if (digitalRead(TO_CENTER_PIN) == LOW) {
        if (pass_left == 1) {
            digitalWrite(OUT_2_PIN, HIGH);
            while (!center_triggered) {
                // รอให้ CENTER_PIN ถูกกด
            }
            pass_left = 0;
            center_triggered = false;
        }
        if (pass_right == 1) {
            digitalWrite(OUT_1_PIN, HIGH);
            while (!center_triggered) {
                // รอให้ CENTER_PIN ถูกกด
            }
            pass_right = 0;
            center_triggered = false;
        }
    }
}