#include <cstdint>

#include "xuartlite.h"

struct Gpio {
    volatile uint32_t data;
};

#define gpio0 ((Gpio*)0x40000000)

namespace {
constexpr uintptr_t kUart0BaseAddress = 0x40600000;
XUartLite uart0_inst;
XUartLite_Config uart0_config = {
    .DeviceId = 0,
    .RegBaseAddr = kUart0BaseAddress,
    .BaudRate = 115200,
    .UseParity = false,
    .DataBits = 8,
};

XUartLite* uart0 = &uart0_inst;

void InitUarts() {
    XUartLite_CfgInitialize(uart0, &uart0_config, uart0_config.RegBaseAddr);
}

uint8_t UartRead() {
    uint8_t c;
    while (XUartLite_Recv(uart0, &c, 1) < 1) {
    }
    return c;
}

void UartWrite(uint8_t c) {
    XUartLite_Send(uart0, &c, 1);
    while (XUartLite_IsSending(uart0)) {}
}

void Jump(uint32_t addr) {
    auto jump = reinterpret_cast<void (*)()>(addr);
    jump();
}

constexpr uint8_t kBackspace = 0x7f;
constexpr uint8_t kOtherBackspace = 0x08;

uint8_t ReadHexNibble(uint8_t c) {
    // lowercase only
    if (c <= '9') {
        return c - '0';
    }
    return 10 + (c - 'a');
}

uint32_t ReadHex(const char* buf) {
    uint32_t out = 0;
    while (*buf == ' ') {
        buf++;
    }
    for (int i = 0; i < 8; i++) {
        uint8_t c = ReadHexNibble(*buf);
        if (c > 0xf) {
            break;
        }
        out = (out << 4) + c;
        buf++;
    }

    return out;
}

void WriteHexNibble(uint8_t c) {
    if (c > 9) {
        UartWrite('a' + c - 10);
    } else {
        UartWrite('0' + c);
    }
}

void UartWriteUint32(uint32_t a) {
    for (int i = 0; i < 8; i++) {
        WriteHexNibble((a >> 28) & 0xf);
        a <<= 4;
    }
}

void UartWriteUint8(uint8_t a) {
    WriteHexNibble(a >> 4);
    WriteHexNibble(a & 0xf);
}

void UartDump(uint32_t addr, int count) {
   for (int i = 0; i < count; i++) {
       UartWrite(' ');
       UartWriteUint8(*reinterpret_cast<uint8_t*>(addr + i));
   } 
}

void DumpHex(uint32_t addr) {
    addr &= 0xfffffffc;
    UartWriteUint32(addr);
    UartWrite(':');
    UartDump(addr, 4);
    UartWrite('\r');
    UartWrite('\n');
}

int FindChar(const char* buf, uint8_t c) {
    int found = 0;
    while (*buf) {
        if (*buf == c) {
            return found;
        }
        found++;
        buf++;
    }
    return -1;
}


}  // namespace

int main() {
    gpio0->data = 1;
    uint32_t cur_addr = 0;
    uint32_t cur_data = 0;
    bool writing = false;

    char inbuf[64] = {};
    char* inptr = inbuf;

    InitUarts();

    while (1) {
        uint8_t c = UartRead();
        UartWrite(c); // echo
        if (c == '\r') {
            gpio0->data = 0x55;
            *inptr = 0;
            if (inptr == inbuf) {
                cur_addr += 4;
            } else if (FindChar(inbuf, 'r') >= 0) {
                Jump(cur_addr);
            } else {
                cur_addr = ReadHex(inbuf);
                UartWrite('\n');
            }
            DumpHex(cur_addr);
            int assigned = FindChar(inbuf, ':');
            if (assigned >= 0) {
                cur_data = ReadHex(inbuf + assigned + 1);
                *(reinterpret_cast<uint32_t*>(cur_addr)) = cur_data;
            }
            inptr = inbuf;
        } else if (c == kBackspace) {
            inptr--;
            if (inptr < inbuf) {
                inptr = inbuf;
                continue;
            }
            UartWrite(kOtherBackspace);
            UartWrite(' ');
            UartWrite(kOtherBackspace);
        } else {
            *inptr++ = c;
        }
    }
}