module robot_controller (
    input clk,
    input rst,
    input enable,
    input signed [15:0] setpoint,
    input signed [15:0] position,
    input [7:0] kp,
    input [7:0] ki,
    input [7:0] kd,
    output reg signed [15:0] control_out
);

    // Internal signals - Expanded for precision
    // Error can be range [-32768 - 32767, 32767 - (-32768)] = [-65535, +65535] -> 17 bits signed
    reg signed [16:0] error;
    reg signed [16:0] prev_error;
    reg signed [15:0] integral; // State variable (saturated)

    // Constants for saturation (Q8.8 range)
    localparam MAX_VAL = 16'sh7FFF; // +32767
    localparam MIN_VAL = 16'sh8000; // -32768

    // Calculate error
    always @(*) begin
        // Sign extend operands to 17 bits to prevent overflow
        error = {setpoint[15], setpoint} - {position[15], position};
    end

    // Multipliers
    // Error(17) * Gain(9, unsigned extended) -> 26 bits
    // ErrorDiff(18) * Gain(9) -> 27 bits
    wire signed [25:0] p_prod;
    wire signed [25:0] i_prod;
    wire signed [26:0] d_prod;

    assign p_prod = error * $signed({1'b0, kp});
    assign i_prod = error * $signed({1'b0, ki});
    
    // Derivative term: (error - prev_error) * kd
    // (error - prev_error) can be 18 bits.
    // Explicitly extend operands to 18 bits before subtraction
    wire signed [17:0] error_diff;
    assign error_diff = {error[16], error} - {prev_error[16], prev_error};
    assign d_prod = error_diff * $signed({1'b0, kd});

    // Scaled terms (Shift right by 8)
    // p_prod >> 8 -> 18 bits (26-8). 
    // i_prod >> 8 -> 18 bits.
    // d_prod >> 8 -> 19 bits (27-8).
    wire signed [17:0] p_term;
    wire signed [17:0] i_increment;
    wire signed [18:0] d_term;

    assign p_term = p_prod >>> 8;
    assign i_increment = i_prod >>> 8;
    assign d_term = d_prod >>> 8;

    // Integral Update
    // integral(16) + i_increment(18) -> 19 bits
    wire signed [18:0] integral_next;
    // Sign extend integral to match i_increment width
    assign integral_next = {{3{integral[15]}}, integral} + {i_increment[17], i_increment};

    // Saturated Integral for sum
    // Use a wire for next state logic
    wire signed [15:0] integral_saturated;
    
    // Saturation logic for integral
    function signed [15:0] saturate_integral (input signed [18:0] val);
        begin
            if (val > MAX_VAL)
                saturate_integral = MAX_VAL;
            else if (val < MIN_VAL)
                saturate_integral = MIN_VAL;
            else
                saturate_integral = val[15:0];
        end
    endfunction
    
    assign integral_saturated = saturate_integral(integral_next);
    
    // Output Sum Calculation
    // p_term(18) + integral_saturated(16) + d_term(19) -> 21 bits roughly
    wire signed [20:0] sum_extended;
    
    // Sign extend all terms to 21 bits
    assign sum_extended = {{3{p_term[17]}}, p_term} + 
                          {{5{integral_saturated[15]}}, integral_saturated} + 
                          {{2{d_term[18]}}, d_term};
    
    // Main sequential block
    always @(posedge clk) begin
        if (rst) begin
            integral <= 0;
            prev_error <= 0;
            control_out <= 0;
        end else if (enable) begin
            // Update integral with saturation
            integral <= integral_saturated;
            
            // Update previous error
            prev_error <= error;
            
            // Update control output with saturation
            if (sum_extended > MAX_VAL)
                control_out <= MAX_VAL;
            else if (sum_extended < MIN_VAL)
                control_out <= MIN_VAL;
            else
                control_out <= sum_extended[15:0];
        end
    end

endmodule