#include <xc.h>

// Osilatör frekansı tanımı (4 MHz kristal varsayımı)
#define _XTAL_FREQ 4000000 // 4 MHz

// Konfigürasyon bitleri
#pragma config FOSC = HS    // Yüksek hızlı osilatör
#pragma config WDTE = OFF  // Watchdog Timer kapalı
#pragma config PWRTE = OFF // Power-up Timer kapalı
#pragma config BOREN = ON  // Brown-out Reset açık
#pragma config LVP = OFF   // Düşük voltaj programlama kapalı
#pragma config CPD = OFF   // Veri belleği kodu koruma kapalı
#pragma config CP = OFF    // Program belleği kodu koruma kapalı

// Giriş/Çıkış Tanımlamaları
#define B1 RA0
#define B2 RA1
#define B3 RA2
#define B4 RA3
#define S1 RB0
#define S2 RB1
#define S3 RB2
#define R1 RB3
#define R2 RB4
#define SYSTEM_SWITCH RE0
#define RESET_ERROR RE1
#define M1 RD0
#define M2 RD1
#define M3 RD2
#define M4 RD3
#define Z1 RD4
#define Z2 RD5
#define Z3 RD6
#define S1_ERROR RC0
#define S2_ERROR RC1
#define S3_ERROR RC2
#define R1_ERROR RC3
#define R2_ERROR RC4
#define SYSTEM_ON RC5
#define SYSTEM_OFF RC6

// Gecikme fonksiyonu (XC8 için optimize)
void delay_ms(unsigned int ms) {
    while (ms--) {
        __delay_ms(1); // XC8 dahili gecikme fonksiyonu
    }
}

// Buton sırası için kuyruk
unsigned char queue[4] = {0};
unsigned char queue_head = 0;
unsigned char queue_tail = 0;
unsigned char processing = 0;

void enqueue(unsigned char button) {
    if (queue_tail < 4 && !processing) {
        queue[queue_tail++] = button;
    }
}

unsigned char dequeue() {
    if (queue_head < queue_tail) {
        return queue[queue_head++];
    }
    return 0xFF;
}

void init_system() {
    M4 = 1; M3 = 0;
    unsigned int reed_timeout = 0;
    while (R1 == 0 && reed_timeout < 5) {
        delay_ms(100);
        reed_timeout++;
    }
    M4 = 0;
    if (reed_timeout >= 5)
        R1_ERROR = 1;
}

void reset_error_leds() {
    S1_ERROR = 0; S2_ERROR = 0;
    S3_ERROR = 0;
    R1_ERROR = 0; R2_ERROR = 0;
}

void main() {
    // Port yapılandırması
    TRISA = 0xFF; // RA giriş
    TRISB = 0xFF; // RB giriş
    TRISC = 0x00; // RC çıkış
    TRISD = 0x00; // RD çıkış
    TRISE = 0xFF; // RE giriş
    ADCON1 = 0x06; // Analog girişleri kapat

    // Başlangıç durumu
    S1_ERROR = 0; S2_ERROR = 0;
    S3_ERROR = 0;
    R1_ERROR = 0; R2_ERROR = 0;
    SYSTEM_ON = 0; SYSTEM_OFF = 1;

    while (1) {
        // Hata LED'lerini sıfırlama
        if (RESET_ERROR == 1) {
            reset_error_leds();
            delay_ms(50);
            while (RESET_ERROR == 1);
        }

        if (SYSTEM_SWITCH == 1) {
            SYSTEM_ON = 1;
            SYSTEM_OFF = 0;
            if (queue_head == 0 && queue_tail == 0) init_system();

            // Buton girişleri
            if (B1 == 1 && !processing) {
                enqueue(0); delay_ms(50); while (B1 == 1);
            }
            if (B2 == 1 && !processing && queue_tail == 0) {
                enqueue(1); delay_ms(50); while (B2 == 1);
            }
            if (B3 == 1 && !processing && queue_tail < 2) {
                enqueue(2); delay_ms(50); while (B3 == 1);
            }
            if (B4 == 1 && !processing && queue_tail < 3) {
                enqueue(3); delay_ms(50); while (B4 == 1);
            }

            // Kuyruk işleme
            if (!processing && queue_head < queue_tail) {
                processing = 1;
                unsigned char current_button = dequeue();

                if (current_button == 0) {
                    // B1 (Kol 2 gönderim)
                    M4 = 1; M3 = 0;
                    unsigned int reed_timeout = 0;
                    while (R1 == 0 && reed_timeout < 5) {
                        delay_ms(100);
                        reed_timeout++;
                    }
                    M4 = 0;
                    if (reed_timeout >= 5) R1_ERROR = 1;
                    M2 = 1; M1 = 0;
                    unsigned int sensor_timeout = 0;
                    while (S3 == 0 && sensor_timeout < 60) {
                        delay_ms(100);
                        sensor_timeout++;
                    }
                    M2 = 0;
                    if (sensor_timeout >= 60) S3_ERROR = 1;
                    Z3 = 1; delay_ms(500); Z3 = 0;
                }
                else if (current_button == 1) {
                    // B2 (Kol 3 gönderim)
                    M3 = 1; M4 = 0;
                    unsigned int reed_timeout = 0;
                    while (R2 == 0 && reed_timeout < 5) {
                        delay_ms(100);
                        reed_timeout++;
                    }
                    M3 = 0;
                    if (reed_timeout >= 5) R2_ERROR = 1;
                    M2 = 1; M1 = 0;
                    unsigned int sensor_timeout = 0;
                    while (S2 == 0 && sensor_timeout < 60) {
                        delay_ms(100);
                        sensor_timeout++;
                    }
                    M2 = 0;
                    if (sensor_timeout >= 60) S2_ERROR = 1;
                    Z2 = 1; delay_ms(500); Z2 = 0;
                }
                else if (current_button == 2) {
                    // B3 (Kol 2 geri alma)
                    M3 = 1; M4 = 0;
                    unsigned int reed_timeout = 0;
                    while (R2 == 0 && reed_timeout < 5) {
                        delay_ms(100);
                        reed_timeout++;
                    }
                    M3 = 0;
                    if (reed_timeout >= 5) R2_ERROR = 1;
                    M1 = 1; M2 = 0;
                    unsigned int sensor_timeout = 0;
                    while (S1 == 0 && sensor_timeout < 60) {
                        delay_ms(100);
                        sensor_timeout++;
                    }
                    M1 = 0;
                    if (sensor_timeout >= 60) S1_ERROR = 1;
                    Z1 = 1; delay_ms(500); Z1 = 0;
                }
                else if (current_button == 3) {
                    // B4 (Kol 3 geri alma)
                    M4 = 1; M3 = 0;
                    unsigned int reed_timeout = 0;
                    while (R1 == 0 && reed_timeout < 5) {
                        delay_ms(100);
                        reed_timeout++;
                    }
                    M4 = 0;
                    if (reed_timeout >= 5) R1_ERROR = 1;
                    M1 = 1; M2 = 0;
                    unsigned int sensor_timeout = 0;
                    while (S1 == 0 && sensor_timeout < 60) {
                        delay_ms(100);
                        sensor_timeout++;
                    }
                    M1 = 0;
                    if (sensor_timeout >= 60) S1_ERROR = 1;
                    Z1 = 1; delay_ms(500); Z1 = 0;
                }
                processing = 0;
            }
        } else {
            SYSTEM_ON = 0;
            SYSTEM_OFF = 1;
            queue_head = 0; queue_tail = 0;
            M1 = 0; M2 = 0; M3 = 0; M4 = 0;
            Z1 = 0; Z2 = 0; Z3 = 0;
        }
    }
}