#include <cstdint>

uint8_t UartRead();
void UartWrite(uint8_t);

namespace {

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

__attribute__((used))
void wozmon() {
    uint32_t cur_addr = 0;
    uint32_t cur_data = 0;

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

    while (1) {
        uint8_t c = UartRead();
        UartWrite(c); // echo
        if (c == '\r') {
            *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;
        }
    }
}