3 files changed, 240 insertions, 317 deletions

diff --git a/.gitignore b/.gitignore
index 472c55f..82b46f9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
 rtl/lisp_coproc_sim
+*.vcd
 *~
-#*
\ No newline at end of file
+#*
diff --git a/rtl/lisp_coproc.sv b/rtl/lisp_coproc.sv
index bd3057a..b855b7b 100644
--- a/rtl/lisp_coproc.sv
+++ b/rtl/lisp_coproc.sv
@@ -8,248 +8,195 @@ module lisp_coproc (
     output reg  [7:0]  data_out
 );
 
-    // FSM States (One-Hot Encoding)
-    parameter [4:0] RESET    = 5'b00001;
-    parameter [4:0] IDLE     = 5'b00010;
-    parameter [4:0] DECODE   = 5'b00100;
-    parameter [4:0] EXECUTE  = 5'b01000;
-    parameter [4:0] WRITEBACK= 5'b10000;
-
-    // Internal Registers
-    reg [4:0] state, next_state;
-    reg [7:0] opcode_reg, arg1_reg, arg2_reg, result_reg, status_reg;
-    reg [7:0] heap [0:15];  // 16 entries x 8 bits
+    // ========================================================================
+    // 1. DATAPATH Signals & Storage
+    // ========================================================================
+    
+    // Registers
+    reg [7:0] opcode_reg, arg1_reg, arg2_reg, result_reg;
+    reg [7:0] heap [0:15]; 
+    
+    // Bump Allocator: Uniform 4-bit register (0-15)
+    reg [3:0] bump_alloc;
     
-    // FIX: Bump allocator must be 5 bits to hold the value '16' (Full)
-    // without wrapping around to 0.
-    reg [4:0] bump_alloc;   
+    // Heap Status: Sticky bit to track if we have wrapped around (Full)
+    reg heap_filled;
     
-    // Status Register Bits
-    wire busy = (state != IDLE);
-    reg err_heap_full, err_type, carry, zero;
+    // Internal Flags (Transient for current OP)
+    reg flag_err_heap, flag_err_type, flag_carry, flag_zero;
     
     // ALU Signals
-    reg [5:0] alu_a, alu_b;
+    wire [5:0] alu_val_a = arg1_reg[5:0];
+    wire [5:0] alu_val_b = arg2_reg[5:0];
+    wire [6:0] alu_sum   = alu_val_a + alu_val_b; 
+    wire       alu_eq    = (arg1_reg == arg2_reg);
+    
+    // Type Checkers
+    wire is_cons_a   = (arg1_reg[7:6] == 2'b11);
+    wire is_cons_b   = (arg2_reg[7:6] == 2'b11);
+    wire is_num_a    = (arg1_reg[7:6] == 2'b10);
+    wire is_num_b    = (arg2_reg[7:6] == 2'b10);
+    
+    // Allocation Logic (Datapath Adder)
+    // We use a 5-bit wire to capture the carry out.
+    // If bump_alloc is 14 (1110) + 2 = 16 (10000).
+    // alloc_sum[4] (Carry) is 1. alloc_sum[3:0] is 0000.
+    wire [4:0] alloc_sum = {1'b0, bump_alloc} + 5'd2;
+    wire       alloc_carry = alloc_sum[4];
+
+    // ========================================================================
+    // 2. FSM CONTROLLER
+    // ========================================================================
     
-    // Temporary registers for operations
-    reg [7:0] temp_result;
-    reg [3:0] temp_ptr;
+    parameter [4:0] RESET     = 5'b00001;
+    parameter [4:0] IDLE      = 5'b00010;
+    parameter [4:0] DECODE    = 5'b00100;
+    parameter [4:0] EXECUTE   = 5'b01000;
+    parameter [4:0] WRITEBACK = 5'b10000;
+
+    reg [4:0] state, next_state;
 
-    // FSM State Transition
     always @(posedge clk or posedge rst) begin
-        if (rst) begin
-            state <= RESET;
-        end else begin
-            state <= next_state;
-        end
+        if (rst) state <= RESET;
+        else     state <= next_state;
     end
-    
-    // FSM Combinational Logic
+
     always @(*) begin
         next_state = state;
-        
         case (state)
-            RESET: begin
-                next_state = IDLE;
-            end
-            
-            IDLE: begin
-                if (cs && !rw && addr == 3'h0) begin  // Writing to OPCODE triggers operation
-                    next_state = DECODE;
-                end
-            end
-            
-            DECODE: begin
-                next_state = EXECUTE;
-            end
-            
-            EXECUTE: begin
-                next_state = WRITEBACK;
-            end
-            
-            WRITEBACK: begin
-                next_state = IDLE;
-            end
-            
-            default: begin
-                next_state = IDLE;
-            end
+            RESET:     next_state = IDLE;
+            IDLE:      if (cs && !rw && addr == 3'h0) next_state = DECODE;
+            DECODE:    next_state = EXECUTE;
+            EXECUTE:   next_state = WRITEBACK;
+            WRITEBACK: next_state = IDLE;
+            default:   next_state = IDLE;
         endcase
     end
-    
-    // Register File and Memory Interface
+
+    // ========================================================================
+    // 3. SEQUENTIAL LOGIC
+    // ========================================================================
+
+    integer i;
     always @(posedge clk or posedge rst) begin
         if (rst) begin
-            opcode_reg <= 8'h00;
-            arg1_reg <= 8'h00;
-            arg2_reg <= 8'h00;
-            result_reg <= 8'h00;
-            status_reg <= 8'h00;
-            bump_alloc <= 5'h00; // Reset 5-bit register
-            err_heap_full <= 1'b0;
-            err_type <= 1'b0;
-            carry <= 1'b0;
-            zero <= 1'b0;
+            opcode_reg    <= 8'h00;
+            arg1_reg      <= 8'h00;
+            arg2_reg      <= 8'h00;
+            result_reg    <= 8'h00;
+            bump_alloc    <= 4'h0;
+            heap_filled   <= 1'b0;
+            
+            flag_err_heap <= 1'b0;
+            flag_err_type <= 1'b0;
+            flag_carry    <= 1'b0;
+            flag_zero     <= 1'b0;
+            
+            for (i=0; i<16; i=i+1) heap[i] <= 8'h00;
+            
         end else begin
-            // Memory-mapped register writes
+            
+            // --- MMIO Writes ---
             if (cs && !rw) begin
                 case (addr)
                     3'h0: opcode_reg <= data_in;
-                    3'h1: arg1_reg <= data_in;
-                    3'h2: arg2_reg <= data_in;
-                    3'h3: result_reg <= data_in;  // Direct write to result
-                    3'h4: status_reg <= data_in;  // Direct write to status
+                    3'h1: arg1_reg   <= data_in;
+                    3'h2: arg2_reg   <= data_in;
+                    3'h3: result_reg <= data_in;
                 endcase
             end
             
-            // FSM State-specific operations
+            // --- State Actions ---
             case (state)
                 RESET: begin
-                    // Clear heap on reset
-                    integer i;
-                    for (i = 0; i < 16; i = i + 1) begin
-                        heap[i] <= 8'h00;
-                    end
-                    bump_alloc <= 5'h00;
-                    opcode_reg <= 8'h00;
-                    arg1_reg <= 8'h00;
-                    arg2_reg <= 8'h00;
-                    result_reg <= 8'h00;
-                    status_reg <= 8'h00;
+                    bump_alloc  <= 4'h0;
+                    heap_filled <= 1'b0;
                 end
-                
+
                 IDLE: begin
-                    // Only clear flags when a NEW operation starts.
                     if (cs && !rw && addr == 3'h0) begin
-                        err_heap_full <= 1'b0;
-                        err_type <= 1'b0;
-                        carry <= 1'b0;
-                        zero <= 1'b0;
+                        flag_err_heap <= 1'b0;
+                        flag_err_type <= 1'b0;
+                        flag_carry    <= 1'b0;
+                        flag_zero     <= 1'b0;
                     end
                 end
-                
+
                 EXECUTE: begin
-                    // Assign ALU inputs for ADD operation using BLOCKING assignment
-                    alu_a = arg1_reg[5:0];
-                    alu_b = arg2_reg[5:0];
-                    
                     case (opcode_reg)
-                        // CONS operation
-                        8'h01: begin
-                            // 5-bit arithmetic: 16 + 2 = 18. 18 > 16 is TRUE.
-                            if (bump_alloc + 2 > 16) begin 
-                                err_heap_full <= 1'b1;
+                        8'h01: begin // CONS
+                            if (heap_filled) begin
+                                // If sticky flag is set, we are full. Error.
+                                flag_err_heap <= 1'b1;
                             end else begin
-                                // Store ARG1 and ARG2 in heap
-                                heap[bump_alloc[3:0]] <= arg1_reg;
-                                heap[bump_alloc[3:0] + 1] <= arg2_reg;
-                                // Return CONS tag with pointer as value
-                                temp_ptr <= bump_alloc[3:0];
-                                bump_alloc <= bump_alloc + 2;
+                                // Perform allocation
+                                heap[bump_alloc]     <= arg1_reg;
+                                heap[bump_alloc + 1] <= arg2_reg;
+                                
+                                // Update pointer (wraps automatically due to 4-bit)
+                                bump_alloc <= alloc_sum[3:0];
+                                
+                                // If we generated a carry (14->16), mark heap as filled
+                                if (alloc_carry) heap_filled <= 1'b1;
                             end
                         end
-                        
-                        // CAR operation
-                        8'h02: begin
-                            if (arg1_reg[7:6] != 2'b11) begin  // Not a CONS
-                                err_type <= 1'b1;
-                            end else begin
-                                temp_result <= heap[arg1_reg[3:0]];
-                            end
+                        8'h02: begin // CAR
+                            if (!is_cons_a) flag_err_type <= 1'b1;
                         end
-                        
-                        // CDR operation
-                        8'h03: begin
-                            if (arg1_reg[7:6] != 2'b11) begin  // Not a CONS
-                                err_type <= 1'b1;
-                            end else begin
-                                temp_result <= heap[arg1_reg[3:0] + 1];
-                            end
+                        8'h03: begin // CDR
+                            if (!is_cons_a) flag_err_type <= 1'b1;
                         end
-                        
-                        // ATOM operation
-                        8'h04: begin
-                            if (arg1_reg[7:6] == 2'b11) begin  // Is a CONS
-                                temp_result <= 8'h00;  // NIL
-                            end else begin
-                                temp_result <= 8'h41;  // 'T' (01_000001)
-                            end
+                        8'h05: begin // EQ
+                            if (alu_eq) flag_zero <= 1'b1;
                         end
-                        
-                        // EQ operation
-                        8'h05: begin
-                            if (arg1_reg == arg2_reg) begin
-                                temp_result <= 8'h41;  // 'T' (01_000001)
-                                zero <= 1'b1;
+                        8'h06: begin // ADD
+                            if (!is_num_a || !is_num_b) begin
+                                flag_err_type <= 1'b1;
                             end else begin
-                                temp_result <= 8'h00;  // NIL
-                            end
-                        end
-                        
-                        // ADD operation
-                        8'h06: begin
-                            if (arg1_reg[7:6] != 2'b10 || arg2_reg[7:6] != 2'b10) begin  // Not both NUMBERs
-                                err_type <= 1'b1;
-                            end else begin
-                                if ((alu_a + alu_b) > 6'd63) begin
-                                    carry <= 1'b1;
-                                end
-                                if ((alu_a + alu_b) == 6'd0) begin
-                                    zero <= 1'b1;
-                                end
-                                temp_result <= {2'b10, alu_a + alu_b};  // NUMBER tag with sum
+                                if (alu_sum[6]) flag_carry <= 1'b1;
+                                if (alu_sum[5:0] == 6'd0) flag_zero <= 1'b1;
                             end
                         end
                     endcase
                 end
-                
+
                 WRITEBACK: begin
-                    // Write the result based on the operation
                     case (opcode_reg)
-                        8'h01: begin  // CONS
-                            if (!err_heap_full) begin
-                                // Tag [7:6] must be set correctly.
-                                result_reg <= {2'b11, 2'b00, temp_ptr}; 
-                            end
+                        8'h01: begin // CONS
+                            if (!flag_err_heap) 
+                                // Math trick: If bump_alloc wrapped to 0, 
+                                // 0 - 2 = 14 (1110 in 2's comp), which is the correct pointer.
+                                result_reg <= {2'b11, 2'b00, bump_alloc - 4'd2};
                         end
-                        
-                        8'h02: begin  // CAR
-                            if (!err_type) begin
-                                result_reg <= temp_result;
-                            end
+                        8'h02: begin // CAR
+                            if (!flag_err_type) result_reg <= heap[arg1_reg[3:0]];
                         end
-                        
-                        8'h03: begin  // CDR
-                            if (!err_type) begin
-                                result_reg <= temp_result;
-                            end
+                        8'h03: begin // CDR
+                            if (!flag_err_type) result_reg <= heap[arg1_reg[3:0] + 1];
                         end
-                        
-                        8'h04: begin  // ATOM
-                            result_reg <= temp_result;
+                        8'h04: begin // ATOM
+                            result_reg <= is_cons_a ? 8'h00 : 8'h41;
                         end
-                        
-                        8'h05: begin  // EQ
-                            result_reg <= temp_result;
+                        8'h05: begin // EQ
+                            result_reg <= alu_eq ? 8'h41 : 8'h00;
                         end
-                        
-                        8'h06: begin  // ADD
-                            if (!err_type) begin
-                                result_reg <= temp_result;
-                            end
+                        8'h06: begin // ADD
+                            if (!flag_err_type) result_reg <= {2'b10, alu_sum[5:0]};
                         end
                     endcase
                 end
             endcase
         end
     end
+
+    // ========================================================================
+    // 4. OUTPUT LOGIC
+    // ========================================================================
     
-    // Update status register
-    always @(*) begin
-        status_reg = {3'b000, zero, carry, err_type, err_heap_full, busy};
-    end
-    
-    // Output Logic
+    wire busy_bit = (state != IDLE);
+    // Note: bit 1 is the transient error flag, not the internal sticky state
+    wire [7:0] current_status = {3'b000, flag_zero, flag_carry, flag_err_type, flag_err_heap, busy_bit};
+
     always @(*) begin
         if (cs && rw) begin
             case (addr)
@@ -257,11 +204,11 @@ module lisp_coproc (
                 3'h1: data_out = arg1_reg;
                 3'h2: data_out = arg2_reg;
                 3'h3: data_out = result_reg;
-                3'h4: data_out = status_reg;
+                3'h4: data_out = current_status;
                 default: data_out = 8'h00;
             endcase
         end else begin
-            data_out = 8'hZZ;  // High impedance when not reading
+            data_out = 8'hZZ;
         end
     end
 
diff --git a/rtl/tb_lisp_coproc.sv b/rtl/tb_lisp_coproc.sv
index 6c39aa4..aa721fc 100644
--- a/rtl/tb_lisp_coproc.sv
+++ b/rtl/tb_lisp_coproc.sv
@@ -2,170 +2,145 @@
 
 module tb_lisp_coproc;
 
-    // Testbench signals
-    reg        clk;
-    reg        rst;
-    reg        cs;
-    reg        rw;      // 0=Write, 1=Read
+    // Signals
+    reg        clk, rst, cs, rw;
     reg [2:0]  addr;
     reg [7:0]  data_in;
     wire [7:0] data_out;
     
-    // Test variables
+    // Verification Vars
     reg [7:0]  read_data;
-    integer i; 
+    integer    i;
     
-    // Instantiate the DUT
+    // DUT Instance
     lisp_coproc dut (
-        .clk(clk),
-        .rst(rst),
-        .cs(cs),
-        .rw(rw),
-        .addr(addr),
-        .data_in(data_in),
-        .data_out(data_out)
+        .clk(clk), .rst(rst), .cs(cs), .rw(rw),
+        .addr(addr), .data_in(data_in), .data_out(data_out)
     );
     
-    // Clock generation
+    // 100MHz Clock
     initial begin
         clk = 0;
         forever #5 clk = ~clk;
     end
     
-    // CPU Write Task
-    task cpu_write;
-        input [2:0] addr_in;
-        input [7:0] data_in_in;
+    // --- CPU Bus Tasks ---
+    
+    task cpu_write(input [2:0] w_addr, input [7:0] w_data);
         begin
             @(posedge clk);
-            cs = 1'b1;
-            rw = 1'b0;  // Write
-            addr = addr_in;
-            data_in = data_in_in;
+            cs = 1; rw = 0; addr = w_addr; data_in = w_data;
             @(posedge clk);
-            cs = 1'b0;
+            cs = 0; data_in = 8'h00; // Clear bus
         end
     endtask
     
-    // CPU Read Task
-    task cpu_read;
-        input [2:0] addr_in;
-        output [7:0] data_out_out;
-        reg [7:0] status;
+    task cpu_read(input [2:0] r_addr, output [7:0] r_data);
+        reg [7:0] stat;
         begin
-            // Poll until not busy
+            // 1. Poll Status for BUSY=0
             do begin
                 @(posedge clk);
-                cs = 1'b1;
-                rw = 1'b1;  // Read
-                addr = 3'h4;  // Status register
+                cs = 1; rw = 1; addr = 3'h4; // Status Reg
                 @(posedge clk);
-                status = data_out;
-                cs = 1'b0;
-            end while (status[0]);  // Check BUSY bit
+                stat = data_out;
+                cs = 0;
+            end while (stat[0] == 1'b1);
             
-            // Read the requested address
+            // 2. Perform Actual Read
             @(posedge clk);
-            cs = 1'b1;
-            rw = 1'b1;  // Read
-            addr = addr_in;
+            cs = 1; rw = 1; addr = r_addr;
             @(posedge clk);
-            data_out_out = data_out;
-            cs = 1'b0;
+            r_data = data_out;
+            cs = 0;
         end
     endtask
     
-    // Test Sequence
+    // --- Main Test Sequence ---
     initial begin
-        // Initialize signals
-        rst = 1'b1;
-        cs = 1'b0;
-        rw = 1'b0;
-        addr = 3'h0;
-        data_in = 8'h00;
-        
-        // Apply reset
-        #20;
-        rst = 1'b0;
-        #20;
-        
-        // Test 1: Reset Test
-        $display("Test 1: Reset Test");
-        cpu_read(3'h4, read_data);  // Read STATUS
-        $display("STATUS after reset: 0x%02h (expected: 0x00)", read_data);
-        if (read_data != 8'h00) $display("ERROR: Reset test failed");
-        
-        // Test 2: ADD Test
-        $display("\nTest 2: ADD Test");
-        cpu_write(3'h1, 8'h85);  // ARG1 = NUMBER(5)
-        cpu_write(3'h2, 8'h86);  // ARG2 = NUMBER(6)
-        cpu_write(3'h0, 8'h06);  // OPCODE = ADD
-        
-        cpu_read(3'h3, read_data);  // Read RESULT
-        $display("ADD Result: 0x%02h (expected: 0x8B, NUMBER(11))", read_data);
-        if (read_data != 8'h8B) $display("ERROR: ADD test failed");
-        
-        cpu_read(3'h4, read_data);  // Read STATUS
-        $display("STATUS after ADD: 0x%02h (expected: 0x00, ZERO=0)", read_data);
-        if (read_data != 8'h00) $display("ERROR: ADD status test failed");
-        
-        // Test 3: CONS Test
-        $display("\nTest 3: CONS Test");
-        cpu_write(3'h1, 8'h41);  // ARG1 = ATOM('T')
-        cpu_write(3'h2, 8'h42);  // ARG2 = ATOM('B')
-        cpu_write(3'h0, 8'h01);  // OPCODE = CONS
-        
-        cpu_read(3'h3, read_data);  // Read RESULT
-        $display("CONS Result: 0x%02h (expected: 0xC0, CONS(0))", read_data);
-        if (read_data != 8'hC0) $display("ERROR: CONS test failed");
-        
-        // Test CAR and CDR
-        cpu_write(3'h1, read_data);  // Use the CONS result as ARG1
-        cpu_write(3'h0, 8'h02);      // OPCODE = CAR
-        
-        cpu_read(3'h3, read_data);   // Read RESULT
-        $display("CAR Result: 0x%02h (expected: 0x41, ATOM('T'))", read_data);
-        if (read_data != 8'h41) $display("ERROR: CAR test failed");
-        
-        cpu_write(3'h1, 8'hC0);      // ARG1 = CONS(0)
-        cpu_write(3'h0, 8'h03);      // OPCODE = CDR
-        
-        cpu_read(3'h3, read_data);   // Read RESULT
-        $display("CDR Result: 0x%02h (expected: 0x42, ATOM('B'))", read_data);
-        if (read_data != 8'h42) $display("ERROR: CDR test failed");
-        
-        // Test 4: Error Test (CAR on a Number)
-        $display("\nTest 4: Error Test (CAR on a Number)");
-        cpu_write(3'h1, 8'h85);  // ARG1 = NUMBER(5)
-        cpu_write(3'h0, 8'h02);  // OPCODE = CAR
-        
-        cpu_read(3'h4, read_data);  // Read STATUS
-        $display("STATUS after CAR on Number: 0x%02h (expected: 0x04, ERR_TYPE=1)", read_data);
-        if (read_data != 8'h04) $display("ERROR: Error test failed");
+        $dumpfile("lisp_coproc.vcd");
+        $dumpvars(0, tb_lisp_coproc);
+
+        // Init
+        rst = 1; cs = 0; rw = 0; addr = 0; data_in = 0;
+        #20 rst = 0; #20;
         
-        // Test 5: Heap Full Test
-        $display("\nTest 5: Heap Full Test");
-        // Fill the heap with CONS operations
-        // Since bump_alloc is now 2, we have used 2 cells. 
-        // 16 cells total. 14 left. 7 more CONS possible.
-        for (i = 0; i < 7; i = i + 1) begin 
-            cpu_write(3'h1, 8'h41);  
-            cpu_write(3'h2, 8'h42);  
-            cpu_write(3'h0, 8'h01);  
-            cpu_read(3'h3, read_data); 
+        $display("--- Starting Lisp Coprocessor Verification ---");
+
+        // 1. Reset Test
+        cpu_read(3'h4, read_data);
+        assert_equals(8'h00, read_data, "Reset Status");
+
+        // 2. ADD Test (5 + 6 = 11)
+        $display("\n[Test] ADD Operation");
+        cpu_write(3'h1, 8'h85); // Number(5)
+        cpu_write(3'h2, 8'h86); // Number(6)
+        cpu_write(3'h0, 8'h06); // OPCODE: ADD
+        cpu_read(3'h3, read_data);
+        assert_equals(8'h8B, read_data, "ADD Result (11)");
+        cpu_read(3'h4, read_data);
+        assert_equals(8'h00, read_data, "ADD Status (Clean)");
+
+        // 3. CONS Test
+        $display("\n[Test] CONS Operation");
+        cpu_write(3'h1, 8'h41); // Atom('T')
+        cpu_write(3'h2, 8'h42); // Atom('B')
+        cpu_write(3'h0, 8'h01); // OPCODE: CONS
+        cpu_read(3'h3, read_data);
+        // Expect CONS Tag (11) | Address (0000) -> 0xC0
+        assert_equals(8'hC0, read_data, "CONS Pointer");
+
+        // Verify Heap Persistence via CAR/CDR
+        cpu_write(3'h1, 8'hC0); // Pointer to just allocated cell
+        cpu_write(3'h0, 8'h02); // OPCODE: CAR
+        cpu_read(3'h3, read_data);
+        assert_equals(8'h41, read_data, "CAR Check");
+
+        cpu_write(3'h1, 8'hC0); 
+        cpu_write(3'h0, 8'h03); // OPCODE: CDR
+        cpu_read(3'h3, read_data);
+        assert_equals(8'h42, read_data, "CDR Check");
+
+        // 4. Error Test (Type Error)
+        $display("\n[Test] Error Handling (Type)");
+        cpu_write(3'h1, 8'h85); // Number
+        cpu_write(3'h0, 8'h02); // OPCODE: CAR (Invalid on Number)
+        cpu_read(3'h4, read_data);
+        // Bit 2 (ERR_TYPE) should be set -> 0x04
+        assert_equals(8'h04, read_data, "Error Type Flag");
+
+        // 5. Heap Full Test
+        $display("\n[Test] Heap Full Error");
+        // We used 2 cells (1 CONS). 14 cells remain.
+        // Loop 7 times to fill exactly to 16.
+        for(i=0; i<7; i=i+1) begin
+            cpu_write(3'h1, 8'h41);
+            cpu_write(3'h2, 8'h42);
+            cpu_write(3'h0, 8'h01); // CONS
+            cpu_read(3'h3, read_data); // Sync
         end
         
-        // One more CONS should cause heap full error
-        cpu_write(3'h1, 8'h41); 
-        cpu_write(3'h2, 8'h42); 
-        cpu_write(3'h0, 8'h01); 
-        
-        cpu_read(3'h4, read_data);  // Read STATUS
-        $display("STATUS after Heap Full: 0x%02h (expected: 0x02, ERR_HEAP_FULL=1)", read_data);
-        if (read_data != 8'h02) $display("ERROR: Heap Full test failed");
+        // Try 8th CONS -> Should fail
+        cpu_write(3'h1, 8'hAA);
+        cpu_write(3'h2, 8'hBB);
+        cpu_write(3'h0, 8'h01); // CONS
         
-        $display("\nAll tests completed.");
+        cpu_read(3'h4, read_data);
+        // Bit 1 (ERR_HEAP) should be set -> 0x02
+        assert_equals(8'h02, read_data, "Heap Full Flag");
+
+        $display("\n--- All Tests Passed Successfully ---");
         $finish;
     end
+    
+    // Helper task for reporting
+    task assert_equals(input [7:0] expected, input [7:0] actual, input string name);
+        if (expected !== actual) begin
+            $display("ERROR: %s failed. Expected 0x%h, Got 0x%h", name, expected, actual);
+            $finish;
+        end else begin
+            $display("PASS: %s", name);
+        end
+    endtask
 
 endmodule