"""
FMHA D2: Head-packed multi-head attention.

Strategy A: Fold the head dimension into M. Q is reshaped from (n_h, T, hd)
to (n_h*T, hd, 1). K/V are (s_k, hd, 1) — shared MQA.
At decode T=1, n_h=128: M=128, exactly one MMA tile.
The kernel treats each row as independent attention (per-row softmax).

Run: ~/.openclaw/workspace/fire_b200_test tests/unit/test_d2_headpacked.py
"""
import torch
import math
import cutlass.cute as cute
import cuda.bindings.driver as cuda
import cutlass.torch as ct

from dsv4.kernels.attention.fmha import FmhaKernel


def reference_fmha(q, k, v, scale):
    """FP32 reference: q (M, hd), k (s_k, hd), v (s_k, hd) → o (M, hd)"""
    scores = torch.matmul(q.float(), k.float().T) * scale
    max_s = scores.max(dim=-1, keepdim=True).values
    exp_s = (scores - max_s).exp()
    sum_s = exp_s.sum(dim=-1, keepdim=True)
    p = exp_s / sum_s
    o = torch.matmul(p, v.float())
    return o.to(torch.bfloat16)


def _run_fmha(fmha, q_3d, k_3d, v_3d, o_3d):
    """Run FmhaKernel with CuTe tensors."""
    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)
    q_c = ct.from_dlpack(q_3d).mark_layout_dynamic(leading_dim=ct.get_leading_dim(q_3d))
    k_c = ct.from_dlpack(k_3d).mark_layout_dynamic(leading_dim=ct.get_leading_dim(k_3d))
    v_c = ct.from_dlpack(v_3d).mark_layout_dynamic(leading_dim=ct.get_leading_dim(v_3d))
    o_c = ct.from_dlpack(o_3d).mark_layout_dynamic(leading_dim=ct.get_leading_dim(o_3d))
    fmha(q_c, k_c, v_c, o_c, stream)


def test_d2_headpacked_n1():
    """Regression: n_h=1 (same as single-head)."""
    print("\n=== Test 1: n_h=1 regression (hd=64) ===")
    torch.manual_seed(42)
    M, s_k, hd = 128, 128, 64
    scale = 1.0 / math.sqrt(hd)
    
    q = torch.randn(M, hd, 1, dtype=torch.bfloat16, device='cuda')
    k = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    o = torch.zeros(M, hd, 1, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True)
    _run_fmha(fmha, q, k, v, o)
    
    ref = reference_fmha(q[:,:,0], k[:,:,0], v[:,:,0], scale)
    cos = torch.nn.functional.cosine_similarity(
        o[:,:,0].flatten().float().unsqueeze(0), ref.flatten().float().unsqueeze(0)
    ).item()
    print(f"  cos = {cos:.6f}")
    assert cos >= 0.99, f"cosine too low: {cos}"
    print("  ✅ PASS")


def test_d2_headpacked_128():
    """n_h=128, T=1 (Pro decode): M=128, one M tile, all heads packed."""
    print("\n=== Test 2: n_h=128, T=1 (Pro decode, hd=64) ===")
    torch.manual_seed(42)
    n_h, T, s_k, hd = 128, 1, 128, 64
    scale = 1.0 / math.sqrt(hd)
    
    q_heads = torch.randn(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    # Pack heads into M: (n_h*T, hd) → (128, 64, 1)
    q = q_heads.reshape(n_h * T, hd).unsqueeze(-1)
    k = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    o = torch.zeros(n_h * T, hd, 1, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    _run_fmha(fmha, q, k, v, o)
    
    # Reference: per-head attention
    o_ref = torch.zeros(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    for h in range(n_h):
        o_ref[h, 0] = reference_fmha(q_heads[h], k[:,:,0], v[:,:,0], scale)[0]
    o_ref_flat = o_ref.reshape(n_h * T, hd)
    
    cos = torch.nn.functional.cosine_similarity(
        o[:,:,0].flatten().float().unsqueeze(0), o_ref_flat.flatten().float().unsqueeze(0)
    ).item()
    print(f"  cos = {cos:.6f}")
    assert cos >= 0.99, f"cosine too low: {cos}"
    print("  ✅ PASS")


def test_d2_headpacked_64():
    """n_h=64, T=1 (Flash decode): M=64, pad to 128."""
    print("\n=== Test 3: n_h=64, T=1 (Flash decode, hd=64) ===")
    torch.manual_seed(42)
    n_h, T, s_k, hd = 64, 1, 128, 64
    scale = 1.0 / math.sqrt(hd)
    
    q_heads = torch.randn(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    q_flat = q_heads.reshape(n_h * T, hd)  # (64, 64)
    # Pad to 128 rows
    q = torch.nn.functional.pad(q_flat, (0, 0, 0, 128 - n_h * T)).unsqueeze(-1)  # (128, 64, 1)
    k = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    o_padded = torch.zeros(128, hd, 1, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    _run_fmha(fmha, q, k, v, o_padded)
    
    o = o_padded[:n_h * T, :, 0]  # Trim padding
    o_ref = torch.zeros(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    for h in range(n_h):
        o_ref[h, 0] = reference_fmha(q_heads[h], k[:,:,0], v[:,:,0], scale)[0]
    o_ref_flat = o_ref.reshape(n_h * T, hd)
    
    cos = torch.nn.functional.cosine_similarity(
        o.flatten().float().unsqueeze(0), o_ref_flat.flatten().float().unsqueeze(0)
    ).item()
    print(f"  cos = {cos:.6f}")
    assert cos >= 0.99, f"cosine too low: {cos}"
    print("  ✅ PASS")


def test_d2_headpacked_hd128():
    """n_h=8, T=1, hd=128: pad to 128 rows, larger head dim."""
    print("\n=== Test 4: n_h=8, T=1, hd=128 ===")
    torch.manual_seed(42)
    n_h, T, s_k, hd = 8, 1, 128, 128
    scale = 1.0 / math.sqrt(hd)
    
    q_heads = torch.randn(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    q_flat = q_heads.reshape(n_h * T, hd)  # (8, 128)
    q = torch.nn.functional.pad(q_flat, (0, 0, 0, 128 - n_h * T)).unsqueeze(-1)  # (128, 128, 1)
    k = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, 1, dtype=torch.bfloat16, device='cuda')
    o_padded = torch.zeros(128, hd, 1, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    _run_fmha(fmha, q, k, v, o_padded)
    
    o = o_padded[:n_h * T, :, 0]
    o_ref = torch.zeros(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    for h in range(n_h):
        o_ref[h, 0] = reference_fmha(q_heads[h], k[:,:,0], v[:,:,0], scale)[0]
    o_ref_flat = o_ref.reshape(n_h * T, hd)
    
    cos = torch.nn.functional.cosine_similarity(
        o.flatten().float().unsqueeze(0), o_ref_flat.flatten().float().unsqueeze(0)
    ).item()
    print(f"  cos = {cos:.6f}")
    assert cos >= 0.99, f"cosine too low: {cos}"
    print("  ✅ PASS")


def test():
    print("=== D2: Head-Packed Multi-Head FMHA ===")
    test_d2_headpacked_n1()
    test_d2_headpacked_128()
    test_d2_headpacked_64()
    test_d2_headpacked_hd128()
    print("\n=== ALL TESTS PASSED ===")


if __name__ == '__main__':
    test()