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

Strategy A: Fold the head dimension into M. Each CTA processes
all heads' queries for its M tile. At decode T=1, n_h=128, M=128
fills exactly one MMA tile. The kernel doesn't need to know about
heads — it just processes M rows with per-row softmax.

Q is reshaped from (n_h, T, hd) to (n_h * T, hd) in Python.
K/V are shared (MQA) with shape (s_k, hd).

Run: ~/.openclaw/workspace/fire_b200_test tests/unit/test_d2_headpacked.py
"""
import torch
import math
import cutlass
import cutlass.cute as cute
from cutlass import Float32, BFloat16
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 test_d2_headpacked_n1():
    """Regression: n_h=1 (same as single-head, backward compatible)."""
    print("\n=== Test 1: n_h=1 regression (hd=64) ===")
    torch.manual_seed(42)
    T, s_k, hd = 1, 128, 64
    scale = 1.0 / math.sqrt(hd)
    
    q = torch.randn(T, hd, dtype=torch.bfloat16, device='cuda')
    k = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True)
    o = torch.zeros(T, hd, dtype=torch.bfloat16, device='cuda')
    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)
    
    q_c = ct.from_dlpack(q).mark_layout_dynamic(leading_dim=ct.get_leading_dim(q))
    k_c = ct.from_dlpack(k).mark_layout_dynamic(leading_dim=ct.get_leading_dim(k))
    v_c = ct.from_dlpack(v).mark_layout_dynamic(leading_dim=ct.get_leading_dim(v))
    o_c = ct.from_dlpack(o).mark_layout_dynamic(leading_dim=ct.get_leading_dim(o))
    fmha(q_c, k_c, v_c, o_c, stream)
    
    ref = reference_fmha(q, k, v, scale)
    cos = torch.nn.functional.cosine_similarity(
        o.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_basic():
    """n_h=128, T=1 (Pro decode): M=128, exactly one M tile."""
    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: (n_h, T, hd) → (n_h*T, hd) = (128, 64)
    q_heads = torch.randn(n_h, T, hd, dtype=torch.bfloat16, device='cuda')
    q = q_heads.reshape(n_h * T, hd)
    k = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    o = torch.zeros(n_h * T, hd, dtype=torch.bfloat16, device='cuda')
    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)
    
    q_c = ct.from_dlpack(q).mark_layout_dynamic(leading_dim=ct.get_leading_dim(q))
    k_c = ct.from_dlpack(k).mark_layout_dynamic(leading_dim=ct.get_leading_dim(k))
    v_c = ct.from_dlpack(v).mark_layout_dynamic(leading_dim=ct.get_leading_dim(v))
    o_c = ct.from_dlpack(o).mark_layout_dynamic(leading_dim=ct.get_leading_dim(o))
    fmha(q_c, k_c, v_c, o_c, stream)
    
    # 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, v, 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_flash():
    """n_h=64, T=1 (Flash decode): M=64, underutilized (1 CTA, 64 rows)."""
    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 = q_heads.reshape(n_h * T, hd)
    # Pad to 128 rows (M tile size) — kernel expects M >= 128
    q_padded = torch.nn.functional.pad(q, (0, 0, 0, 128 - n_h * T))
    k = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    o_padded = torch.zeros(128, hd, dtype=torch.bfloat16, device='cuda')
    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)
    
    q_c = ct.from_dlpack(q_padded).mark_layout_dynamic(leading_dim=ct.get_leading_dim(q_padded))
    k_c = ct.from_dlpack(k).mark_layout_dynamic(leading_dim=ct.get_leading_dim(k))
    v_c = ct.from_dlpack(v).mark_layout_dynamic(leading_dim=ct.get_leading_dim(v))
    o_c = ct.from_dlpack(o_padded).mark_layout_dynamic(leading_dim=ct.get_leading_dim(o_padded))
    fmha(q_c, k_c, v_c, o_c, stream)
    
    o = o_padded[:n_h * T]  # 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, v, 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 (multi-head with 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 = q_heads.reshape(n_h * T, hd)
    k = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    v = torch.randn(s_k, hd, dtype=torch.bfloat16, device='cuda')
    
    fmha = FmhaKernel(head_dim=hd, s_k=s_k, normalize=True, num_query_heads=n_h)
    o = torch.zeros(n_h * T, hd, dtype=torch.bfloat16, device='cuda')
    stream = cuda.CUstream(torch.cuda.current_stream().cuda_stream)
    
    q_c = ct.from_dlpack(q).mark_layout_dynamic(leading_dim=ct.get_leading_dim(q))
    k_c = ct.from_dlpack(k).mark_layout_dynamic(leading_dim=ct.get_leading_dim(k))
    v_c = ct.from_dlpack(v).mark_layout_dynamic(leading_dim=ct.get_leading_dim(v))
    o_c = ct.from_dlpack(o).mark_layout_dynamic(leading_dim=ct.get_leading_dim(o))
    fmha(q_c, k_c, v_c, o_c, stream)
    
    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, v, 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_basic()
    test_d2_headpacked_flash()
    test_d2_headpacked_hd128()
    print("\n=== ALL TESTS PASSED ===")


if __name__ == '__main__':
    test()