module axi4l_reg_map #(
  parameter int unsigned ADDR_W = 16,
  parameter int unsigned DATA_W = 32,
  parameter int unsigned USER_W = 1,
  parameter int unsigned N_REGS = 4,

  parameter logic [N_REGS-1:0][31:0][2:0] REG_MODE = '{default:'0},
  parameter logic [N_REGS-1:0][31:0]       REG_RST  = '{default:'0}
)(
  input  logic clk,
  input  logic rst_n,
  axi4l_if.slave s_axil,

  input  logic [N_REGS-1:0][31:0] reg_i,
  output logic [N_REGS-1:0][31:0] reg_o
);
  import axi_pkg::*;

  typedef enum logic [2:0] {
    REG_BIT_RSVD = 3'd0,
    REG_BIT_RO   = 3'd1,
    REG_BIT_RW   = 3'd2,
    REG_BIT_W1S  = 3'd3,
    REG_BIT_W1C  = 3'd4
  } reg_bit_mode_t;

  localparam int unsigned STRB_W = DATA_W/8;
  localparam int unsigned ADDR_LSB = $clog2(DATA_W/8);
  localparam int unsigned REG_INDEX_W = (N_REGS <= 1) ? 1 : $clog2(N_REGS);

  logic [ADDR_W-1:0] awaddr_q;
  logic              aw_seen_q;
  logic [DATA_W-1:0] wdata_q;
  logic [STRB_W-1:0] wstrb_q;
  logic              w_seen_q;

  logic              bvalid_q;
  logic [1:0]        bresp_q;

  logic              rvalid_q;
  logic [1:0]        rresp_q;
  logic [DATA_W-1:0] rdata_q;

  logic [REG_INDEX_W-1:0] wr_idx;
  logic [REG_INDEX_W-1:0] rd_idx;
  logic                    wr_addr_valid;
  logic                    rd_addr_valid;

  integer b;
  logic [31:0] wr_mask;
  logic [31:0] wr_data32;
  logic [31:0] rw_cur;
  logic [31:0] rw_new;
  logic [31:0] rd_word;

  always_comb begin
    wr_idx        = '0;
    rd_idx        = '0;
    wr_addr_valid = 1'b0;
    rd_addr_valid = 1'b0;

    if (awaddr_q[ADDR_LSB + REG_INDEX_W - 1 -: REG_INDEX_W] < N_REGS) begin
      wr_idx        = awaddr_q[ADDR_LSB + REG_INDEX_W - 1 -: REG_INDEX_W];
      wr_addr_valid = 1'b1;
    end

    if (s_axil.req.ar.addr[ADDR_LSB + REG_INDEX_W - 1 -: REG_INDEX_W] < N_REGS) begin
      rd_idx        = s_axil.req.ar.addr[ADDR_LSB + REG_INDEX_W - 1 -: REG_INDEX_W];
      rd_addr_valid = 1'b1;
    end
  end

  always_comb begin
    wr_mask = '0;
    for (int k = 0; k < STRB_W; k++) begin
      wr_mask[k*8 +: 8] = {8{wstrb_q[k]}};
    end
    wr_data32 = wdata_q[31:0];
  end

  assign s_axil.resp.aw_ready = !aw_seen_q && !bvalid_q;
  assign s_axil.resp.w_ready  = !w_seen_q  && !bvalid_q;
  assign s_axil.resp.ar_ready = !rvalid_q;

  assign s_axil.resp.b.valid  = bvalid_q;
  assign s_axil.resp.b.resp   = axi_resp_t'(bresp_q);
  assign s_axil.resp.b.user   = '0;

  assign s_axil.resp.r.valid  = rvalid_q;
  assign s_axil.resp.r.resp   = axi_resp_t'(rresp_q);
  assign s_axil.resp.r.data   = rdata_q;
  assign s_axil.resp.r.user   = '0;

  always_ff @(posedge clk or negedge rst_n) begin
    if (!rst_n) begin
      awaddr_q   <= '0;
      aw_seen_q  <= 1'b0;
      wdata_q    <= '0;
      wstrb_q    <= '0;
      w_seen_q   <= 1'b0;
      bvalid_q   <= 1'b0;
      bresp_q    <= 2'b00;
      rvalid_q   <= 1'b0;
      rresp_q    <= 2'b00;
      rdata_q    <= '0;
      reg_o      <= REG_RST;
    end else begin
      for (int r = 0; r < N_REGS; r++) begin
        for (int bit_idx = 0; bit_idx < 32; bit_idx++) begin
          if (reg_bit_mode_t'(REG_MODE[r][bit_idx]) == REG_BIT_W1S)
            reg_o[r][bit_idx] <= 1'b0;
        end
      end

      if (s_axil.req.aw.valid && s_axil.resp.aw_ready) begin
        awaddr_q  <= s_axil.req.aw.addr;
        aw_seen_q <= 1'b1;
      end

      if (s_axil.req.w.valid && s_axil.resp.w_ready) begin
        wdata_q   <= s_axil.req.w.data;
        wstrb_q   <= s_axil.req.w.strb;
        w_seen_q  <= 1'b1;
      end

      if (aw_seen_q && w_seen_q && !bvalid_q) begin
        bvalid_q <= 1'b1;
        bresp_q  <= 2'b00;

        if (!wr_addr_valid) begin
          bresp_q <= 2'b10;
        end else begin
          rw_cur = reg_o[wr_idx];
          rw_new = rw_cur;

          for (b = 0; b < 32; b = b + 1) begin
            if (wr_mask[b]) begin
              unique case (reg_bit_mode_t'(REG_MODE[wr_idx][b]))
                REG_BIT_RSVD: begin end
                REG_BIT_RO  : begin bresp_q <= 2'b10; end
                REG_BIT_RW  : rw_new[b] = wr_data32[b];
                REG_BIT_W1S : if (wr_data32[b]) rw_new[b] = 1'b1;
                REG_BIT_W1C : if (wr_data32[b]) rw_new[b] = 1'b0;
                default     : begin end
              endcase
            end
          end

          reg_o[wr_idx] <= rw_new;
        end

        aw_seen_q <= 1'b0;
        w_seen_q  <= 1'b0;
      end

      if (bvalid_q && s_axil.req.b_ready) begin
        bvalid_q <= 1'b0;
      end

      if (s_axil.req.ar.valid && s_axil.resp.ar_ready) begin
        rvalid_q <= 1'b1;
        rresp_q  <= 2'b00;
        rd_word  = '0;

        if (!rd_addr_valid) begin
          rresp_q <= 2'b10;
        end else begin
          for (b = 0; b < 32; b = b + 1) begin
            unique case (reg_bit_mode_t'(REG_MODE[rd_idx][b]))
              REG_BIT_RSVD: rd_word[b] = 1'b0;
              REG_BIT_RO  : rd_word[b] = reg_i[rd_idx][b];
              REG_BIT_RW  : rd_word[b] = reg_o[rd_idx][b];
              REG_BIT_W1S : rd_word[b] = 1'b0;
              REG_BIT_W1C : rd_word[b] = reg_o[rd_idx][b];
              default     : rd_word[b] = 1'b0;
            endcase
          end
        end

        rdata_q <= rd_word;
      end

      if (rvalid_q && s_axil.req.r_ready) begin
        rvalid_q <= 1'b0;
      end
    end
  end

endmodule