`timescale 1ns / 1ps

module testbench;

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

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

    // Test Variables
    integer i;
    integer pass_count;
    integer fail_count;
    
    // Signed calculations for error checking
    integer expected_sin_signed;
    integer expected_cos_signed;
    integer actual_sin_signed;
    integer actual_cos_signed;
    integer error_sin;
    integer error_cos;
    integer wait_cycles;

    localparam ERROR_TOL = 24;     // ~0.00073 in Q1.15
    localparam MAX_LATENCY = 40;   // cycles allowed from start to done

    // Instantiate the Unit Under Test (UUT)
    cordic_sincos uut (
        .clk(clk), 
        .rst(rst), 
        .start(start), 
        .angle_in(angle_in), 
        .sin_out(sin_out), 
        .cos_out(cos_out), 
        .done(done)
    );

    // Auto-generated golden vectors: 57 test cases
    // Format: angle (Q2.14), expected_sin (Q1.15), expected_cos (Q1.15)
    localparam NUM_TESTS = 57;

    // Input angles (Q2.14 radians)
    reg [15:0] test_angles [0:NUM_TESTS-1];
    initial begin
        test_angles[0] = 16'h9B7A; // -1.570679 rad
        test_angles[1] = 16'h9F94; // -1.506592 rad
        test_angles[2] = 16'hA3AE; // -1.442505 rad
        test_angles[3] = 16'hA7C9; // -1.378357 rad
        test_angles[4] = 16'hABE3; // -1.314270 rad
        test_angles[5] = 16'hAFFE; // -1.250122 rad
        test_angles[6] = 16'hB418; // -1.186035 rad
        test_angles[7] = 16'hB832; // -1.121948 rad
        test_angles[8] = 16'hBC4D; // -1.057800 rad
        test_angles[9] = 16'hBCFB; // -1.047180 rad
        test_angles[10] = 16'hC067; // -0.993713 rad
        test_angles[11] = 16'hC481; // -0.929626 rad
        test_angles[12] = 16'hC89C; // -0.865479 rad
        test_angles[13] = 16'hCCB6; // -0.801392 rad
        test_angles[14] = 16'hCDBC; // -0.785400 rad
        test_angles[15] = 16'hD0D1; // -0.737244 rad
        test_angles[16] = 16'hD4EB; // -0.673157 rad
        test_angles[17] = 16'hD905; // -0.609070 rad
        test_angles[18] = 16'hDD20; // -0.544922 rad
        test_angles[19] = 16'hDE7D; // -0.523621 rad
        test_angles[20] = 16'hE13A; // -0.480835 rad
        test_angles[21] = 16'hE555; // -0.416687 rad
        test_angles[22] = 16'hE96F; // -0.352600 rad
        test_angles[23] = 16'hED89; // -0.288513 rad
        test_angles[24] = 16'hF1A4; // -0.224365 rad
        test_angles[25] = 16'hF5BE; // -0.160278 rad
        test_angles[26] = 16'hF9D8; // -0.096191 rad
        test_angles[27] = 16'hFDF3; // -0.032043 rad
        test_angles[28] = 16'h0000; // 0.000000 rad
        test_angles[29] = 16'h020D; // 0.032043 rad
        test_angles[30] = 16'h0628; // 0.096191 rad
        test_angles[31] = 16'h0A42; // 0.160278 rad
        test_angles[32] = 16'h0E5C; // 0.224365 rad
        test_angles[33] = 16'h1277; // 0.288513 rad
        test_angles[34] = 16'h1691; // 0.352600 rad
        test_angles[35] = 16'h1AAB; // 0.416687 rad
        test_angles[36] = 16'h1EC6; // 0.480835 rad
        test_angles[37] = 16'h2183; // 0.523621 rad
        test_angles[38] = 16'h22E0; // 0.544922 rad
        test_angles[39] = 16'h26FB; // 0.609070 rad
        test_angles[40] = 16'h2B15; // 0.673157 rad
        test_angles[41] = 16'h2F2F; // 0.737244 rad
        test_angles[42] = 16'h3244; // 0.785400 rad
        test_angles[43] = 16'h334A; // 0.801392 rad
        test_angles[44] = 16'h3764; // 0.865479 rad
        test_angles[45] = 16'h3B7F; // 0.929626 rad
        test_angles[46] = 16'h3F99; // 0.993713 rad
        test_angles[47] = 16'h4305; // 1.047180 rad
        test_angles[48] = 16'h43B3; // 1.057800 rad
        test_angles[49] = 16'h47CE; // 1.121948 rad
        test_angles[50] = 16'h4BE8; // 1.186035 rad
        test_angles[51] = 16'h5002; // 1.250122 rad
        test_angles[52] = 16'h541D; // 1.314270 rad
        test_angles[53] = 16'h5837; // 1.378357 rad
        test_angles[54] = 16'h5C52; // 1.442505 rad
        test_angles[55] = 16'h606C; // 1.506592 rad
        test_angles[56] = 16'h6486; // 1.570679 rad
    end

    // Expected sin values (Q1.15)
    reg [15:0] expected_sin [0:NUM_TESTS-1];
    initial begin
        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;
    end

    // Expected cos values (Q1.15)
    reg [15:0] expected_cos [0:NUM_TESTS-1];
    initial begin
        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

    // Test sequence
    initial begin
        // Initialize Inputs
        rst = 1;
        start = 0;
        angle_in = 0;
        pass_count = 0;
        fail_count = 0;

        // Wait 100 ns for global reset to finish
        #100;
        rst = 0;
        #20;
        
        // Loop over test cases
        for (i = 0; i < NUM_TESTS; i = i + 1) begin
            // Wait if DUT still holds done high (should be one cycle)
            @(posedge clk);
            wait_cycles = 0;
            while (done === 1'b1 && wait_cycles < MAX_LATENCY) begin
                @(posedge clk);
                wait_cycles = wait_cycles + 1;
            end
            if (done === 1'b1) begin
                fail_count = fail_count + 1;
                $display("ERROR: Test %0d done stuck high before start", i);
            end else begin
                angle_in = test_angles[i];
                start = 1;
                
                @(posedge clk);
                start = 0;
                
                // Wait for done with timeout
                wait_cycles = 0;
                while (done !== 1'b1 && wait_cycles < MAX_LATENCY) begin
                    @(posedge clk);
                    wait_cycles = wait_cycles + 1;
                end

                if (done !== 1'b1) begin
                    fail_count = fail_count + 1;
                    $display("ERROR: Test %0d timeout waiting for done (Angle: %h)", i, test_angles[i]);
                end else begin
                    #1; // sample outputs while done is asserted

                    // Check results against tolerance
                    actual_sin_signed = $signed(sin_out);
                    actual_cos_signed = $signed(cos_out);
                    expected_sin_signed = $signed(expected_sin[i]);
                    expected_cos_signed = $signed(expected_cos[i]);
                    
                    error_sin = actual_sin_signed - expected_sin_signed;
                    if (error_sin < 0) error_sin = -error_sin;
                    
                    error_cos = actual_cos_signed - expected_cos_signed;
                    if (error_cos < 0) error_cos = -error_cos;
                    
                    if (error_sin <= ERROR_TOL && error_cos <= ERROR_TOL) begin
                        pass_count = pass_count + 1;
                    end else begin
                        fail_count = fail_count + 1;
                        $display("ERROR: Test %0d failed (Angle: %h)", i, test_angles[i]);
                        $display("  Expected Sin: %h, Got: %h, Diff: %0d", expected_sin[i], sin_out, error_sin);
                        $display("  Expected Cos: %h, Got: %h, Diff: %0d", expected_cos[i], cos_out, error_cos);
                    end
                end
            end
            
            // Wait a few cycles before next
            #20;
        end
        
        if (fail_count == 0) begin
            $display("TEST_RESULT: PASS");
        end else begin
            $display("TEST_RESULT: FAIL (%0d errors)", fail_count);
        end
        
        $finish;
    end
      
endmodule