`timescale 1ns/1ps

module tb();

localparam ADDR_FAILCODE = 32'h800;
localparam WATCHDOG_MAX = 32'h00001000;

initial $timeformat(-9, 2, " ns", 20);
initial begin: dump
    integer i;
    $dumpfile("tb.vcd");
    $dumpvars(0, tb);
    // for (i=0; i<32; i=i+1) begin
    //     $dumpvars(0, dut.regfile[i]);
    // end
    // $dumpvars(0, mem[ADDR_FAILCODE]);
end

reg clk, reset;
wire dummy_out;

// Memory Parameters
localparam  MEM_ROM_LENGTH = 2048;  // bytes
localparam  MEM_LENGTH = MEM_ROM_LENGTH + 2048;  // bytes
localparam  DATA_INVALID    = 32'hdeadbeef;

// Memory
reg [7:0] mem [0:MEM_LENGTH-1];
initial $readmemh("tb.hex", mem);

// Instruction Memory
wire [31:0] mem_inst_addr;
reg [31:0] mem_inst_data;
always @(*) begin
    if (mem_inst_addr < MEM_LENGTH - 3) begin
        mem_inst_data[ 7: 0] = mem[mem_inst_addr+0];
        mem_inst_data[15: 8] = mem[mem_inst_addr+1];
        mem_inst_data[23:16] = mem[mem_inst_addr+2];
        mem_inst_data[31:24] = mem[mem_inst_addr+3];
    end else begin
        mem_inst_data = DATA_INVALID;
    end
end

// Data memory
wire [31:0] mem_data_addr;
reg [31:0] mem_data_rdata;
wire [31:0] mem_data_wdata;
wire [3:0] mem_data_wmask;
wire mem_data_we;
reg mem_data_wready;
reg mem_data_rvalid;

always @(mem_data_addr) begin    
    mem_data_rvalid = 1'b1;
    if (mem_data_addr < MEM_LENGTH - 3) begin
        mem_data_rdata[ 7: 0] = mem[mem_data_addr+0];
        if (mem_data_addr[0] == 0) begin
            mem_data_rdata[15: 8] = mem[mem_data_addr+1];
            if (mem_data_addr[1] == 0) begin
                mem_data_rdata[23:16] = mem[mem_data_addr+2];
                mem_data_rdata[31:24] = mem[mem_data_addr+3];
            end else begin
                mem_data_rdata[31:16] = 0;
            end
        end else begin
            mem_data_rdata[31:8] = 0;
        end
    end else begin
        mem_data_rdata = DATA_INVALID;
    end
end

always @(posedge clk) begin
    if (mem_data_we) begin
        if (mem_data_wready) begin
            mem_data_wready = 1'b0;
            if (mem_data_addr < MEM_LENGTH && mem_data_addr >= MEM_ROM_LENGTH) begin
                if (mem_data_wmask[0]) begin
                    mem[mem_data_addr+0] <= mem_data_wdata[7:0];
                end
                if (mem_data_wmask[1]) begin
                    mem[mem_data_addr+1] <= mem_data_wdata[15:8];
                end
                if (mem_data_wmask[2]) begin
                    mem[mem_data_addr+2] <= mem_data_wdata[23:16];
                end
                if (mem_data_wmask[3]) begin
                    mem[mem_data_addr+3] <= mem_data_wdata[31:24];
                end
            end else begin
                // ignore illegal writes
            end
        end else begin
            mem_data_wready = 1'b1;
        end
    end else begin
        mem_data_wready = 1'b0;
    end
end


// Main control
initial begin
    #0
    clk = 0;
    reset = 1;
    
    #10
    reset = 0;
    
    #10000
    reset = 1;
    $finish;
end

integer watchdog = 0;
logic [31:0] returnval;
always @(posedge clk) begin
    #100
    returnval = {mem[ADDR_FAILCODE+3], mem[ADDR_FAILCODE+2], mem[ADDR_FAILCODE+1], mem[ADDR_FAILCODE+0]};
    case (returnval)
        32'h00000000: begin
            // Initial value
        end
        32'hffffffff: begin
            // Success
            #100
            $display("%0t:\tSUCCESS: TEST PASSED", $time);
            $finish;
        end
        default: begin
            $display("%0t:\tERROR: FAILCODE = 0x%h", $time, returnval);
            #200
            $finish;
        end
    endcase

    watchdog = watchdog + 1;
    if (watchdog > WATCHDOG_MAX) begin
            $display("%0t:\tERROR: WATCHDOG", $time);
            #200
            $finish;
    end
end

always #2 clk  = !clk;

core dut(
    .clk(clk),
    .reset(reset),

    .mem_inst_addr(mem_inst_addr),
    .mem_inst_data(mem_inst_data),

    .mem_data_addr(mem_data_addr),
    .mem_data_rdata(mem_data_rdata),
    .mem_data_wdata(mem_data_wdata),
    .mem_data_wmask(mem_data_wmask),
    .mem_data_we(mem_data_we),
    .mem_data_rvalid(mem_data_rvalid),
    .mem_data_wready(mem_data_wready),

    // .mem_data_addr(mem_data_addr),
    // .mem_data_wdata(mem_data_wdata),
    // .mem_data_rdata(mem_data_rdata),
    // .mem_data_en(mem_data_en),
    // .mem_data_we(mem_data_we),
    // .mem_data_valid(mem_data_valid),
    // .mem_data_done(mem_data_done)

    .dummy_out(dummy_out)
);

// wire axi_mem_data_awvalid;
// wire [11:0] axi_mem_data_awaddr;
// wire [2:0] axi_mem_data_awprot;
// wire axi_mem_data_awready;
// wire axi_mem_data_wvalid;
// wire [31:0] axi_mem_data_wdata;
// wire [3:0] axi_mem_data_wstrb;
// wire axi_mem_data_wready;
// wire axi_mem_data_bvalid;
// wire axi_mem_data_bready;
// wire [1:0] axi_mem_data_bresp;
// wire axi_mem_data_arvalid;
// wire [11:0] axi_mem_data_araddr;
// wire [2:0] axi_mem_data_arprot;
// wire axi_mem_data_arready;
// wire axi_mem_data_rvalid;
// wire [31:0] axi_mem_data_rdata;
// wire [1:0] axi_mem_data_resp;
// wire axi_mem_data_rready;

// axi_lite_memory axi_mem_data(
//     .ACLK(clk),
//     .ARESETn(!reset),
//     .AWVALID(axi_mem_data_awvalid),
//     .AWADDR(axi_mem_data_awaddr),
//     .AWPROT(axi_mem_data_awprot),
//     .AWREADY(axi_mem_data_awready),
//     .WVALID(axi_mem_data_wvalid),
//     .WDATA(axi_mem_data_wdata),
//     .WSTRB(axi_mem_data_wstrb),
//     .WREADY(axi_mem_data_wready),
//     .BVALID(axi_mem_data_bvalid),
//     .BREADY(axi_mem_data_bready),
//     .BRESP(axi_mem_data_bresp),
//     .ARVALID(axi_mem_data_arvalid),
//     .ARADDR(axi_mem_data_araddr),
//     .ARPROT(axi_mem_data_arprot),
//     .ARREADY(axi_mem_data_arready),
//     .RVALID(axi_mem_data_rvalid),
//     .RDATA(axi_mem_data_rdata),
//     .RRESP(axi_mem_data_resp),
//     .RREADY(axi_mem_data_rready),

//     .WB_WADDR(mem_data_waddr),
//     .WB_WPROT(),
//     .WB_WDATA(mem_data_wdata),
//     .WB_WSTRB(mem_data_wmask),
//     .WB_WVALID(mem_data_we),
//     .WB_WREADY(1'b1),

//     .WB_RADDR(mem_data_raddr),
//     .WB_RDATA(mem_data_rdata),
//     .WB_RVALID(1'b1),
//     .WB_RREADY()
// );

endmodule