"""
Stage B: Two MMAs + Identity Softmax via TMEM

Architecture:
  MMA1: Q @ K^T -> tmem_scores (a_source=SMEM, accumulate=False)
  Identity softmax: tcgen05.ld -> fill 1.0 -> tcgen05.st back to TMEM
  MMA2: P @ V -> tmem_output (a_source=TMEM, accumulate=True)
  Two barriers: scores_full (MMA->epi), softmax_done (epi->MMA)
  Two TMEM regions: scores (offset 0), output (offset N)

With identity softmax (P = 1.0), output = ones(M,N) @ V = column sums of V tiled.
"""
import torch
import cutlass
import cutlass.cute as cute
import cutlass.utils as utils
import cutlass.pipeline as pipeline
from cutlass.cute.nvgpu import cpasync, tcgen05
from cutlass import Float32, BFloat16, Int32, Boolean, const_expr
from cutlass.utils import LayoutEnum
import cuda.bindings.driver as cuda


class StageBKernel:
    def __init__(self, mma_tiler_mn, use_2cta_instrs=False, use_tma_store=True):
        self.acc_dtype = Float32
        self.use_2cta_instrs = use_2cta_instrs
        self.mma_tiler_mn = mma_tiler_mn
        self.mma_tiler = (*mma_tiler_mn, 1)
        self.use_tma_store = use_tma_store
        self.cluster_shape_mn = (1, 1)
        self.cta_group = tcgen05.CtaGroup.TWO if use_2cta_instrs else tcgen05.CtaGroup.ONE
        self.epilogue_warp_id = (0, 1, 2, 3)
        self.mma_warp_id = 4
        self.tma_warp_id = 5
        self.threads_per_cta = 32 * 6
        self.epilog_sync_bar_id = 1
        self.tmem_alloc_sync_bar_id = 2
        self.tmem_dealloc_sync_bar_id = 3
        self.scores_full_bar_id = 5
        self.softmax_done_bar_id = 6

    def _setup_attributes(self, tiled_mma1, tiled_mma2):
        mma_inst_shape_k = cute.size(tiled_mma1.shape_mnk, mode=[2])
        mma_inst_tile_k = 4
        self.mma_tiler = (self.mma_tiler[0], self.mma_tiler[1],
                          mma_inst_shape_k * mma_inst_tile_k)
        self.cta_tile_shape_mnk = (
            self.mma_tiler[0] // cute.size(tiled_mma1.thr_id.shape),
            self.mma_tiler[1],
            self.mma_tiler[2],
        )
        self.cluster_layout_vmnk = cute.tiled_divide(
            cute.make_layout((1, 1, 1)), (tiled_mma1.thr_id.shape,))

        self.epi_tile = utils.sm100.compute_epilogue_tile_shape(
            self.cta_tile_shape_mnk, self.use_2cta_instrs, self.c_layout, self.c_dtype)

        self.num_ab_stage = 1
        self.num_acc_stage = 1
        self.num_c_stage = 2

        self.a_smem_layout_staged = utils.sm100.make_smem_layout_a(
            tiled_mma1, self.mma_tiler, self.a_dtype, self.num_ab_stage)
        self.b_smem_layout_staged = utils.sm100.make_smem_layout_b(
            tiled_mma1, self.mma_tiler, self.b_dtype, self.num_ab_stage)
        self.c_smem_layout_staged = utils.sm100.make_smem_layout_epi(
            self.c_dtype, self.c_layout, self.epi_tile, self.num_c_stage)

        acc_shape = tiled_mma1.partition_shape_C(self.mma_tiler_mn)
        tCtAcc_fake = tiled_mma1.make_fragment_C(cute.append(acc_shape, self.num_acc_stage))
        self.num_tmem_cols_per_region = utils.get_num_tmem_alloc_cols(tCtAcc_fake, arch="sm_100")
        total = self.num_tmem_cols_per_region * 2
        self.total_tmem_cols = 256
        if total > 256:
            self.total_tmem_cols = 512

        a_smem_layout = cute.slice_(self.a_smem_layout_staged, (None, None, None, 0))
        b_smem_layout = cute.slice_(self.b_smem_layout_staged, (None, None, None, 0))
        self.num_tma_load_bytes = (
            cute.size_in_bytes(self.a_dtype, a_smem_layout) +
            cute.size_in_bytes(self.b_dtype, b_smem_layout)
        ) * cute.size(tiled_mma1.thr_id.shape)

    @cute.jit
    def __call__(self, a: cute.Tensor, b: cute.Tensor, c: cute.Tensor,
                 stream: cuda.CUstream):
        self.a_dtype = a.element_type
        self.b_dtype = b.element_type
        self.c_dtype = c.element_type
        self.a_major_mode = LayoutEnum.from_tensor(a).mma_major_mode()
        self.b_major_mode = LayoutEnum.from_tensor(b).mma_major_mode()
        self.c_layout = LayoutEnum.from_tensor(c)

        tiled_mma1 = utils.sm100.make_trivial_tiled_mma(
            self.a_dtype, self.b_dtype, self.a_major_mode, self.b_major_mode,
            self.acc_dtype, self.cta_group, self.mma_tiler_mn,
            tcgen05.OperandSource.SMEM,
        )
        tiled_mma2 = utils.sm100.make_trivial_tiled_mma(
            self.a_dtype, self.b_dtype, self.a_major_mode, self.b_major_mode,
            self.acc_dtype, self.cta_group, self.mma_tiler_mn,
            tcgen05.OperandSource.TMEM,
        )

        self._setup_attributes(tiled_mma1, tiled_mma2)

        a_smem_layout = cute.slice_(self.a_smem_layout_staged, (None, None, None, 0))
        b_smem_layout = cute.slice_(self.b_smem_layout_staged, (None, None, None, 0))

        tma_atom_a, tma_tensor_a = cute.nvgpu.make_tiled_tma_atom_A(
            utils.sm100.cluster_shape_to_tma_atom_A(
                self.cluster_shape_mn, tiled_mma1.thr_id),
            a, a_smem_layout, self.mma_tiler, tiled_mma1,
            self.cluster_layout_vmnk.shape,
        )
        tma_atom_b, tma_tensor_b = cute.nvgpu.make_tiled_tma_atom_B(
            utils.sm100.cluster_shape_to_tma_atom_B(
                self.cluster_shape_mn, tiled_mma1.thr_id),
            b, b_smem_layout, self.mma_tiler, tiled_mma1,
            self.cluster_layout_vmnk.shape,
        )

        epi_smem_layout = cute.select(self.c_smem_layout_staged, mode=[0, 1])
        tma_atom_c, tma_tensor_c = cpasync.make_tiled_tma_atom(
            cpasync.CopyBulkTensorTileS2GOp(), c, epi_smem_layout, self.epi_tile)

        self._kernel(
            tiled_mma1, tiled_mma2,
            tma_atom_a, tma_tensor_a, tma_atom_b, tma_tensor_b,
            tma_atom_c, tma_tensor_c, self.cluster_layout_vmnk,
            self.a_smem_layout_staged, self.b_smem_layout_staged,
            self.c_smem_layout_staged, self.epi_tile,
        ).launch(grid=(1, 1, 1), block=[self.threads_per_cta, 1, 1], stream=stream)

    @cute.kernel
    def _kernel(self, tiled_mma1, tiled_mma2,
                tma_atom_a, mA_mkl, tma_atom_b, mB_nkl,
                tma_atom_c, mC_mnl, cluster_layout_vmnk,
                a_smem_layout_staged, b_smem_layout_staged,
                c_smem_layout_staged, epi_tile):
        warp_idx = cute.arch.warp_idx()
        warp_idx = cute.arch.make_warp_uniform(warp_idx)
        tidx, _, _ = cute.arch.thread_idx()
        use_2cta_instrs = cute.size(tiled_mma1.thr_id.shape) == 2
        is_leader_cta = True

        if warp_idx == self.tma_warp_id:
            cpasync.prefetch_descriptor(tma_atom_a)
            cpasync.prefetch_descriptor(tma_atom_b)
            cpasync.prefetch_descriptor(tma_atom_c)

        @cute.struct
        class SharedStorage:
            ab_full_mbar_ptr: cute.struct.MemRange[cutlass.Int64, self.num_ab_stage * 2]
            acc_full_mbar_ptr: cute.struct.MemRange[cutlass.Int64, self.num_acc_stage * 2]
            tmem_dealloc_mbar: cutlass.Int64
            tmem_holding_buf: cutlass.Int32

        smem = utils.SmemAllocator()
        storage = smem.allocate(SharedStorage)

        ab_producer, ab_consumer = pipeline.PipelineTmaUmma.create(
            barrier_storage=storage.ab_full_mbar_ptr.data_ptr(),
            num_stages=self.num_ab_stage,
            producer_group=pipeline.CooperativeGroup(pipeline.Agent.Thread),
            consumer_group=pipeline.CooperativeGroup(pipeline.Agent.Thread, 1),
            tx_count=self.num_tma_load_bytes,
            cta_layout_vmnk=cluster_layout_vmnk,
            defer_sync=True,
        ).make_participants()

        acc_pipeline = pipeline.PipelineUmmaAsync.create(
            barrier_storage=storage.acc_full_mbar_ptr.data_ptr(),
            num_stages=self.num_acc_stage,
            producer_group=pipeline.CooperativeGroup(pipeline.Agent.Thread),
            consumer_group=pipeline.CooperativeGroup(
                pipeline.Agent.Thread, len(self.epilogue_warp_id) * (2 if use_2cta_instrs else 1)),
            cta_layout_vmnk=cluster_layout_vmnk,
            defer_sync=True,
        )

        tmem_alloc_barrier = pipeline.NamedBarrier(
            barrier_id=self.tmem_alloc_sync_bar_id,
            num_threads=32 * len((self.mma_warp_id, *self.epilogue_warp_id)),
        )
        tmem = utils.TmemAllocator(
            storage.tmem_holding_buf.ptr,
            barrier_for_retrieve=tmem_alloc_barrier,
            allocator_warp_id=self.epilogue_warp_id[0],
            is_two_cta=use_2cta_instrs,
            two_cta_tmem_dealloc_mbar_ptr=storage.tmem_dealloc_mbar.ptr,
        )

        # MMA <-> softmax handshake barriers
        scores_full_barrier = pipeline.NamedBarrier(
            barrier_id=self.scores_full_bar_id,
            num_threads=32 * (1 + len(self.epilogue_warp_id)),
        )
        softmax_done_barrier = pipeline.NamedBarrier(
            barrier_id=self.softmax_done_bar_id,
            num_threads=32 * (len(self.epilogue_warp_id) + 1),
        )

        pipeline.pipeline_init_arrive(cluster_shape_mn=cluster_layout_vmnk, is_relaxed=True)

        sA = smem.allocate_tensor(
            element_type=self.a_dtype, layout=a_smem_layout_staged.outer,
            byte_alignment=128, swizzle=a_smem_layout_staged.inner)
        sB = smem.allocate_tensor(
            element_type=self.b_dtype, layout=b_smem_layout_staged.outer,
            byte_alignment=128, swizzle=b_smem_layout_staged.inner)
        sC = smem.allocate_tensor(
            element_type=self.c_dtype, layout=c_smem_layout_staged.outer,
            byte_alignment=128, swizzle=c_smem_layout_staged.inner)

        gA_mkl = cute.local_tile(mA_mkl, cute.slice_(self.mma_tiler, (None, 0, None)), (None, None, None))
        gB_nkl = cute.local_tile(mB_nkl, cute.slice_(self.mma_tiler, (0, None, None)), (None, None, None))
        gC_mnl = cute.local_tile(mC_mnl, cute.slice_(self.mma_tiler, (None, None, 0)), (None, None, None))
        k_tile_cnt = cute.size(gA_mkl, mode=[3])

        thr_mma1 = tiled_mma1.get_slice(0)
        tCgA = thr_mma1.partition_A(gA_mkl)
        tCgB = thr_mma1.partition_B(gB_nkl)
        tCgC = thr_mma1.partition_C(gC_mnl)

        a_cta_layout = cute.make_layout(cute.slice_(cluster_layout_vmnk, (0, 0, None, 0)).shape)
        tAsA, tAgA = cpasync.tma_partition(
            tma_atom_a, 0, a_cta_layout,
            cute.group_modes(sA, 0, 3), cute.group_modes(tCgA, 0, 3))
        b_cta_layout = cute.make_layout(cute.slice_(cluster_layout_vmnk, (0, None, 0, 0)).shape)
        tBsB, tBgB = cpasync.tma_partition(
            tma_atom_b, 0, b_cta_layout,
            cute.group_modes(sB, 0, 3), cute.group_modes(tCgB, 0, 3))

        tAgA_slice = tAgA[(None, 0, None, 0)]
        tBgB_slice = tBgB[(None, 0, None, 0)]

        tCrA = tiled_mma1.make_fragment_A(sA)
        tCrB = tiled_mma1.make_fragment_B(sB)
        tCrA_mma2 = tiled_mma2.make_fragment_A(sA)
        tCrB_mma2 = tiled_mma2.make_fragment_B(sB)

        acc_shape = tiled_mma1.partition_shape_C(self.mma_tiler_mn)
        tCtAcc_fake = tiled_mma1.make_fragment_C(cute.append(acc_shape, self.num_acc_stage))

        pipeline.pipeline_init_wait(cluster_shape_mn=cluster_layout_vmnk)

        # TMA LOAD WARP
        if warp_idx == self.tma_warp_id:
            ab_producer.reset()
            peek_ab_empty_status = ab_producer.try_acquire()
            for k_tile in cutlass.range(k_tile_cnt, unroll=1):
                handle = ab_producer.acquire_and_advance(peek_ab_empty_status)
                cute.copy(tma_atom_a, tAgA_slice[(None, handle.count)], tAsA[(None, handle.index)],
                          tma_bar_ptr=handle.barrier)
                cute.copy(tma_atom_b, tBgB_slice[(None, handle.count)], tBsB[(None, handle.index)],
                          tma_bar_ptr=handle.barrier)
                peek_ab_empty_status = cutlass.Boolean(1)
                if handle.count + 1 < k_tile_cnt:
                    peek_ab_empty_status = ab_producer.try_acquire()
            ab_producer.tail()

        # MMA WARP
        if warp_idx == self.mma_warp_id:
            tmem.wait_for_alloc()
            tmem_ptr = tmem.retrieve_ptr(self.acc_dtype)

            tCtScores_base = cute.make_tensor(tmem_ptr, tCtAcc_fake.layout)
            tCtScores = tCtScores_base[(None, None, None, 0)]

            output_tmem_ptr = cute.recast_ptr(
                tmem_ptr + self.num_tmem_cols_per_region, dtype=self.acc_dtype)
            tCtOutput_base = cute.make_tensor(output_tmem_ptr, tCtAcc_fake.layout)
            tCtOutput = tCtOutput_base[(None, None, None, 0)]

            ab_consumer.reset()
            peek_ab_full_status = cutlass.Boolean(1)
            if is_leader_cta:
                peek_ab_full_status = ab_consumer.try_wait()

            # MMA1: Q @ K^T -> tmem_scores
            tiled_mma1.set(tcgen05.Field.ACCUMULATE, False)
            for k_tile in range(k_tile_cnt):
                if is_leader_cta:
                    handle = ab_consumer.wait_and_advance(peek_ab_full_status)
                    num_kblocks = cute.size(tCrA, mode=[2])
                    for kblk_idx in cutlass.range(num_kblocks, unroll_full=True):
                        kblk_crd = (None, None, kblk_idx, handle.index)
                        cute.gemm(tiled_mma1, tCtScores, tCrA[kblk_crd], tCrB[kblk_crd], tCtScores)
                    handle.release()
                    peek_ab_full_status = cutlass.Boolean(1)
                    if handle.count + 1 < k_tile_cnt:
                        peek_ab_full_status = ab_consumer.try_wait()

            # Signal scores ready
            scores_full_barrier.arrive()
            # Wait for softmax done
            softmax_done_barrier.arrive_and_wait()

            # MMA2: P @ V -> tmem_output
            tiled_mma2.set(tcgen05.Field.ACCUMULATE, True)
            num_kblocks_mma2 = cute.size(tCrB_mma2, mode=[2])
            for kblk_idx in cutlass.range(num_kblocks_mma2, unroll_full=True):
                kblk_crd = (None, None, kblk_idx, 0)
                cute.gemm(tiled_mma2, tCtOutput, tCrA_mma2[kblk_crd], tCrB_mma2[kblk_crd], tCtOutput)

            acc_producer_state = pipeline.make_pipeline_state(
                pipeline.PipelineUserType.Producer, self.num_acc_stage)
            if is_leader_cta:
                acc_pipeline.producer_acquire(acc_producer_state)
                acc_pipeline.producer_commit(acc_producer_state)
            acc_producer_state.advance()
            acc_pipeline.producer_tail(acc_producer_state)

        # EPILOGUE WARPS
        if warp_idx < self.mma_warp_id:
            tmem.allocate(self.total_tmem_cols)
            tmem.wait_for_alloc()
            tmem_ptr = tmem.retrieve_ptr(self.acc_dtype)

            tCtScores_base = cute.make_tensor(tmem_ptr, tCtAcc_fake.layout)
            tCtScores = tCtScores_base[(None, None, None, 0)]

            output_tmem_ptr = cute.recast_ptr(
                tmem_ptr + self.num_tmem_cols_per_region, dtype=self.acc_dtype)
            tCtOutput_base = cute.make_tensor(output_tmem_ptr, tCtAcc_fake.layout)

            # Wait for scores
            scores_full_barrier.arrive_and_wait()

            # Identity softmax: load, fill 1.0, store back
            tiled_copy_t2r, tTR_tScores, tTR_rScores = utils.gemm.sm100.epilogue_tmem_copy_and_partition(
                self, tidx, tCtScores, tCgC, epi_tile, self.use_2cta_instrs)

            cute.copy(tiled_copy_t2r, tTR_tScores, tTR_rScores)

            for idx in cutlass.range(cute.size(tTR_rScores), unroll_full=True):
                tTR_rScores[idx] = self.acc_dtype(1.0)

            copy_atom_r2t = cute.make_copy_atom(
                tcgen05.St16x128bOp(tcgen05.Repetition.x32, tcgen05.Unpack.NONE),
                self.acc_dtype,
            )
            tiled_copy_r2t = tcgen05.make_tmem_copy(copy_atom_r2t, tCtScores)
            thr_copy_r2t = tiled_copy_r2t.get_slice(tidx)
            tRT_tScores = thr_copy_r2t.partition_D(tCtScores)
            tRT_rP = thr_copy_r2t.partition_S(tTR_rScores)
            cute.copy(tiled_copy_r2t, tRT_rP, tRT_tScores)
            cute.arch.fence_view_async_tmem_load()

            # Signal softmax done
            softmax_done_barrier.arrive()

            # Store output
            acc_consumer_state = pipeline.make_pipeline_state(
                pipeline.PipelineUserType.Consumer, self.num_acc_stage)

            c_producer_group = pipeline.CooperativeGroup(
                pipeline.Agent.Thread, 32 * len(self.epilogue_warp_id))
            c_pipeline = pipeline.PipelineTmaStore.create(
                num_stages=self.num_c_stage, producer_group=c_producer_group)

            mma_tile_coord_mnl = (0, 0, 0)
            epilogue_op = const_expr(lambda x: x)
            num_tiles_executed = 0

            acc_consumer_state = utils.gemm.sm100.epilogue_tma_store(
                self, tidx, warp_idx, tma_atom_c, tCtOutput_base, sC, tCgC,
                epi_tile, num_tiles_executed, epilogue_op,
                mma_tile_coord_mnl, acc_consumer_state, acc_pipeline, c_pipeline)

            c_pipeline.producer_tail()
            tmem.relinquish_alloc_permit()
            tmem.free(tmem_ptr)


def test_stage_b():
    device = torch.device("cuda")
    torch.manual_seed(42)

    m, n, k = 128, 128, 128

    a = torch.randn(m, k, 1, dtype=torch.bfloat16, device="cuda")
    b = torch.randn(n, k, 1, dtype=torch.bfloat16, device="cuda")
    c = torch.zeros(m, n, 1, dtype=torch.bfloat16, device="cuda")

    # Identity softmax: P = 1.0, output = ones(M,N) @ V(N,K)
    v = b[:, :, 0].float()  # (128, 128)
    ref = torch.ones(m, n, dtype=torch.float32) @ v

    import cutlass.torch as cutlass_torch
    mA = cutlass_torch.from_dlpack(a).mark_layout_dynamic(
        leading_dim=cutlass_torch.get_leading_dim(a))
    mB = cutlass_torch.from_dlpack(b).mark_layout_dynamic(
        leading_dim=cutlass_torch.get_leading_dim(b))
    mC = cutlass_torch.from_dlpack(c).mark_layout_dynamic(
        leading_dim=cutlass_torch.get_leading_dim(c))

    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)

    kernel = StageBKernel(mma_tiler_mn=(128, 128), use_2cta_instrs=False, use_tma_store=True)
    compiled = cute.compile(kernel, mA, mB, mC, stream)

    compiled(mA, mB, mC, stream)
    torch.cuda.synchronize()

    output = c[:, :, 0].float()
    cos = torch.nn.functional.cosine_similarity(
        output.flatten().unsqueeze(0), ref.flatten().unsqueeze(0)).item()
    max_err = (output - ref).abs().max().item()

    print("Stage B: Q @ K^T -> identity_softmax(P=1) @ V -> output")
    print("  Cosine: {:.6f}, Max error: {:.6f}".format(cos, max_err))
    print("  {}".format("PASS" if cos >= 0.99 else "FAIL"))
    return cos


if __name__ == "__main__":
    test_stage_b()
