#include "/opt/arm_user/include/AT91SAM9263.h"

    #define DBGU_DALL 0xFFFFFFFF
    #define P_DTXD (0x1 << 31)
    #define P_DRXD (0x1 << 30)

    #define CD 54
    #define ASCII_MASK 	0x7F
    #define ASCII_CASE 	(0x1 << 5)
    #define BUF_SIZE   	16

    void dbgu_print_ascii(const char* string){};

    void Init_DBGU() {

        *AT91C_DBGU_IDR = DBGU_DALL;
        *AT91C_DBGU_CR = AT91C_US_TXDIS | AT91C_US_RXDIS | AT91C_US_RSTTX | AT91C_US_RSTRX;
        *AT91C_PIOC_PDR = P_DRXD | P_DTXD;
        *AT91C_PIOC_ASR = P_DRXD | P_DTXD;
        *AT91C_DBGU_MR = AT91C_US_CHMODE_NORMAL | AT91C_US_PAR_NONE;
        *AT91C_DBGU_BRGR = CD;
        *AT91C_DBGU_CR = AT91C_US_TXEN | AT91C_US_RXEN;
    }

    char receive_DBGU() {
        while((*AT91C_DBGU_CSR & AT91C_US_RXRDY) == 0);
        return *AT91C_DBGU_RHR & ASCII_MASK;
    }

    void transmit_DBGU(char wyslijchar) {
        while((*AT91C_DBGU_CSR & AT91C_US_TXRDY) == 0);
        *AT91C_DBGU_THR = wyslijchar;
    }

    char swap_case(char AZ) {
        if (AZ >= 'A' && AZ <= 'Z') {
            return AZ | ASCII_CASE;
        } else if (AZ >= 'a' && AZ <= 'z') {
            return AZ & ~ASCII_CASE;
        } else {
            return AZ;
        }
    }

    typedef struct FIFO {
        char String[BUF_SIZE + 1];
        int head;
        int tail;
    } FIFO;

    FIFO Buffer;

    void Init_FIFO(FIFO *Queue) {
        Queue->head = 0;
        Queue->tail = 0;
        int i = BUF_SIZE;
        while (i--) {
            Queue->String[i] = 0;
        }
    }

    int dequeue_FIFO(FIFO *Queue, char *chrToBeWritten) {
        if (Queue->head == Queue->tail) return 0;
        *chrToBeWritten = Queue->String[Queue->tail++];
        if (Queue->tail > BUF_SIZE) Queue->tail = 0;
        return 1;
    }

    int enqueue_FIFO(FIFO *Queue, char *chrToWrite) {
        if ((Queue->tail - Queue->head) == 1 || (Queue->tail == 0 && Queue->head == BUF_SIZE)) return 0;
        Queue->String[Queue->head++] =*chrToWrite;
        if (Queue->head > BUF_SIZE) Queue->head = 0;
        return 1;
    }

    void Empty_FIFO(FIFO *Queue) {
        Queue->head = Queue->tail;
    }

    void println(char *drukuj) {
        int i = 0;
        while (drukuj[i] != '\0') {
            transmit_DBGU(*(drukuj+i++));
        }
        transmit_DBGU('\n');
        transmit_DBGU('\r');
    }

    void println_sc(char *drukuj) {
        int i = 0;
        char chr;
        while (drukuj[i] != '\0') {
            chr = swap_case(*(drukuj+i++));
            transmit_DBGU(chr);
        }
        transmit_DBGU('\n');
        transmit_DBGU('\r');
    }

    void perr_overflow() {
        char error[] = "max 16 znakow";
        println(error);
    }

    void print_from_FIFO(FIFO *Queue) {
        int get_result = 0;
        int i;
        char chr_TxD, drukuj[BUF_SIZE + 1];
        for (i=0; i < BUF_SIZE; i++) {
            get_result = dequeue_FIFO(Queue, &chr_TxD);
            if (!get_result) {
                drukuj[i] = '\0';
                break;
            }
            drukuj[i] = swap_case(chr_TxD);
        }
        drukuj[BUF_SIZE] = '\0';
        println(drukuj);
    }

    void print_male() {
        char chr;
        for (chr='a'; chr<='z'; chr++) {
            transmit_DBGU(chr);
        }
        transmit_DBGU('\n');
        transmit_DBGU('\r');
    }

    void print_duze() {
        char chr;
        for (chr='A'; chr<='Z'; chr++) {
            transmit_DBGU(chr);
        }
        transmit_DBGU('\n');
        transmit_DBGU('\r');
    }

    void hello() {
        char nosiema[] = "Hello World!";
        println_sc(nosiema);
    }


    int main() {
        Init_DBGU();
        Init_FIFO(&Buffer);
        char chr_RxD;
        int post_result = 0;
        print_male();
        print_duze();
        hello();
        while(1) {
            chr_RxD = receive_DBGU();
            if (chr_RxD == '\n' || chr_RxD == '\r') {
                print_from_FIFO(&Buffer);
            } else {
                post_result = enqueue_FIFO(&Buffer, &chr_RxD);
                if (!post_result) {
                    perr_overflow();
                    Empty_FIFO(&Buffer);
                }
            }
        }

        return 0;
    }