// retroDE_ps2 — tb_gs_scanout_psmt4_clut (Ch103)
//
// Locks the contract for PSMT4 (= 0x14) scanout. PSMT4 stores
// 2 pixels per byte (low nibble = even pixel, high nibble = odd
// pixel). pcrtc derives byte_offset = pixel_index >> 1 and
// nibble_sel = pixel_index[0]. With clut_enable=1, the 4-bit
// nibble (zero-extended to 8 bits and added to CSA*16) indexes
// clut_stub for real RGB. With clut_enable=0, the nibble is
// replicated across the byte (4→8 bit-expansion) so the
// storage lane is visually verifiable.
//
// Three phases (no gs_stub — direct VRAM and CLUT writes):
//   Phase 1 — clut_enable=1, clut_csa=0. Stage a 4×4 PSMT4
//             sprite at indices 0x0..0xF; stage 16 CLUT entries
//             with the Ch97/Ch102 `clut_entry(i)` formula.
//             Asserts each in-sprite pixel reads the CLUT entry
//             matching its nibble; non-sprite pixels read
//             CLUT[0] (the loaded entry for nibble=0).
//   Phase 2 — clut_enable=0 (grayscale fallback). Same VRAM
//             contents; asserts each in-sprite pixel reads
//             RGB = {nibble, nibble} (so 4'hF → 0xFF, 4'h5 →
//             0x55, etc.) and non-sprite pixels (nibble=0)
//             read 0x00.
//   Phase 3 — clut_enable=1, clut_csa=1. Programs CLUT entries
//             0x10..0x1F with a distinct (clut_entry-formula)
//             pattern, leaves entries 0..15 cleared, and asserts
//             each in-sprite nibble reads `CLUT[nibble + 0x10]`.
//             Non-sprite pixels (nibble=0 + CSA*16 = 0x10) read
//             the same entry as in-sprite (0,0). Locks the
//             `zero_extend(nibble) + (CSA << 4)` indexing math.

`timescale 1ns/1ps

module tb_gs_scanout_psmt4_clut;

    localparam int PCRTC_H_ACTIVE = 16;
    localparam int PCRTC_V_ACTIVE = 8;

    logic clk;
    logic rst_n;
    initial clk = 1'b0;
    always #5 clk = ~clk;

    // Privileged-register-shaped TB feeds (no gs_stub).
    logic [63:0] pmode_q;
    logic [63:0] dispfb1_q;
    logic [63:0] display1_q;

    // VRAM with a TB-direct write port for staging.
    logic        vram_we;
    logic [31:0] vram_waddr;
    logic [31:0] vram_wdata;
    logic [3:0]  vram_wbe;
    logic [31:0] vram_raddr;
    logic [31:0] vram_rdata;

    vram_stub #(.BYTES(4096)) u_vram (
        .clk(clk), .rst_n(rst_n),
        .write_en  (vram_we),
        .write_addr(vram_waddr),
        .write_data(vram_wdata),
        .write_be  (vram_wbe),
        .write_mask(32'hFFFF_FFFF),
        .read_addr (vram_raddr),
        .read_data (vram_rdata),
        .read2_addr(32'd0),
        .read2_data()
    );

    // CLUT — TB-direct write, comb read by pcrtc.
    logic        clut_we;
    logic [7:0]  clut_widx;
    logic [31:0] clut_wdata;
    logic [7:0]  clut_ridx;
    logic [31:0] clut_rdata;

    clut_stub u_clut (
        .clk(clk), .rst_n(rst_n),
        .write_en  (clut_we),
        .write_idx (clut_widx),
        .write_data(clut_wdata),
        .read_idx  (clut_ridx),
        .read_data (clut_rdata)
    );

    logic       clut_enable;
    logic [4:0] clut_csa;
    logic       hsync_o, vsync_o, de_o;
    logic [7:0] r_o, g_o, b_o;
    logic                pcrtc_ev_valid;
    trace_pkg::subsys_e  pcrtc_ev_subsys;
    trace_pkg::event_e   pcrtc_ev_event;
    logic [63:0]         pcrtc_ev_arg0, pcrtc_ev_arg1;
    logic [63:0]         pcrtc_ev_arg2, pcrtc_ev_arg3;
    logic [31:0]         pcrtc_ev_flags;

    gs_pcrtc_stub #(
        .H_ACTIVE(PCRTC_H_ACTIVE), .H_FRONT(1), .H_SYNC(1), .H_BACK(1),
        .V_ACTIVE(PCRTC_V_ACTIVE), .V_FRONT(1), .V_SYNC(1), .V_BACK(1)
    ) u_pcrtc (
        .clk(clk), .rst_n(rst_n),
        .pmode_q       (pmode_q),
        .dispfb1_q     (dispfb1_q),
        .display1_q    (display1_q),
        .vram_read_addr(vram_raddr),
        .vram_read_data(vram_rdata),
        .clut_enable   (clut_enable),
        .clut_csa      (clut_csa),
        .clut_read_idx (clut_ridx),
        .clut_read_data(clut_rdata),
        .hsync(hsync_o), .vsync(vsync_o), .de(de_o),
        .r(r_o), .g(g_o), .b(b_o),
        .ev_valid(pcrtc_ev_valid),
        .ev_subsys(pcrtc_ev_subsys),
        .ev_event(pcrtc_ev_event),
        .ev_arg0(pcrtc_ev_arg0), .ev_arg1(pcrtc_ev_arg1),
        .ev_arg2(pcrtc_ev_arg2), .ev_arg3(pcrtc_ev_arg3),
        .ev_flags(pcrtc_ev_flags)
    );

    logic [7:0] cap_r [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic [7:0] cap_g [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic [7:0] cap_b [0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];
    logic       cap_de[0:PCRTC_V_ACTIVE-1][0:PCRTC_H_ACTIVE-1];

    int  errors;
    bit  capture_armed;

    initial begin
        for (int y = 0; y < PCRTC_V_ACTIVE; y++)
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                cap_r[y][x]  = 8'd0;
                cap_g[y][x]  = 8'd0;
                cap_b[y][x]  = 8'd0;
                cap_de[y][x] = 1'b0;
            end
        errors        = 0;
        capture_armed = 1'b0;
    end

    always_ff @(posedge clk) begin
        if (rst_n && capture_armed && de_o
            && (u_pcrtc.vcnt < PCRTC_V_ACTIVE)
            && (u_pcrtc.hcnt < PCRTC_H_ACTIVE)) begin
            cap_r [u_pcrtc.vcnt][u_pcrtc.hcnt] <= r_o;
            cap_g [u_pcrtc.vcnt][u_pcrtc.hcnt] <= g_o;
            cap_b [u_pcrtc.vcnt][u_pcrtc.hcnt] <= b_o;
            cap_de[u_pcrtc.vcnt][u_pcrtc.hcnt] <= 1'b1;
        end
    end

    task automatic vram_write32(input logic [31:0] addr,
                                input logic [31:0] data,
                                input logic [3:0]  be);
        @(negedge clk);
        vram_we    = 1'b1;
        vram_waddr = addr;
        vram_wdata = data;
        vram_wbe   = be;
        @(posedge clk);
        @(negedge clk);
        vram_we    = 1'b0;
        vram_waddr = 32'd0;
        vram_wdata = 32'd0;
        vram_wbe   = 4'b0000;
    endtask

    task automatic clut_write(input logic [7:0]  idx,
                              input logic [31:0] entry);
        @(negedge clk);
        clut_we    = 1'b1;
        clut_widx  = idx;
        clut_wdata = entry;
        @(posedge clk);
        @(negedge clk);
        clut_we    = 1'b0;
        clut_widx  = 8'd0;
        clut_wdata = 32'd0;
    endtask

    task automatic clear_capture();
        for (int y = 0; y < PCRTC_V_ACTIVE; y++)
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                cap_r[y][x]  = 8'd0;
                cap_g[y][x]  = 8'd0;
                cap_b[y][x]  = 8'd0;
                cap_de[y][x] = 1'b0;
            end
    endtask

    function automatic logic [31:0] clut_entry(input logic [7:0] i);
        // Same Ch97/Ch99/Ch102 formula: A=0xFF, B=i+0x80,
        // G=i+0x40, R=i.
        logic [7:0] r8, g8, b8, a8;
        r8 = i;
        g8 = i + 8'h40;
        b8 = i + 8'h80;
        a8 = 8'hFF;
        return {a8, b8, g8, r8};
    endfunction

    // Sprite indices follow row-major idx = y*4 + x, range 0x0..0xF.
    function automatic logic [3:0] sprite_idx(input int x, input int y);
        return 4'(y * 4 + x);
    endfunction

    // DISPFB1: FBP=0, FBW=1, PSM=PSMT4 (= 0x14 = 5'b10100).
    //   FBW=1 → bit 9 → 0x200
    //   PSM=0x14 << 15 = 0xA_0000
    localparam logic [63:0] DISPFB1_VAL  = 64'h0000_0000_000A_0200;
    localparam logic [63:0] DISPLAY1_VAL =
          (64'(PCRTC_H_ACTIVE - 1) << 32)
        | (64'(PCRTC_V_ACTIVE - 1) << 44);
    localparam logic [63:0] PMODE_EN1    = 64'h0000_0000_0000_0001;

    localparam int SPRITE_W = 4;
    localparam int SPRITE_H = 4;

    initial begin
        rst_n        = 1'b0;
        pmode_q      = 64'd0;
        dispfb1_q    = 64'd0;
        display1_q   = 64'd0;
        clut_enable  = 1'b0;
        clut_csa     = 5'd0;
        vram_we      = 1'b0;
        vram_waddr   = 32'd0;
        vram_wdata   = 32'd0;
        vram_wbe     = 4'b0000;
        clut_we      = 1'b0;
        clut_widx    = 8'd0;
        clut_wdata   = 32'd0;

        repeat (4) @(posedge clk);
        rst_n = 1'b1;
        repeat (2) @(posedge clk);

        // --- Stage CLUT entries 0..15 with the standard pattern.
        for (int i = 0; i < 16; i++) begin
            logic [7:0] idx_8;
            idx_8 = i[7:0];
            clut_write(idx_8, clut_entry(idx_8));
        end

        // --- Stage the 4×4 PSMT4 sprite. Each row holds 4
        // 4-bit indices = 2 bytes.
        //   FBW=1 → row stride = FBW * 64 / 2 = 32 bytes.
        // For row y, column x (x in 0..3): byte offset = y*32 + x/2;
        // low nibble (x even) or high nibble (x odd).
        // Pack each row as two bytes: byte 0 = {idx1, idx0},
        // byte 1 = {idx3, idx2}. Both fit in one 32-bit write
        // at the row's start (byte 0..3).
        for (int y = 0; y < SPRITE_H; y++) begin
            logic [3:0]  i0, i1, i2, i3;
            logic [7:0]  byte0, byte1;
            logic [31:0] data;
            int          row_byte;

            i0 = sprite_idx(0, y);
            i1 = sprite_idx(1, y);
            i2 = sprite_idx(2, y);
            i3 = sprite_idx(3, y);
            byte0 = {i1, i0};
            byte1 = {i3, i2};
            // Place in low 16 bits of the 32-bit word; upper
            // 16 bits will hold pixels (4..7, y) which are NOT
            // part of the sprite — leave them as 0 so non-sprite
            // VRAM still produces nibble=0 (CLUT[0] / grayscale 0).
            data     = {16'd0, byte1, byte0};
            row_byte = y * 32;
            vram_write32(row_byte, data, 4'b1111);
        end

        // ---- Phase 1: CLUT enabled.
        dispfb1_q   = DISPFB1_VAL;
        display1_q  = DISPLAY1_VAL;
        clut_enable = 1'b1;
        clut_csa    = 5'd0;
        @(posedge clk);

        if (dispfb1_q[19:15] !== 5'h14) begin
            $error("DISPFB1.PSM=0x%02x (expected 0x14 PSMT4)", dispfb1_q[19:15]);
            errors = errors + 1;
        end

        pmode_q = PMODE_EN1;
        @(posedge clk);

        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b1;
        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b0;

        for (int y = 0; y < PCRTC_V_ACTIVE; y++) begin
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                logic [3:0]  nibble;
                logic [31:0] entry;
                logic [7:0]  er, eg, eb;

                if (x < SPRITE_W && y < SPRITE_H)
                    nibble = sprite_idx(x, y);
                else
                    nibble = 4'd0;

                entry = clut_entry({4'd0, nibble});
                er    = entry[7:0];
                eg    = entry[15:8];
                eb    = entry[23:16];

                if (!cap_de[y][x]) begin
                    $error("[phase1] (%0d,%0d) DE never asserted", x, y);
                    errors = errors + 1;
                end
                if (cap_r[y][x] !== er || cap_g[y][x] !== eg || cap_b[y][x] !== eb) begin
                    $error("[phase1] (%0d,%0d) got (%02x,%02x,%02x) expected (%02x,%02x,%02x) nibble=0x%01x",
                           x, y, cap_r[y][x], cap_g[y][x], cap_b[y][x],
                           er, eg, eb, nibble);
                    errors = errors + 1;
                end
            end
        end

        $display("[tb_gs_scanout_psmt4_clut] phase 1 (CLUT enabled, CSA=0) done");

        // ---- Phase 2: grayscale fallback (clut_enable=0).
        clut_enable = 1'b0;
        @(posedge clk);
        clear_capture();

        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b1;
        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b0;

        for (int y = 0; y < PCRTC_V_ACTIVE; y++) begin
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                logic [3:0] nibble;
                logic [7:0] expanded;
                logic [7:0] er, eg, eb;

                if (x < SPRITE_W && y < SPRITE_H)
                    nibble = sprite_idx(x, y);
                else
                    nibble = 4'd0;

                expanded = {nibble, nibble};
                er = expanded; eg = expanded; eb = expanded;

                if (!cap_de[y][x]) begin
                    $error("[phase2] (%0d,%0d) DE never asserted", x, y);
                    errors = errors + 1;
                end
                if (cap_r[y][x] !== er || cap_g[y][x] !== eg || cap_b[y][x] !== eb) begin
                    $error("[phase2] (%0d,%0d) got (%02x,%02x,%02x) expected grayscale 0x%02x nibble=0x%01x",
                           x, y, cap_r[y][x], cap_g[y][x], cap_b[y][x],
                           expanded, nibble);
                    errors = errors + 1;
                end
            end
        end

        $display("[tb_gs_scanout_psmt4_clut] phase 2 (grayscale fallback) done");

        // ---- Phase 3: CSA=1. Lookup index = zero_ext(nibble) +
        // (CSA << 4) = nibble + 0x10. Program CLUT entries
        // 0x10..0x1F with the same `clut_entry(i)` formula so
        // each nibble lands on a distinct, identifiable RGB.
        // Re-clear entries 0..15 (well, set them to a sentinel
        // value distinct from clut_entry(0x10..0x1F)) so a buggy
        // CSA-not-applied path would visibly fail.
        for (int i = 0; i < 16; i++) begin
            logic [7:0] idx_8;
            idx_8 = i[7:0];
            // Sentinel for entries 0..15 — make them all 0 so
            // a CSA-bypass bug would produce black instead of
            // the expected shifted-window color.
            clut_write(idx_8, 32'hDEAD_BEEF);
        end
        for (int i = 0; i < 16; i++) begin
            logic [7:0] idx_8;
            idx_8 = 8'h10 + i[7:0];
            clut_write(idx_8, clut_entry(idx_8));
        end

        clut_enable = 1'b1;
        clut_csa    = 5'd1;
        @(posedge clk);
        clear_capture();

        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b1;
        @(posedge u_pcrtc.end_of_frame);
        @(posedge clk);
        capture_armed = 1'b0;

        for (int y = 0; y < PCRTC_V_ACTIVE; y++) begin
            for (int x = 0; x < PCRTC_H_ACTIVE; x++) begin
                logic [3:0]  nibble;
                logic [7:0]  expected_idx;
                logic [31:0] entry;
                logic [7:0]  er, eg, eb;

                if (x < SPRITE_W && y < SPRITE_H)
                    nibble = sprite_idx(x, y);
                else
                    nibble = 4'd0;

                expected_idx = {4'd0, nibble} + 8'h10;
                entry = clut_entry(expected_idx);
                er    = entry[7:0];
                eg    = entry[15:8];
                eb    = entry[23:16];

                if (!cap_de[y][x]) begin
                    $error("[phase3] (%0d,%0d) DE never asserted", x, y);
                    errors = errors + 1;
                end
                if (cap_r[y][x] !== er || cap_g[y][x] !== eg || cap_b[y][x] !== eb) begin
                    $error("[phase3] (%0d,%0d) got (%02x,%02x,%02x) expected (%02x,%02x,%02x) nibble=0x%01x effective_idx=0x%02x",
                           x, y, cap_r[y][x], cap_g[y][x], cap_b[y][x],
                           er, eg, eb, nibble, expected_idx);
                    errors = errors + 1;
                end
            end
        end

        $display("[tb_gs_scanout_psmt4_clut] phase 3 (CLUT enabled, CSA=1 → nibble + 0x10) done");

        if (errors == 0) $display("[tb_gs_scanout_psmt4_clut] PASS");
        else             $display("[tb_gs_scanout_psmt4_clut] FAIL");
        $finish;
    end

    initial begin
        #5000000;
        $error("[tb_gs_scanout_psmt4_clut] timeout");
        $finish;
    end

endmodule : tb_gs_scanout_psmt4_clut
