// skreetsh.ino

// Підключення пінів згідно diagrams.json:
// H-Напрямок (Авто)
const int LED_H_GREEN  = PA6; // D10
const int LED_H_YELLOW = PA7; // D11
const int LED_H_RED    = PA0; // D12

// P-Напрямок (Пішохід) - Працюють синхронно
const int LED_P_GREEN  = PA1; // D1 (Ped1 Green)
const int LED_P_RED    = PA8; // D8 (Ped1 Red)
const int LED_P2_GREEN = PA4; // D4 (Ped2 Green)
const int LED_P2_RED   = PA5; // D5 (Ped2 Red)

// Кнопки пішохідного виклику
const int BUTTON_PIN_1 = PA9;  // D9
const int BUTTON_PIN_2 = PA10; // D7

// --- Визначення Станів FSM (Кінцевий Автомат) ---
enum State {
    S1_H_GREEN,   // H-G (50c)
    S2_H_YELLOW,  // H-Y (2c)
    S3_PAUSE_1,   // All-R (2c) - Точка перемикання K
    S4_V_GREEN,   // V-G (50c) - В коді буде H_RED/P_RED
    S5_V_YELLOW,  // V-Y (2c)
    S8_PAUSE_2,   // All-R (3c) - Фінальна пауза

    S6_P_GREEN,   // P-G (25c) - Пішохідний цикл
    S7_P_FLASH,   // P-Мигає (2c)
};

State currentState = S1_H_GREEN;
unsigned long stateStartTime;
bool pedestrianRequest = false; // Флаг натискання кнопки
bool lastButtonState = HIGH;    // Для дебаунсингу кнопок (для кнопки)

// --- Таймаути (в мілісекундах) ---
// Тимчасово зменшені для швидкого тестування:
const unsigned long T_FACTOR = 100; // Множник для швидкого тестування (50c -> 5с)
const unsigned long T_S1 = 500 * T_FACTOR; // 50000;
const unsigned long T_S2 = 20 * T_FACTOR;  // 2000;
const unsigned long T_S3 = 20 * T_FACTOR;  // 2000;
const unsigned long T_S4 = 500 * T_FACTOR; // 50000;
const unsigned long T_S5 = 20 * T_FACTOR;  // 2000;
const unsigned long T_S6 = 250 * T_FACTOR; // 25000;
const unsigned long T_S7 = 20 * T_FACTOR;  // 2000;
const unsigned long T_S8 = 30 * T_FACTOR;  // 3000;

// Функція для встановлення виходів світлофора
void setTrafficLights(int h_r, int h_y, int h_g, int p_r, int p_g) {
    // H-Напрямок
    digitalWrite(LED_H_RED, h_r);
    digitalWrite(LED_H_YELLOW, h_y);
    digitalWrite(LED_H_GREEN, h_g);

    // P-Напрямок (синхронно для Ped1 та Ped2)
    digitalWrite(LED_P_RED, p_r);
    digitalWrite(LED_P_GREEN, p_g);
    digitalWrite(LED_P2_RED, p_r);
    digitalWrite(LED_P2_GREEN, p_g);
}

// Функція переходу до нового стану
void changeState(State newState) {
    currentState = newState;
    stateStartTime = millis();
    // Виводимо поточний час для відстеження
    Serial.print("--- ");
    Serial.print(millis());
    Serial.print("ms: Transition to State: ");
    switch (newState) {
        case S1_H_GREEN: Serial.println("S1 (H-Green: 50s)"); break;
        case S2_H_YELLOW: Serial.println("S2 (H-Yellow: 2s)"); break;
        case S3_PAUSE_1: Serial.println("S3 (All-Red: 2s) - Checking Request"); break;
        case S4_V_GREEN: Serial.println("S4 (V-Green: 50s) - H/P Red"); break;
        case S5_V_YELLOW: Serial.println("S5 (V-Yellow: 2s) - H/P Red"); break;
        case S6_P_GREEN: Serial.println("S6 (P-Green: 25s)"); break;
        case S7_P_FLASH: Serial.println("S7 (P-Flash: 2s)"); break;
        case S8_PAUSE_2: Serial.println("S8 (All-Red: 3s) - Final Pause"); break;
    }
}

// --- Setup ---
void setup() {
    // ... (pinMode залишається тим самим) ...
    pinMode(LED_H_RED, OUTPUT);
    pinMode(LED_H_YELLOW, OUTPUT);
    pinMode(LED_H_GREEN, OUTPUT);

    pinMode(LED_P_RED, OUTPUT);
    pinMode(LED_P_GREEN, OUTPUT);
    pinMode(LED_P2_RED, OUTPUT);
    pinMode(LED_P2_GREEN, OUTPUT);

    pinMode(BUTTON_PIN_1, INPUT_PULLUP);
    pinMode(BUTTON_PIN_2, INPUT_PULLUP);

    Serial.begin(115200);
    Serial.println("Traffic Light FSM Started.");
    Serial.print("--- Using test factor: x1/");
    Serial.println(1000/T_FACTOR);

    // Початковий стан
    changeState(S1_H_GREEN);
}

// --- Loop ---
void loop() {
    unsigned long currentTime = millis();
    unsigned long elapsedTime = currentTime - stateStartTime;

    // --- Обробка Кнопки (Дебаунсинг та Реєстрація Одноразового Натискання) ---
    bool currentButtonState = digitalRead(BUTTON_PIN_1) && digitalRead(BUTTON_PIN_2); // HIGH якщо обидві ВІДПУЩЕНІ
    bool buttonPressed = !currentButtonState; // TRUE якщо хоча б одна НАТИСНУТА

    if (buttonPressed && lastButtonState == HIGH) {
        // Кнопка була натиснута (перехід HIGH -> LOW)
        if (!pedestrianRequest) {
            pedestrianRequest = true; // Запит встановлюється лише один раз
            Serial.println("-> Pedestrian Button Pressed! Request set.");
        }
    }
    lastButtonState = currentButtonState; // Оновлюємо стан

    // --- Логіка Кінцевого Автомату ---
    switch (currentState) {
        case S1_H_GREEN:
            setTrafficLights(LOW, LOW, HIGH, HIGH, LOW); // H-G, P-R. (V-R)
            if (elapsedTime >= T_S1) {
                changeState(S2_H_YELLOW);
            }
            break;

        case S2_H_YELLOW:
            setTrafficLights(LOW, HIGH, LOW, HIGH, LOW); // H-Y, P-R. (V-R)
            if (elapsedTime >= T_S2) {
                changeState(S3_PAUSE_1);
            }
            break;

        case S3_PAUSE_1:
            setTrafficLights(HIGH, LOW, LOW, HIGH, LOW); // H-R, P-R. (V-R)
            if (elapsedTime >= T_S3) {
                if (pedestrianRequest) {
                    changeState(S6_P_GREEN); // Кнопка натиснута -> Пішохід
                    pedestrianRequest = false; // Скидаємо запит
                } else {
                    changeState(S4_V_GREEN); // Кнопка не натиснута -> V-Напрямок
                }
            }
            break;

        case S4_V_GREEN:
            // S4: V-G, H-R, P-R. (H-R продовжує горіти)
            setTrafficLights(HIGH, LOW, LOW, HIGH, LOW); 
            if (elapsedTime >= T_S4) {
                changeState(S5_V_YELLOW);
            }
            break;

        case S5_V_YELLOW:
            // S5: V-Y, H-R, P-R. (H-R продовжує горіти)
            setTrafficLights(HIGH, LOW, LOW, HIGH, LOW); 
            if (elapsedTime >= T_S5) {
                changeState(S8_PAUSE_2);
            }
            break;

        case S6_P_GREEN:
            setTrafficLights(HIGH, LOW, LOW, LOW, HIGH); // H-R, P-G. (V-R)
            if (elapsedTime >= T_S6) {
                changeState(S7_P_FLASH);
            }
            break;

        case S7_P_FLASH:
            // S7: P-Green мигає.
            // Ми встановлюємо H-R, V-R
            setTrafficLights(HIGH, LOW, LOW, LOW, LOW); // Спочатку вимикаємо всі P
            
            // Мигання P-GREEN: вмикається/вимикається кожні 250мс
            if (elapsedTime % 500 < 250) {
                 digitalWrite(LED_P_GREEN, HIGH);
                 digitalWrite(LED_P2_GREEN, HIGH);
                 digitalWrite(LED_P_RED, LOW);
                 digitalWrite(LED_P2_RED, LOW);
            } else {
                 // У фазі вимкнення P-Green, P-Red має горіти
                 digitalWrite(LED_P_GREEN, LOW);
                 digitalWrite(LED_P2_GREEN, LOW);
                 digitalWrite(LED_P_RED, HIGH);
                 digitalWrite(LED_P2_RED, HIGH);
            }

            if (elapsedTime >= T_S7) {
                changeState(S8_PAUSE_2);
            }
            break;

        case S8_PAUSE_2:
            setTrafficLights(HIGH, LOW, LOW, HIGH, LOW); // All-R.
            if (elapsedTime >= T_S8) {
                changeState(S1_H_GREEN);
            }
            break;
    }
}