#include "Vcpu.h"
#include "Vcpu_cpu.h"
#include "verilated.h"

Vcpu *top;

void dump_regs(void)
{
    printf("-> regs:\n");
    for (int i = 0; i < 8; i++)
        printf("  %i: 0x%02x\n", i, top->v->vr[i]);
}

void step(void)
{
    top->cpu_clock = 0;
    top->eval();
    top->cpu_clock = 1;
    top->eval();
}

void step3(void)
{
    step(); step(); step();
}

void run_instruction(uint16_t instruction)
{
    top->v->ram[0] = instruction >> 8;
    top->v->ram[1] = instruction & 0xFF;
    top->v->pc = 0;
}

void reset(void)
{
    top->reset = 0;
    step3(); step3();
    top->reset = 1;
}

void test_alu_instruction(uint8_t x, uint8_t y, uint16_t ins, uint8_t expected)
{
    printf("INS %04x: ", ins);
    reset();
    top->v->vr[1] = x;
    top->v->vr[2] = y;
    run_instruction(ins);
    step3();
    if (top->v->vr[1] != expected)
        printf(" FAIL! Expected %02x, got %02x.\n", expected, top->v->vr[1]);
    else
        printf(" OK.\n");
}

void test_alu_instruction_vf(uint8_t x, uint8_t y, uint16_t ins, uint8_t expected, uint8_t evf)
{
    printf("INS %04x: ", ins);
    reset();
    top->v->vr[1] = x;
    top->v->vr[2] = y;
    run_instruction(ins);
    step3();
    if (top->v->vr[1] != expected)
        printf(" FAIL! Expected %02x, got %02x.\n", expected, top->v->vr[1]);
    else
    {
        if (top->v->vr[15] != evf)
            printf(" FAIL! Expected VF %02x, got %02x.\n", evf, top->v->vr[15]);
        else
            printf(" OK.\n");
    }
}

int main(int argc, char **argv)
{
    Verilated::commandArgs(argc, argv);
    top = new Vcpu;
    // LD
    printf("LD ");
    test_alu_instruction(0x00, 0x45, 0x8120, 0x45);
    // OR
    printf("OR ");
    test_alu_instruction(0x0F, 0xF0, 0x8121, 0xFF);
    // AND
    printf("AND ");
    test_alu_instruction(0xF0, 0x45, 0x8122, 0x40);
    // XOR
    printf("XOR ");
    test_alu_instruction(0xFF, 0x11, 0x8123, 0xEE);
    // ADD
    printf("ADD (no carry) ");
    test_alu_instruction_vf(0x10, 0x25, 0x8124, 0x35, 0x00);
    printf("ADD (with carry) ");
    test_alu_instruction_vf(0xF0, 0x43, 0x8124, 0x33, 0x01);
    // SUB
    printf("SUB (no borrow) ");
    test_alu_instruction_vf(0x45, 0x15, 0x8125, 0x30, 0x01);
    printf("SUB (zero, no borrow) ");
    test_alu_instruction_vf(0x45, 0x45, 0x8125, 0x00, 0x01);
    printf("SUB (borrow) ");
    test_alu_instruction_vf(0x15, 0x45, 0x8125, 0xD0, 0x00);
    // SHR
    printf("SHR (one) ");
    test_alu_instruction_vf(0x45, 0x00, 0x8126, 0x45>>1, 0x01); 
    printf("SHR (zero) ");
    test_alu_instruction_vf(0x44, 0x00, 0x8126, 0x44>>1, 0x00);
    // SUBN
    printf("SUBN (no borrow) ");
    test_alu_instruction_vf(0x15, 0x45, 0x8127, 0x30, 0x01);
    printf("SUBN (zero, no borrow) ");
    test_alu_instruction_vf(0x45, 0x45, 0x8127, 0x00, 0x01);
    printf("SUBN (borrow) ");
    test_alu_instruction_vf(0x45, 0x15, 0x8127, 0xD0, 0x00);
    // SHL
    printf("SHL (one) ");
    test_alu_instruction_vf(0xA4, 0x00, 0x812E, (0xA4<<1)&0xFF, 0x01);
    printf("SHL (zero) ");
    test_alu_instruction_vf(0x24, 0x00, 0x812E, (0x24<<1)&0xFF, 0x00);

    printf("PC increment: ");
    reset();
    run_instruction(0x0000); // NOP
    step3();
    if (top->v->pc != 0x2)
        printf(" FAIL!\n");
    else
        printf(" OK.\n");

    printf("JMP imm INS 1456: ");
    reset();
    run_instruction(0x1456);
    step3();
    if (top->v->pc != 0x456)
        printf(" FAIL!\n");
    else
        printf(" OK.\n");
}