// ---------------------------------------------------------------------------
// safety_interlock.sv  —  QX-250 FPGA blocks B6 + B9 (arm/failsafe + watchdog)
// FPGA-QX250-001 · traces REQ-SYS-004, REQ-REG-002 · MADe F7 mitigation
//
// The hardware safety gate. Motor outputs are forced to DISARMED (all zero =
// DShot "0") unless the system is ARMED, and it can only be ARMED while ALL of:
//     * arm_request   — MCU commanded arm (B7 CONTROL.b0)
//     * heartbeat fresh — MCU refreshed HEARTBEAT within the watchdog window (B9)
//     * rc_valid      — RC link is up
//   AND it was armed from an idle-throttle state. ANY condition loss disarms
// immediately and latches a lockout so the system cannot silently re-arm when a
// flickering link/heartbeat recovers — the operator must command disarm first.
//
// SAFETY INVARIANT (formal-ready, asserted in the TB):
//     (armed == 0)  ->  (all motor_out == 0)
// The motor gate is combinational from `armed`, so this holds by construction.
//
// B9 also drives the status LED/buzzer. Logic is a 1:1 transcription of the
// validated /tmp/b6_model.py.
// ---------------------------------------------------------------------------
`timescale 1ns/1ps
`default_nettype none

module safety_interlock #(
    parameter int HEARTBEAT_TIMEOUT = 4_800_000, // clk cycles (~100 ms @48 MHz)
    parameter int IDLE_THRESH       = 48,        // throttle < this == idle
    parameter int BLINK_MSB         = 24         // status blink rate (counter bit)
) (
    input  wire        clk,
    input  wire        rst_n,

    // control / status inputs
    input  wire        arm_request,      // B7 CONTROL.b0
    input  wire        heartbeat_strobe, // B7: MCU wrote HEARTBEAT (0x49)
    input  wire        rc_valid,         // RC link up (async-safe, synced here)

    // motor setpoints in (from B7), gated motor commands out (to B1)
    input  wire [15:0] motor_in1, motor_in2, motor_in3, motor_in4,
    output wire [15:0] motor_out1, motor_out2, motor_out3, motor_out4,

    // status
    output reg         armed,
    output reg         heartbeat_fresh,
    output reg  [7:0]  fault_flags,      // b0 hb_lost b1 rc_lost b2 fault_disarm b3 arm_blocked
    output wire        led,
    output wire        buzzer
);
    // ---- rc_valid synchronizer (async input) -------------------------------
    reg rc_m, rc_s;
    always_ff @(posedge clk or negedge rst_n) begin
        if (!rst_n) {rc_m, rc_s} <= 2'b00;
        else        {rc_m, rc_s} <= {rc_valid, rc_m};
    end

    // ---- B9 watchdog -------------------------------------------------------
    reg [31:0] wd_cnt;
    always_ff @(posedge clk or negedge rst_n) begin
        if (!rst_n) begin
            wd_cnt <= HEARTBEAT_TIMEOUT[31:0];
            heartbeat_fresh <= 1'b0;         // not fresh until first heartbeat
        end else if (heartbeat_strobe) begin
            wd_cnt <= 32'd0;
            heartbeat_fresh <= 1'b1;
        end else if (wd_cnt >= HEARTBEAT_TIMEOUT - 1) begin
            heartbeat_fresh <= 1'b0;
        end else begin
            wd_cnt <= wd_cnt + 32'd1;
        end
    end

    // ---- arming conditions -------------------------------------------------
    wire cond_ok = heartbeat_fresh & rc_s;
    wire idle    = (motor_in1[10:0] < IDLE_THRESH) &
                   (motor_in2[10:0] < IDLE_THRESH) &
                   (motor_in3[10:0] < IDLE_THRESH) &
                   (motor_in4[10:0] < IDLE_THRESH);

    // ---- B6 arm/disarm FSM -------------------------------------------------
    reg lockout;
    always_ff @(posedge clk or negedge rst_n) begin
        if (!rst_n) begin
            armed <= 1'b0; lockout <= 1'b0; fault_flags <= 8'd0;
        end else begin
            if (armed) begin
                if (!arm_request) begin
                    armed <= 1'b0;                    // normal disarm
                end else if (!cond_ok) begin
                    armed   <= 1'b0;                  // fault disarm
                    lockout <= 1'b1;
                    fault_flags[2] <= 1'b1;           // disarmed_by_fault
                    if (!heartbeat_fresh) fault_flags[0] <= 1'b1;
                    if (!rc_s)            fault_flags[1] <= 1'b1;
                end
            end else begin
                if (!arm_request)
                    lockout <= 1'b0;                  // disarm cmd clears lockout
                if (arm_request & cond_ok & idle & ~lockout) begin
                    armed       <= 1'b1;              // ARM
                    fault_flags <= 8'd0;              // clear on successful arm
                end else if (arm_request & cond_ok & ~idle) begin
                    fault_flags[3] <= 1'b1;           // arm blocked: throttle hot
                end
            end
        end
    end

    // ---- motor gate (combinational from armed → invariant holds) -----------
    assign motor_out1 = armed ? motor_in1 : 16'd0;
    assign motor_out2 = armed ? motor_in2 : 16'd0;
    assign motor_out3 = armed ? motor_in3 : 16'd0;
    assign motor_out4 = armed ? motor_in4 : 16'd0;

    // ---- B9 status indicators ----------------------------------------------
    reg [31:0] blink;
    always_ff @(posedge clk or negedge rst_n) begin
        if (!rst_n) blink <= 32'd0;
        else        blink <= blink + 32'd1;
    end
    wire blink_slow = blink[BLINK_MSB];
    wire blink_fast = blink[BLINK_MSB-2];
    // LED: solid armed / slow blink ready-to-arm / fast blink on latched fault
    assign led    = fault_flags[2] ? blink_fast :
                    armed          ? 1'b1        :
                    (cond_ok & ~lockout) ? blink_slow : 1'b0;
    assign buzzer = fault_flags[2] ? blink_fast : 1'b0;

endmodule

`default_nettype wire