module fifo_2w2r (
    input  wire             clk,
    input  wire             rst,
    input  wire [WIDTH-1:0] a1_data,
    input  wire             a1_valid,
    input  wire [WIDTH-1:0] a0_data,
    input  wire             a0_valid,
    output wire             a_ready,
    output wire             a_almost_full,
    output wire             a_full,
    output wire [WIDTH-1:0] b1_data,
    output wire             b1_valid,
    input  wire             b1_ready,
    output wire [WIDTH-1:0] b0_data,
    output wire             b0_valid,
    input  wire             b0_ready
);

    parameter WIDTH = 64;
    parameter ABITS = 2;
    parameter DEPTH = 4;
    //localparam DEPTH = (1 << ABITS);

    reg [WIDTH-1:0] fifo [0:DEPTH-1];
    reg [ABITS:0] fifo_level;
    reg [ABITS-1:0] wr_ptr;
    reg [ABITS-1:0] rd_ptr;

    wire a0_active = a_ready && a0_valid;
    wire a1_active = a_ready && a1_valid;
    wire b0_active = b0_ready && b0_valid;
    wire b1_active = b1_ready && b1_valid;

    wire a_active = a0_active || a1_active;
    wire a_both_active = a0_active && a1_active;
    wire b_active = b0_active || b1_active;
    wire b_both_active = b0_active && b1_active;

    always @(posedge clk) begin
        if (b1_active && !b0_active) begin
            $display("Invalid FIFO input, taking data OoO");
        end
    end

    wire fifo_empty = fifo_level == 0;
    wire fifo_almost_empty = fifo_level == 1;
    wire fifo_almost_full = fifo_level >= DEPTH - 3;
    wire fifo_full = fifo_level >= DEPTH - 1;

    wire [1:0] incoming_count = (a_both_active) ? 2 : (a_active) ? 1 : 0;
    wire [1:0] outgoing_count = (b_both_active) ? 2 : (b_active) ? 1 : 0;

    wire [ABITS-1:0] wr_ptr_plus_2 = (wr_ptr + 2 >= DEPTH) ?
            (wr_ptr + 2 - DEPTH) : (wr_ptr + 2);
    wire [ABITS-1:0] wr_ptr_plus_1 = (wr_ptr + 1 >= DEPTH) ?
            (wr_ptr + 1 - DEPTH) : (wr_ptr + 1);
    wire [ABITS-1:0] rd_ptr_plus_2 = (rd_ptr + 2 >= DEPTH) ?
            (rd_ptr + 2 - DEPTH) : (rd_ptr + 2);
    wire [ABITS-1:0] rd_ptr_plus_1 = (rd_ptr + 1 >= DEPTH) ?
            (rd_ptr + 1 - DEPTH) : (rd_ptr + 1);

    always @(posedge clk) begin
        // Data in
        if (a_active) begin
            if (a_both_active) begin
                fifo[wr_ptr] <= a0_data[WIDTH-1:0];
                fifo[wr_ptr_plus_1] <= a1_data[WIDTH-1:0];
            end
            else if (a1_active) begin
                fifo[wr_ptr] <= a1_data[WIDTH-1:0];
            end
            else if (a0_active) begin
                fifo[wr_ptr] <= a0_data[WIDTH-1:0];
            end
        end

        // Level update
        fifo_level <= fifo_level + incoming_count - outgoing_count;

        // Pointer update
        if (a_active)
            wr_ptr <= (a_both_active) ? wr_ptr_plus_2 : wr_ptr_plus_1;
        if (b_active)
            rd_ptr <= (b_both_active) ? rd_ptr_plus_2 : rd_ptr_plus_1;
        
        if (rst) begin
            wr_ptr <= 0;
            rd_ptr <= 0;
            fifo_level <= 0;
        end
    end
    assign b0_valid = !fifo_empty;
    assign b1_valid = !fifo_almost_empty;
    assign b0_data = fifo[rd_ptr];
    assign b1_data = fifo[rd_ptr_plus_1];
    assign a_ready = !fifo_full;
    assign a_almost_full = fifo_almost_full;
    assign a_full = fifo_full;


endmodule