#include <stdio.h>
#include <conio.h>
#include <c64.h>
#include <string.h>
#include <stdlib.h>
#include "input.h"
#include "menu/menu.h"

#define DATA_ADDR 0xdd01
#define DIR_ADDR 0xdd03
#define SCREEN_ADDR 0x0400
#define COLOR_ADDR 0xD800

signed char NOT_STOP = 1;
char mode = 0;
char itr = 0;

volatile char* dataPtr = (volatile char*)DATA_ADDR;
volatile char* dirPtr = (volatile char*)DIR_ADDR;


char* version = "0.5";
char* creator = "Christian Werner";
unsigned char original_background_color, original_border_color, original_text_color = 0;


char* menu_items[] = {
    "Output",
    "Input"
};

char menu_items_len = sizeof(menu_items)/sizeof(menu_items[0]);

char dirBuff[8];
char dataBuff[8];

char dataState = 255;
char previousKey = 0;

void writeToScreen(char x, char y, char c, char color) {
    unsigned short offset = y * 40 + x;
    *(char*)(SCREEN_ADDR + offset) = c;
    *(char*)(COLOR_ADDR + offset) = color;
}

int getNumber(char* promt, int min, int max) {
    int n;
    printf("%s [%d-%d]: ", promt, min, max);
    cursor(1);
    cscanf("%d", &n);
    cursor(0);

    if (n > max) {
        n = max;
    }

    if (n < min) {
        n = min;
    }
    return n;
}

void resetScreen() {
    clrscr();
    textcolor(COLOR_GRAY1);
    printf("Userport Playground v%s\ncreated by ", version);
    textcolor(COLOR_LIGHTBLUE);
    printf("%s\n", creator);
    textcolor(COLOR_GRAY1);
    chline(40);
    textcolor(COLOR_WHITE);
}

void toBinaryArray(char *buff , char value) {
    char mask = 1;
    char i;
    for(i = 0; i < 8; i++) {
        buff[i] = value & mask;
        value = value >> 1;
    }
}

void drawBinary(char low, char high, char* buff, char x, char y, char c_low, char c_high) {
    char i;
    char c;
    char col;

    for(i = 0; i < 8; i++) {
        if (buff[i] == 0) {
            c = low;
            col = c_low;
        } else {
            c = high;
            col = c_high;
        }
        writeToScreen(x + i * 2, y, c, col);
    }
}

void userport() {
    char direction = *(char*)0xdd03;
    char data = *(char*)0xdd01;
    char key_cur, key_prev, pet_code, p_c, p_p, x, y;

    toBinaryArray(dirBuff, direction);
    toBinaryArray(dataBuff, data);

    gotoxy(0,3);
    printf("dir:  %03d  ", direction);
    drawBinary(73+128,  79+128, dirBuff, 10, 3, COLOR_ORANGE, COLOR_GREEN);

    gotoxy(0,5);
    printf("data: %03d  ", data);
    drawBinary(160,  160, dataBuff, 10, 5, COLOR_GRAY1, COLOR_RED);

    key_cur = getCurrentKeyMatrixCode();
    key_prev = getPreviousKeyMatrixCode();

    p_c = matrixCodeToPetscii(key_cur);
    p_p = matrixCodeToPetscii(key_prev);


    writeToScreen(5,10, p_c, COLOR_GREEN);
    gotoxy(7,10);
    printf("%02d, %03d", key_cur, p_c);

    x = key_cur % 8;
    y = key_cur / 8;

    printf("  %02d, %02d", x, y);

}

void init() {
    original_background_color = bgcolor(COLOR_BLACK);
    original_border_color = bordercolor(COLOR_BLACK);
    original_text_color = textcolor(COLOR_GRAY1);
    resetScreen();
}

int menu() {
    char i;
    int index;
    resetScreen();
    for(i = 0; i < menu_items_len; i++) {
        printf("%d. %s\n", i+1, menu_items[i]);
    }
    index = getNumber("Pick mode", 1, menu_items_len) - 1;
    return index;
}

void restore() {
    bgcolor(original_background_color);
    bordercolor(original_border_color);
    textcolor(original_text_color);
    clrscr();
    // set char set to upper case
    *(char*)0xd018 = 0x14;
    *(char*)DIR_ADDR = 255;
    *(char*)DATA_ADDR = 0;
}

void defaultLoop() {

    while(1) {
        char key = matrixCodeToPetscii(getCurrentKeyMatrixCode());

        if (key == 3) {
            mode = 2;
            break;
        }

        if (key == 95) {
            mode = 1;
            break;
        }

        if (previousKey != key && key > '0' && key < '9') {
            dataState = dataState ^ (1 << key - 49);
            *(char*)0xdd03 = dataState;
        }

        *(char*)0xdd01 = itr;
        userport();
        itr++;
        previousKey = key;
    }
}

void fast() {
     while(1) {
        char key = matrixCodeToPetscii(getCurrentKeyMatrixCode());

        if (key == 3) {
            mode = 2;
            break;
        }

        if (key == 95) {
            mode = 0;
            break;
        }

        if (previousKey != key && key > '0' && key < '9') {
            dataState = dataState ^ (1 << key - 49);
            *(char*)0xdd03 = dataState;
        }

        *(char*)0xdd01 = itr;
        itr++;
        previousKey = key;
    }
}

void dod(char* title) {
    char i = 0;
    *(char*)0xdd03 = dataState;
    while (1) {


        switch (mode)
        {
        case 0:
            defaultLoop();
            break;
        case 1:
            fast();
            break;
        default:
            mode = 0;
            dataState = 255;
            itr = 0;
            return;
        }
    }
}

void writeAllCharsToScreen() {
    char x, y, c;

    for (y = 0; y < 7; y++) {
        for (x = 0; x < 40; x++) {
            c = y * 40 + x;
            writeToScreen(x, y, c, COLOR_GREEN); 
        }
    }
}

void notImplemented(char* title) {
    char origCol;
    origCol = textcolor(COLOR_YELLOW);
    printf("\"%s\" is not implemented :(", title);
    textcolor(origCol);
    printf("\nPress any key to continiue...");
    getInputChar();
}

void exitProgram(char* title) {
    NOT_STOP = 0;
}


void recieveTest(char* title) {
    unsigned short screenStart = SCREEN_ADDR + 40 * 3 ;
    volatile char* screen = (volatile char*)screenStart;
    unsigned short screenEnd = SCREEN_ADDR + 40*25;
    char d, isArduinoReady, buff;
    char lower, upper, fullByte;
    short i = 0;
    lower = 16;
    upper = 16;

    *dirPtr = 0b11100000;
    *dataPtr = 0;

    while (1) {

        char key = matrixCodeToPetscii(getCurrentKeyMatrixCode());

        if (key == 3) {
            break;
        }

        d = *dataPtr;

        isArduinoReady = (d >> 4) & 1;
        buff = d & 15;
        
        if (isArduinoReady) {
            if (upper == 16) {
                upper = buff;
            } else if (lower == 16)
            {
                lower = buff;
            }
        } else {
            continue;
        }

        if (upper != 16 && lower != 16) {
            fullByte = (upper << 4) + lower;
            if (fullByte > 0) {
                *screen = fullByte;
                screen++;
            }
            if (screen == screenEnd) {
                screen = (volatile char*)screenStart;
            }
            upper = 16;
            lower = 16;
        }

        //set comodore ready
        for (i = 0; i < 100; i++) {}
        *dataPtr = d | 0b00100000;

        for (i = 0; i < 100; i++) {}
        *dataPtr = d & 0b11011111;
    }
}


void main() {
    // int selection;
    char huh;

    Menu* menu = createMenu(4, "Main Menu", COLOR_LIGHTGREEN, 0, 3);
    addMenuEntry(menu, "Userport Test", 0, dod);
    addMenuEntry(menu, "Recieve Test", 1, recieveTest);
    addMenuEntry(menu, "Test", 2, notImplemented);
    addMenuEntry(menu, "EXIT", 3, exitProgram);

    init();

    while (NOT_STOP)
    {
        awaitMenuSelect(menu);
    }
    
    //selection = menu();
    // selection = 1;
    // resetScreen();

    // if (selection == 0) {
    //     userport();
    // } else {
    //     userport();
    // }

    //*(char*)0xd018 = 0x14;

    // dod();

    free(menu);
    restore();
    exit(0);
}