`timescale 1ns / 1ps

module testbench;
    
    // Inputs
    reg clk;
    reg rst;
    reg enable;
    reg signed [15:0] setpoint;
    reg signed [15:0] position;
    reg [7:0] kp;
    reg [7:0] ki;
    reg [7:0] kd;

    // Outputs
    wire signed [15:0] control_out;

    // Instantiate the Unit Under Test (UUT)
    robot_controller uut (
        .clk(clk), 
        .rst(rst), 
        .enable(enable),
        .setpoint(setpoint), 
        .position(position), 
        .kp(kp), 
        .ki(ki), 
        .kd(kd), 
        .control_out(control_out)
    );
    
    // Golden Vectors
    localparam NUM_TESTS = 60;
    reg [15:0] test_setpoint [0:NUM_TESTS-1];
    reg [15:0] test_position [0:NUM_TESTS-1];
    reg [7:0]  test_kp [0:NUM_TESTS-1];
    reg [7:0]  test_ki [0:NUM_TESTS-1];
    reg [7:0]  test_kd [0:NUM_TESTS-1];
    reg        test_rst [0:NUM_TESTS-1];
    reg        test_enable [0:NUM_TESTS-1];
    reg [15:0] expected_out [0:NUM_TESTS-1];

    initial begin
        test_rst[0] = 1;
        test_enable[0] = 0;
        test_setpoint[0] = 16'h0000;
        test_position[0] = 16'h0000;
        test_kp[0] = 8'h00;
        test_ki[0] = 8'h00;
        test_kd[0] = 8'h00;
        expected_out[0] = 16'h0000;
        test_rst[1] = 1;
        test_enable[1] = 1;
        test_setpoint[1] = 16'h0000;
        test_position[1] = 16'h0000;
        test_kp[1] = 8'h00;
        test_ki[1] = 8'h00;
        test_kd[1] = 8'h00;
        expected_out[1] = 16'h0000;
        test_rst[2] = 0;
        test_enable[2] = 1;
        test_setpoint[2] = 16'h6400;
        test_position[2] = 16'h5000;
        test_kp[2] = 8'h80;
        test_ki[2] = 8'h00;
        test_kd[2] = 8'h00;
        expected_out[2] = 16'h0A00;
        test_rst[3] = 1;
        test_enable[3] = 1;
        test_setpoint[3] = 16'h0000;
        test_position[3] = 16'h0000;
        test_kp[3] = 8'h00;
        test_ki[3] = 8'h00;
        test_kd[3] = 8'h00;
        expected_out[3] = 16'h0000;
        test_rst[4] = 0;
        test_enable[4] = 1;
        test_setpoint[4] = 16'hDD29;
        test_position[4] = 16'h1CA8;
        test_kp[4] = 8'h84;
        test_ki[4] = 8'h13;
        test_kd[4] = 8'h76;
        expected_out[4] = 16'hBD46;
        test_rst[5] = 0;
        test_enable[5] = 1;
        test_setpoint[5] = 16'hDD29;
        test_position[5] = 16'h1AFA;
        test_kp[5] = 8'h84;
        test_ki[5] = 8'h13;
        test_kd[5] = 8'h76;
        expected_out[5] = 16'hD798;
        test_rst[6] = 0;
        test_enable[6] = 1;
        test_setpoint[6] = 16'hDD29;
        test_position[6] = 16'h1C13;
        test_kp[6] = 8'h84;
        test_ki[6] = 8'h13;
        test_kd[6] = 8'h76;
        expected_out[6] = 16'hD113;
        test_rst[7] = 0;
        test_enable[7] = 1;
        test_setpoint[7] = 16'hDD29;
        test_position[7] = 16'h1C2F;
        test_kp[7] = 8'h84;
        test_ki[7] = 8'h13;
        test_kd[7] = 8'h76;
        expected_out[7] = 16'hCCCB;
        test_rst[8] = 0;
        test_enable[8] = 1;
        test_setpoint[8] = 16'hDD29;
        test_position[8] = 16'h1A7F;
        test_kp[8] = 8'h84;
        test_ki[8] = 8'h13;
        test_kd[8] = 8'h76;
        expected_out[8] = 16'hC9F0;
        test_rst[9] = 0;
        test_enable[9] = 1;
        test_setpoint[9] = 16'hDD29;
        test_position[9] = 16'h1AA7;
        test_kp[9] = 8'h84;
        test_ki[9] = 8'h13;
        test_kd[9] = 8'h76;
        expected_out[9] = 16'hC471;
        test_rst[10] = 0;
        test_enable[10] = 1;
        test_setpoint[10] = 16'hDD29;
        test_position[10] = 16'h19C0;
        test_kp[10] = 8'h84;
        test_ki[10] = 8'h13;
        test_kd[10] = 8'h76;
        expected_out[10] = 16'hC0E5;
        test_rst[11] = 0;
        test_enable[11] = 1;
        test_setpoint[11] = 16'hDD29;
        test_position[11] = 16'h1A1B;
        test_kp[11] = 8'h84;
        test_ki[11] = 8'h13;
        test_kd[11] = 8'h76;
        expected_out[11] = 16'hBB9C;
        test_rst[12] = 0;
        test_enable[12] = 1;
        test_setpoint[12] = 16'hDD29;
        test_position[12] = 16'h1AF0;
        test_kp[12] = 8'h84;
        test_ki[12] = 8'h13;
        test_kd[12] = 8'h76;
        expected_out[12] = 16'hB65F;
        test_rst[13] = 0;
        test_enable[13] = 1;
        test_setpoint[13] = 16'hDD29;
        test_position[13] = 16'h19B6;
        test_kp[13] = 8'h84;
        test_ki[13] = 8'h13;
        test_kd[13] = 8'h76;
        expected_out[13] = 16'hB375;
        test_rst[14] = 1;
        test_enable[14] = 1;
        test_setpoint[14] = 16'h0000;
        test_position[14] = 16'h0000;
        test_kp[14] = 8'h00;
        test_ki[14] = 8'h00;
        test_kd[14] = 8'h00;
        expected_out[14] = 16'h0000;
        test_rst[15] = 0;
        test_enable[15] = 1;
        test_setpoint[15] = 16'hE98E;
        test_position[15] = 16'h0303;
        test_kp[15] = 8'hFF;
        test_ki[15] = 8'h12;
        test_kd[15] = 8'h59;
        expected_out[15] = 16'hDBFF;
        test_rst[16] = 0;
        test_enable[16] = 1;
        test_setpoint[16] = 16'hE98E;
        test_position[16] = 16'h01FE;
        test_kp[16] = 8'hFF;
        test_ki[16] = 8'h12;
        test_kd[16] = 8'h59;
        expected_out[16] = 16'hE47F;
        test_rst[17] = 0;
        test_enable[17] = 1;
        test_setpoint[17] = 16'hE98E;
        test_position[17] = 16'h014D;
        test_kp[17] = 8'hFF;
        test_ki[17] = 8'h12;
        test_kd[17] = 8'h59;
        expected_out[17] = 16'hE366;
        test_rst[18] = 0;
        test_enable[18] = 1;
        test_setpoint[18] = 16'hE98E;
        test_position[18] = 16'h0102;
        test_kp[18] = 8'hFF;
        test_ki[18] = 8'h12;
        test_kd[18] = 8'h59;
        expected_out[18] = 16'hE1E7;
        test_rst[19] = 0;
        test_enable[19] = 1;
        test_setpoint[19] = 16'hE98E;
        test_position[19] = 16'hFF4E;
        test_kp[19] = 8'hFF;
        test_ki[19] = 8'h12;
        test_kd[19] = 8'h59;
        expected_out[19] = 16'hE28E;
        test_rst[20] = 0;
        test_enable[20] = 1;
        test_setpoint[20] = 16'hE98E;
        test_position[20] = 16'hFFF2;
        test_kp[20] = 8'hFF;
        test_ki[20] = 8'h12;
        test_kd[20] = 8'h59;
        expected_out[20] = 16'hDF86;
        test_rst[21] = 0;
        test_enable[21] = 1;
        test_setpoint[21] = 16'hE98E;
        test_position[21] = 16'hFFD1;
        test_kp[21] = 8'hFF;
        test_ki[21] = 8'h12;
        test_kd[21] = 8'h59;
        expected_out[21] = 16'hDE5B;
        test_rst[22] = 0;
        test_enable[22] = 1;
        test_setpoint[22] = 16'hE98E;
        test_position[22] = 16'h010B;
        test_kp[22] = 8'hFF;
        test_ki[22] = 8'h12;
        test_kd[22] = 8'h59;
        expected_out[22] = 16'hDB02;
        test_rst[23] = 0;
        test_enable[23] = 1;
        test_setpoint[23] = 16'hE98E;
        test_position[23] = 16'h0253;
        test_kp[23] = 8'hFF;
        test_ki[23] = 8'h12;
        test_kd[23] = 8'h59;
        expected_out[23] = 16'hD7F8;
        test_rst[24] = 0;
        test_enable[24] = 1;
        test_setpoint[24] = 16'hE98E;
        test_position[24] = 16'h0442;
        test_kp[24] = 8'hFF;
        test_ki[24] = 8'h12;
        test_kd[24] = 8'h59;
        expected_out[24] = 16'hD3F0;
        test_rst[25] = 1;
        test_enable[25] = 1;
        test_setpoint[25] = 16'h0000;
        test_position[25] = 16'h0000;
        test_kp[25] = 8'h00;
        test_ki[25] = 8'h00;
        test_kd[25] = 8'h00;
        expected_out[25] = 16'h0000;
        test_rst[26] = 0;
        test_enable[26] = 1;
        test_setpoint[26] = 16'h0C32;
        test_position[26] = 16'h1A3F;
        test_kp[26] = 8'hD4;
        test_ki[26] = 8'h14;
        test_kd[26] = 8'h5A;
        expected_out[26] = 16'hEE52;
        test_rst[27] = 0;
        test_enable[27] = 1;
        test_setpoint[27] = 16'h0C32;
        test_position[27] = 16'h1A2C;
        test_kp[27] = 8'hD4;
        test_ki[27] = 8'h14;
        test_kd[27] = 8'h5A;
        expected_out[27] = 16'hF240;
        test_rst[28] = 0;
        test_enable[28] = 1;
        test_setpoint[28] = 16'h0C32;
        test_position[28] = 16'h1A07;
        test_kp[28] = 8'hD4;
        test_ki[28] = 8'h14;
        test_kd[28] = 8'h5A;
        expected_out[28] = 16'hF151;
        test_rst[29] = 0;
        test_enable[29] = 1;
        test_setpoint[29] = 16'h0C32;
        test_position[29] = 16'h1A8E;
        test_kp[29] = 8'hD4;
        test_ki[29] = 8'h14;
        test_kd[29] = 8'h5A;
        expected_out[29] = 16'hEF84;
        test_rst[30] = 0;
        test_enable[30] = 1;
        test_setpoint[30] = 16'h0C32;
        test_position[30] = 16'h1975;
        test_kp[30] = 8'hD4;
        test_ki[30] = 8'h14;
        test_kd[30] = 8'h5A;
        expected_out[30] = 16'hEFF5;
        test_rst[31] = 0;
        test_enable[31] = 1;
        test_setpoint[31] = 16'h0C32;
        test_position[31] = 16'h1AB2;
        test_kp[31] = 8'hD4;
        test_ki[31] = 8'h14;
        test_kd[31] = 8'h5A;
        expected_out[31] = 16'hECFB;
        test_rst[32] = 0;
        test_enable[32] = 1;
        test_setpoint[32] = 16'h0C32;
        test_position[32] = 16'h1BCF;
        test_kp[32] = 8'hD4;
        test_ki[32] = 8'h14;
        test_kd[32] = 8'h5A;
        expected_out[32] = 16'hEAE0;
        test_rst[33] = 0;
        test_enable[33] = 1;
        test_setpoint[33] = 16'h0C32;
        test_position[33] = 16'h1AE9;
        test_kp[33] = 8'hD4;
        test_ki[33] = 8'h14;
        test_kd[33] = 8'h5A;
        expected_out[33] = 16'hEB2D;
        test_rst[34] = 0;
        test_enable[34] = 1;
        test_setpoint[34] = 16'h0C32;
        test_position[34] = 16'h1922;
        test_kp[34] = 8'hD4;
        test_ki[34] = 8'h14;
        test_kd[34] = 8'h5A;
        expected_out[34] = 16'hEBF2;
        test_rst[35] = 0;
        test_enable[35] = 1;
        test_setpoint[35] = 16'h0C32;
        test_position[35] = 16'h18C9;
        test_kp[35] = 8'hD4;
        test_ki[35] = 8'h14;
        test_kd[35] = 8'h5A;
        expected_out[35] = 16'hEABF;
        test_rst[36] = 1;
        test_enable[36] = 1;
        test_setpoint[36] = 16'h0000;
        test_position[36] = 16'h0000;
        test_kp[36] = 8'h00;
        test_ki[36] = 8'h00;
        test_kd[36] = 8'h00;
        expected_out[36] = 16'h0000;
        test_rst[37] = 0;
        test_enable[37] = 1;
        test_setpoint[37] = 16'h21B0;
        test_position[37] = 16'h077E;
        test_kp[37] = 8'hE3;
        test_ki[37] = 8'h28;
        test_kd[37] = 8'hF7;
        expected_out[37] = 16'h3497;
        test_rst[38] = 0;
        test_enable[38] = 1;
        test_setpoint[38] = 16'h21B0;
        test_position[38] = 16'h080C;
        test_kp[38] = 8'hE3;
        test_ki[38] = 8'h28;
        test_kd[38] = 8'hF7;
        expected_out[38] = 16'h1E4A;
        test_rst[39] = 0;
        test_enable[39] = 1;
        test_setpoint[39] = 16'h21B0;
        test_position[39] = 16'h073E;
        test_kp[39] = 8'hE3;
        test_ki[39] = 8'h28;
        test_kd[39] = 8'hF7;
        expected_out[39] = 16'h2472;
        test_rst[40] = 0;
        test_enable[40] = 1;
        test_setpoint[40] = 16'h21B0;
        test_position[40] = 16'h0590;
        test_kp[40] = 8'hE3;
        test_ki[40] = 8'h28;
        test_kd[40] = 8'hF7;
        expected_out[40] = 16'h2B2C;
        test_rst[41] = 0;
        test_enable[41] = 1;
        test_setpoint[41] = 16'h21B0;
        test_position[41] = 16'h0509;
        test_kp[41] = 8'hE3;
        test_ki[41] = 8'h28;
        test_kd[41] = 8'hF7;
        expected_out[41] = 16'h2F02;
        test_rst[42] = 0;
        test_enable[42] = 1;
        test_setpoint[42] = 16'h21B0;
        test_position[42] = 16'h03CB;
        test_kp[42] = 8'hE3;
        test_ki[42] = 8'h28;
        test_kd[42] = 8'hF7;
        expected_out[42] = 16'h3577;
        test_rst[43] = 0;
        test_enable[43] = 1;
        test_setpoint[43] = 16'h21B0;
        test_position[43] = 16'h0597;
        test_kp[43] = 8'hE3;
        test_ki[43] = 8'h28;
        test_kd[43] = 8'hF7;
        expected_out[43] = 16'h3554;
        test_rst[44] = 0;
        test_enable[44] = 1;
        test_setpoint[44] = 16'h21B0;
        test_position[44] = 16'h06E4;
        test_kp[44] = 8'hE3;
        test_ki[44] = 8'h28;
        test_kd[44] = 8'hF7;
        expected_out[44] = 16'h38D5;
        test_rst[45] = 0;
        test_enable[45] = 1;
        test_setpoint[45] = 16'h21B0;
        test_position[45] = 16'h0727;
        test_kp[45] = 8'hE3;
        test_ki[45] = 8'h28;
        test_kd[45] = 8'hF7;
        expected_out[45] = 16'h3DC0;
        test_rst[46] = 0;
        test_enable[46] = 1;
        test_setpoint[46] = 16'h21B0;
        test_position[46] = 16'h065F;
        test_kp[46] = 8'hE3;
        test_ki[46] = 8'h28;
        test_kd[46] = 8'hF7;
        expected_out[46] = 16'h43B6;
        test_rst[47] = 1;
        test_enable[47] = 1;
        test_setpoint[47] = 16'h0000;
        test_position[47] = 16'h0000;
        test_kp[47] = 8'h00;
        test_ki[47] = 8'h00;
        test_kd[47] = 8'h00;
        expected_out[47] = 16'h0000;
        test_rst[48] = 0;
        test_enable[48] = 1;
        test_setpoint[48] = 16'hDDDE;
        test_position[48] = 16'h1413;
        test_kp[48] = 8'h7B;
        test_ki[48] = 8'h20;
        test_kd[48] = 8'hEF;
        expected_out[48] = 16'hAC91;
        test_rst[49] = 0;
        test_enable[49] = 1;
        test_setpoint[49] = 16'hDDDE;
        test_position[49] = 16'h14BF;
        test_kp[49] = 8'h7B;
        test_ki[49] = 8'h20;
        test_kd[49] = 8'hEF;
        expected_out[49] = 16'hD75C;
        test_rst[50] = 0;
        test_enable[50] = 1;
        test_setpoint[50] = 16'hDDDE;
        test_position[50] = 16'h137F;
        test_kp[50] = 8'h7B;
        test_ki[50] = 8'h20;
        test_kd[50] = 8'hEF;
        expected_out[50] = 16'hD30C;
        test_rst[51] = 0;
        test_enable[51] = 1;
        test_setpoint[51] = 16'hDDDE;
        test_position[51] = 16'h11F9;
        test_kp[51] = 8'h7B;
        test_ki[51] = 8'h20;
        test_kd[51] = 8'hEF;
        expected_out[51] = 16'hCD86;
        test_rst[52] = 0;
        test_enable[52] = 1;
        test_setpoint[52] = 16'hDDDE;
        test_position[52] = 16'h1201;
        test_kp[52] = 8'h7B;
        test_ki[52] = 8'h20;
        test_kd[52] = 8'hEF;
        expected_out[52] = 16'hC589;
        test_rst[53] = 0;
        test_enable[53] = 1;
        test_setpoint[53] = 16'hDDDE;
        test_position[53] = 16'h13DA;
        test_kp[53] = 8'h7B;
        test_ki[53] = 8'h20;
        test_kd[53] = 8'hEF;
        expected_out[53] = 16'hBC33;
        test_rst[54] = 0;
        test_enable[54] = 1;
        test_setpoint[54] = 16'hDDDE;
        test_position[54] = 16'h125E;
        test_kp[54] = 8'h7B;
        test_ki[54] = 8'h20;
        test_kd[54] = 8'hEF;
        expected_out[54] = 16'hB976;
        test_rst[55] = 0;
        test_enable[55] = 1;
        test_setpoint[55] = 16'hDDDE;
        test_position[55] = 16'h13BD;
        test_kp[55] = 8'h7B;
        test_ki[55] = 8'h20;
        test_kd[55] = 8'hEF;
        expected_out[55] = 16'hAF67;
        test_rst[56] = 0;
        test_enable[56] = 1;
        test_setpoint[56] = 16'hDDDE;
        test_position[56] = 16'h1468;
        test_kp[56] = 8'h7B;
        test_ki[56] = 8'h20;
        test_kd[56] = 8'hEF;
        expected_out[56] = 16'hA8EB;
        test_rst[57] = 0;
        test_enable[57] = 1;
        test_setpoint[57] = 16'hDDDE;
        test_position[57] = 16'h12F3;
        test_kp[57] = 8'h7B;
        test_ki[57] = 8'h20;
        test_kd[57] = 8'hEF;
        expected_out[57] = 16'hA4F7;
        test_rst[58] = 1;
        test_enable[58] = 1;
        test_setpoint[58] = 16'h0000;
        test_position[58] = 16'h0000;
        test_kp[58] = 8'h00;
        test_ki[58] = 8'h00;
        test_kd[58] = 8'h00;
        expected_out[58] = 16'h0000;
        test_rst[59] = 0;
        test_enable[59] = 1;
        test_setpoint[59] = 16'h7530;
        test_position[59] = 16'h8AD0;
        test_kp[59] = 8'hFF;
        test_ki[59] = 8'h00;
        test_kd[59] = 8'h00;
        expected_out[59] = 16'h7FFF;
    end

    // Statistics
    integer passed = 0;
    integer failed = 0;
    integer i;

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

    // Test execution
    initial begin
        // Initialize Inputs
        rst = 1;
        enable = 0;
        setpoint = 0;
        position = 0;
        kp = 0;
        ki = 0;
        kd = 0;

        // Wait for global reset
        #100;
        
        $display("Starting Robot Controller (PID) Testbench...");
        $display("Total vectors: %0d", NUM_TESTS);

        // Iterate through all test vectors
        for (i = 0; i < NUM_TESTS; i = i + 1) begin
            // Apply inputs
            @(negedge clk);
            rst = test_rst[i];
            enable = test_enable[i];
            setpoint = test_setpoint[i];
            position = test_position[i];
            kp = test_kp[i];
            ki = test_ki[i];
            kd = test_kd[i];
            
            // Wait for processing
            @(posedge clk);
            #1; // Wait a tiny bit for outputs to update after clock edge
            
            // Check output
            // Allow small error tolerance if needed, but fixed-point should match exactly 
            // if implementation details align perfectly.
            // The Python model uses integer arithmetic with truncations matching the spec.
            if (control_out !== expected_out[i]) begin
                $display("ERROR: Test %0d failed:", i);
                $display("  Inputs: rst=%b en=%b SP=%d Pos=%d Kp=%d Ki=%d Kd=%d", 
                        rst, enable, $signed(setpoint), $signed(position), kp, ki, kd);
                $display("  Expected: %d (0x%h)", $signed(expected_out[i]), expected_out[i]);
                $display("  Actual:   %d (0x%h)", $signed(control_out), control_out);
                failed = failed + 1;
            end else begin
                passed = passed + 1;
            end
        end

        // Final Report
        $display("--------------------------------");
        $display("Testbench Completed");
        $display("Vectors Passed: %0d", passed);
        $display("Vectors Failed: %0d", failed);
        
        if (failed == 0)
            $display("TEST_RESULT: PASS");
        else
            $display("TEST_RESULT: FAIL (%0d errors)", failed);
            
        $finish;
    end
      
endmodule