// =============================================================================
// kalman_top.sv : Top-level dual Kalman filter (roll + pitch)
// -----------------------------------------------------------------------------
// Two kalman_2x2 cores sharing din_valid; identical FSMs run in lock-step,
// so dout_valid pulses align.
// =============================================================================
`default_nettype none

module kalman_top #(
    parameter signed [31:0] DT_Q725    = 32'sd33554,
    parameter signed [31:0] Q00_Q725   = 32'sd33554,
    parameter signed [31:0] Q11_Q725   = 32'sd100663,
    parameter signed [31:0] R00_Q725   = 32'sd1006633,
    parameter signed [31:0] R11_Q725   = 32'sd1006633,
    parameter signed [31:0] P0_00_Q725 = 32'sd33554432,
    parameter signed [31:0] P0_11_Q725 = 32'sd33554432
) (
    input  wire               clk,
    input  wire               rst_n,

    input  wire               din_valid,
    input  wire signed [15:0] roll_in,
    input  wire signed [15:0] pitch_in,
    input  wire signed [15:0] gx_in,
    input  wire signed [15:0] gy_in,

    output wire signed [15:0] est_roll,
    output wire signed [15:0] est_pitch,
    output wire               dout_valid,
    output wire               busy
);

    wire roll_dvalid, pitch_dvalid;
    wire roll_busy,   pitch_busy;

    assign busy       = roll_busy | pitch_busy;
    assign dout_valid = roll_dvalid & pitch_dvalid;

    kalman_2x2 #(
        .DT_Q725(DT_Q725),
        .Q00_Q725(Q00_Q725), .Q11_Q725(Q11_Q725),
        .R00_Q725(R00_Q725), .R11_Q725(R11_Q725),
        .P0_00_Q725(P0_00_Q725), .P0_11_Q725(P0_11_Q725)
    ) u_roll (
        .clk(clk), .rst_n(rst_n),
        .din_valid(din_valid & ~busy),
        .ang_meas_q9 (roll_in),
        .gyro_meas_q9(gx_in),
        .est_angle_q9(est_roll),
        .dout_valid  (roll_dvalid),
        .busy        (roll_busy)
    );

    kalman_2x2 #(
        .DT_Q725(DT_Q725),
        .Q00_Q725(Q00_Q725), .Q11_Q725(Q11_Q725),
        .R00_Q725(R00_Q725), .R11_Q725(R11_Q725),
        .P0_00_Q725(P0_00_Q725), .P0_11_Q725(P0_11_Q725)
    ) u_pitch (
        .clk(clk), .rst_n(rst_n),
        .din_valid(din_valid & ~busy),
        .ang_meas_q9 (pitch_in),
        .gyro_meas_q9(gy_in),
        .est_angle_q9(est_pitch),
        .dout_valid  (pitch_dvalid),
        .busy        (pitch_busy)
    );

endmodule

`default_nettype wire