`timescale 1ns / 1ps

module testbench;

    // Inputs
    reg clk;
    reg rst;
    reg [15:0] angle_in;
    reg in_valid;

    // Outputs
    wire [15:0] sin_out;
    wire [15:0] cos_out;
    wire out_valid;

    // Test Variables
    integer i;
    integer latency;
    integer fail_count;
    integer pass_count;
    
    // Instantiate the Unit Under Test (UUT)
    cordic_sincos_perf uut (
        .clk(clk), 
        .rst(rst), 
        .angle_in(angle_in), 
        .in_valid(in_valid), 
        .sin_out(sin_out), 
        .cos_out(cos_out), 
        .out_valid(out_valid)
    );

    // Auto-generated golden vectors from generate_golden.py
    // (copy-pasted content will be filled by the include or manual paste)
    // Since I cannot include easily in this environment without creating a file,
    // I will copy-paste the vectors here.
    
    localparam NUM_TESTS = 57;
    reg [15:0] test_angles [0:NUM_TESTS-1];
    reg [15:0] expected_sin [0:NUM_TESTS-1];
    reg [15:0] expected_cos [0:NUM_TESTS-1];
    
    initial begin
        // Paste from generate_golden.py output
        test_angles[0] = 16'h9B7A;
        test_angles[1] = 16'h9F94;
        test_angles[2] = 16'hA3AE;
        test_angles[3] = 16'hA7C9;
        test_angles[4] = 16'hABE3;
        test_angles[5] = 16'hAFFE;
        test_angles[6] = 16'hB418;
        test_angles[7] = 16'hB832;
        test_angles[8] = 16'hBC4D;
        test_angles[9] = 16'hBCFB;
        test_angles[10] = 16'hC067;
        test_angles[11] = 16'hC481;
        test_angles[12] = 16'hC89C;
        test_angles[13] = 16'hCCB6;
        test_angles[14] = 16'hCDBC;
        test_angles[15] = 16'hD0D1;
        test_angles[16] = 16'hD4EB;
        test_angles[17] = 16'hD905;
        test_angles[18] = 16'hDD20;
        test_angles[19] = 16'hDE7D;
        test_angles[20] = 16'hE13A;
        test_angles[21] = 16'hE555;
        test_angles[22] = 16'hE96F;
        test_angles[23] = 16'hED89;
        test_angles[24] = 16'hF1A4;
        test_angles[25] = 16'hF5BE;
        test_angles[26] = 16'hF9D8;
        test_angles[27] = 16'hFDF3;
        test_angles[28] = 16'h0000;
        test_angles[29] = 16'h020D;
        test_angles[30] = 16'h0628;
        test_angles[31] = 16'h0A42;
        test_angles[32] = 16'h0E5C;
        test_angles[33] = 16'h1277;
        test_angles[34] = 16'h1691;
        test_angles[35] = 16'h1AAB;
        test_angles[36] = 16'h1EC6;
        test_angles[37] = 16'h2183;
        test_angles[38] = 16'h22E0;
        test_angles[39] = 16'h26FB;
        test_angles[40] = 16'h2B15;
        test_angles[41] = 16'h2F2F;
        test_angles[42] = 16'h3244;
        test_angles[43] = 16'h334A;
        test_angles[44] = 16'h3764;
        test_angles[45] = 16'h3B7F;
        test_angles[46] = 16'h3F99;
        test_angles[47] = 16'h4305;
        test_angles[48] = 16'h43B3;
        test_angles[49] = 16'h47CE;
        test_angles[50] = 16'h4BE8;
        test_angles[51] = 16'h5002;
        test_angles[52] = 16'h541D;
        test_angles[53] = 16'h5837;
        test_angles[54] = 16'h5C52;
        test_angles[55] = 16'h606C;
        test_angles[56] = 16'h6486;

        expected_sin[0] = 16'h8000;
        expected_sin[1] = 16'h8044;
        expected_sin[2] = 16'h810D;
        expected_sin[3] = 16'h825D;
        expected_sin[4] = 16'h8430;
        expected_sin[5] = 16'h8686;
        expected_sin[6] = 16'h895C;
        expected_sin[7] = 16'h8CAE;
        expected_sin[8] = 16'h907A;
        expected_sin[9] = 16'h9126;
        expected_sin[10] = 16'h94BB;
        expected_sin[11] = 16'h996C;
        expected_sin[12] = 16'h9E8A;
        expected_sin[13] = 16'hA40E;
        expected_sin[14] = 16'hA57D;
        expected_sin[15] = 16'hA9F4;
        expected_sin[16] = 16'hB033;
        expected_sin[17] = 16'hB6C5;
        expected_sin[18] = 16'hBDA7;
        expected_sin[19] = 16'hBFFF;
        expected_sin[20] = 16'hC4CC;
        expected_sin[21] = 16'hCC32;
        expected_sin[22] = 16'hD3CC;
        expected_sin[23] = 16'hDB95;
        expected_sin[24] = 16'hE386;
        expected_sin[25] = 16'hEB92;
        expected_sin[26] = 16'hF3B5;
        expected_sin[27] = 16'hFBE6;
        expected_sin[28] = 16'h0000;
        expected_sin[29] = 16'h041A;
        expected_sin[30] = 16'h0C4B;
        expected_sin[31] = 16'h146E;
        expected_sin[32] = 16'h1C7A;
        expected_sin[33] = 16'h246B;
        expected_sin[34] = 16'h2C34;
        expected_sin[35] = 16'h33CE;
        expected_sin[36] = 16'h3B34;
        expected_sin[37] = 16'h4001;
        expected_sin[38] = 16'h4259;
        expected_sin[39] = 16'h493B;
        expected_sin[40] = 16'h4FCD;
        expected_sin[41] = 16'h560C;
        expected_sin[42] = 16'h5A83;
        expected_sin[43] = 16'h5BF2;
        expected_sin[44] = 16'h6176;
        expected_sin[45] = 16'h6694;
        expected_sin[46] = 16'h6B45;
        expected_sin[47] = 16'h6EDA;
        expected_sin[48] = 16'h6F86;
        expected_sin[49] = 16'h7352;
        expected_sin[50] = 16'h76A4;
        expected_sin[51] = 16'h797A;
        expected_sin[52] = 16'h7BD0;
        expected_sin[53] = 16'h7DA3;
        expected_sin[54] = 16'h7EF3;
        expected_sin[55] = 16'h7FBC;
        expected_sin[56] = 16'h7FFF;

        expected_cos[0] = 16'h0004;
        expected_cos[1] = 16'h0836;
        expected_cos[2] = 16'h1060;
        expected_cos[3] = 16'h187B;
        expected_cos[4] = 16'h207A;
        expected_cos[5] = 16'h2859;
        expected_cos[6] = 16'h300B;
        expected_cos[7] = 16'h378B;
        expected_cos[8] = 16'h3ED2;
        expected_cos[9] = 16'h4000;
        expected_cos[10] = 16'h45D6;
        expected_cos[11] = 16'h4C90;
        expected_cos[12] = 16'h52FB;
        expected_cos[13] = 16'h590D;
        expected_cos[14] = 16'h5A82;
        expected_cos[15] = 16'h5EC3;
        expected_cos[16] = 16'h6414;
        expected_cos[17] = 16'h68FC;
        expected_cos[18] = 16'h6D76;
        expected_cos[19] = 16'h6EDA;
        expected_cos[20] = 16'h717C;
        expected_cos[21] = 16'h750C;
        expected_cos[22] = 16'h7820;
        expected_cos[23] = 16'h7AB6;
        expected_cos[24] = 16'h7CCB;
        expected_cos[25] = 16'h7E5C;
        expected_cos[26] = 16'h7F69;
        expected_cos[27] = 16'h7FEF;
        expected_cos[28] = 16'h7FFF;
        expected_cos[29] = 16'h7FEF;
        expected_cos[30] = 16'h7F69;
        expected_cos[31] = 16'h7E5C;
        expected_cos[32] = 16'h7CCB;
        expected_cos[33] = 16'h7AB6;
        expected_cos[34] = 16'h7820;
        expected_cos[35] = 16'h750C;
        expected_cos[36] = 16'h717C;
        expected_cos[37] = 16'h6EDA;
        expected_cos[38] = 16'h6D76;
        expected_cos[39] = 16'h68FC;
        expected_cos[40] = 16'h6414;
        expected_cos[41] = 16'h5EC3;
        expected_cos[42] = 16'h5A82;
        expected_cos[43] = 16'h590D;
        expected_cos[44] = 16'h52FB;
        expected_cos[45] = 16'h4C90;
        expected_cos[46] = 16'h45D6;
        expected_cos[47] = 16'h4000;
        expected_cos[48] = 16'h3ED2;
        expected_cos[49] = 16'h378B;
        expected_cos[50] = 16'h300B;
        expected_cos[51] = 16'h2859;
        expected_cos[52] = 16'h207A;
        expected_cos[53] = 16'h187B;
        expected_cos[54] = 16'h1060;
        expected_cos[55] = 16'h0836;
        expected_cos[56] = 16'h0004;
    end

    // Clock generation
    initial begin
        clk = 0;
        forever #5 clk = ~clk;
    end

    // Error checking logic
    integer error_sin;
    integer error_cos;
    
    // Latency handling
    reg [15:0] exp_sin_queue [0:127]; // Sufficient depth for pipeline
    reg [15:0] exp_cos_queue [0:127];
    reg [15:0] angle_info_queue [0:127]; // For debugging
    integer q_head, q_tail;
    
    initial begin
        // Initialize Inputs
        rst = 1;
        in_valid = 0;
        angle_in = 0;
        fail_count = 0;
        pass_count = 0;
        q_head = 0;
        q_tail = 0;

        // Reset
        #100;
        @(negedge clk); rst = 0;
        #20;
        
        // --- 1. Latency Detection ---
        $display("Detecting Latency...");
        @(negedge clk);
        angle_in = test_angles[0];
        in_valid = 1;
        @(negedge clk);
        in_valid = 0;
        
        latency = 0;
        while (out_valid === 0 && latency < 100) begin
            @(negedge clk);
            latency = latency + 1;
        end
        
        if (latency >= 100) begin
            $display("ERROR: Timeout waiting for output valid during latency detection");
            $finish;
        end
        $display("Detected Latency: %0d cycles", latency);
        
        // Wait for pipeline to drain
        #100;
        
        // --- 2. Streaming Test ---
        $display("Starting Streaming Test...");
        q_head = 0;
        q_tail = 0;
        
        fork
            // Driver process
            begin
                for (i = 0; i < NUM_TESTS; i = i + 1) begin
                    @(negedge clk);
                    in_valid = 1;
                    angle_in = test_angles[i];
                    
                    // Push expectation
                    exp_sin_queue[q_tail] = expected_sin[i];
                    exp_cos_queue[q_tail] = expected_cos[i];
                    angle_info_queue[q_tail] = test_angles[i];
                    q_tail = (q_tail + 1) % 128;
                end
                
                @(negedge clk);
                in_valid = 0;
            end
            
            // Monitor process
            begin
                while (q_head != NUM_TESTS) begin
                    @(negedge clk); 
                    if (out_valid) begin
                         // Move declarations out or use logic directly
                         // Debug
                         // $display("Monitor: q_head=%0d, ExpSinQ=%h", q_head, exp_sin_queue[q_head]);
                         
                         // Tolerance check (32 LSBs)
                         // Note: diff_sin/diff_cos must be calculated
                         
                         // Calculate signed diff
                         if ($signed(sin_out) > $signed(exp_sin_queue[q_head]))
                            error_sin = $signed(sin_out) - $signed(exp_sin_queue[q_head]);
                         else
                            error_sin = $signed(exp_sin_queue[q_head]) - $signed(sin_out);
                            
                         if ($signed(cos_out) > $signed(exp_cos_queue[q_head]))
                            error_cos = $signed(cos_out) - $signed(exp_cos_queue[q_head]);
                         else
                            error_cos = $signed(exp_cos_queue[q_head]) - $signed(cos_out);

                         if (error_sin <= 32 && error_cos <= 32) begin
                             pass_count = pass_count + 1;
                         end else begin
                             fail_count = fail_count + 1;
                             $display("ERROR: Test failed (Angle: %h)", angle_info_queue[q_head]);
                             $display("  Exp Sin: %h, Got: %h, Diff: %0d", exp_sin_queue[q_head], sin_out, error_sin);
                             $display("  Exp Cos: %h, Got: %h, Diff: %0d", exp_cos_queue[q_head], cos_out, error_cos);
                         end
                         
                         q_head = (q_head + 1) % 128;
                    end
                end
            end
        join
        
        if (fail_count == 0 && pass_count == NUM_TESTS) begin
             $display("TEST_RESULT: PASS");
        end else begin
             $display("TEST_RESULT: FAIL (%0d errors)", fail_count);
        end
        
        $finish;
    end

endmodule