# Copyright (c) 2023, Tri Dao. augmented with varlen

from typing import List, Optional, Tuple, Union

import torch
from einops import rearrange


def _select_positions(
    cos: torch.Tensor, sin: torch.Tensor, positions: torch.Tensor
) -> Tuple[torch.Tensor, torch.Tensor]:
    flat_pos = positions.reshape(-1)
    cos_sel = cos.index_select(0, flat_pos)
    sin_sel = sin.index_select(0, flat_pos)
    target_shape = positions.shape + (cos.shape[-1],)
    return cos_sel.view(target_shape), sin_sel.view(target_shape)


def _apply_rotary_slice(
    tensor: torch.Tensor,
    cos_vals: torch.Tensor,
    sin_vals: torch.Tensor,
    *,
    interleaved: bool,
):
    rotary_dim = tensor.shape[-1]
    if not interleaved:
        rotary_dim_half = rotary_dim // 2
        x0 = tensor[..., :rotary_dim_half]
        x1 = tensor[..., rotary_dim_half:]
        new_x0 = x0 * cos_vals - x1 * sin_vals
        new_x1 = x0 * sin_vals + x1 * cos_vals
        tensor[..., :rotary_dim_half] = new_x0
        tensor[..., rotary_dim_half:] = new_x1
    else:
        x_even = tensor[..., ::2]
        x_odd = tensor[..., 1::2]
        new_even = x_even * cos_vals - x_odd * sin_vals
        new_odd = x_even * sin_vals + x_odd * cos_vals
        tensor[..., ::2] = new_even
        tensor[..., 1::2] = new_odd


def apply_rotary(
    x: torch.Tensor,
    cos: torch.Tensor,
    sin: torch.Tensor,
    seqlen_offsets: Union[int, torch.Tensor] = 0,
    cu_seqlens: Optional[torch.Tensor] = None,
    max_seqlen: Optional[int] = None,
    interleaved: bool = False,
    inplace: bool = False,
) -> torch.Tensor:
    """
    PyTorch implementation of apply_rotary.

    Args:
        x: (batch, seqlen, nheads, headdim) if cu_seqlens is None else (total_seqlen, nheads, headdim)
        cos, sin: (seqlen_ro, rotary_dim / 2)
        seqlen_offsets: scalar or tensor of shape (batch,)
        cu_seqlens: prefix sums for variable-length sequences
        max_seqlen: maximum sequence length (required when cu_seqlens is provided for API parity)
        interleaved: whether to apply GPT-J style rotation
        inplace: modify x in-place
    """

    if cu_seqlens is not None:
        assert (
            max_seqlen is not None
        ), "max_seqlen must be provided when cu_seqlens is passed (API compatibility)."

    seqlen_ro, rotary_dim_half = cos.shape
    rotary_dim = rotary_dim_half * 2
    if rotary_dim == 0:
        return x

    assert sin.shape == cos.shape, "cos and sin must have the same shape"
    assert rotary_dim <= x.shape[-1], f"rotary_dim ({rotary_dim}) must be <= headdim ({x.shape[-1]})"

    if not inplace:
        out = x.clone()
    else:
        out = x

    cos = cos.to(out.device)
    sin = sin.to(out.device)

    if cu_seqlens is None:
        batch, seqlen, _, _ = out.shape
        base = torch.arange(seqlen, device=out.device, dtype=torch.long)
        if isinstance(seqlen_offsets, torch.Tensor):
            offsets = seqlen_offsets.to(device=out.device, dtype=torch.long)
            assert offsets.shape == (batch,), "seqlen_offsets tensor must have shape (batch,)"
            positions = base.unsqueeze(0) + offsets.unsqueeze(1)
        else:
            positions = base.unsqueeze(0) + int(seqlen_offsets)
            positions = positions.expand(out.shape[0], -1)
        cos_sel, sin_sel = _select_positions(cos, sin, positions)
        cos_sel = cos_sel.unsqueeze(2).to(out.dtype)
        sin_sel = sin_sel.unsqueeze(2).to(out.dtype)
        _apply_rotary_slice(out[..., :rotary_dim], cos_sel, sin_sel, interleaved=interleaved)
    else:
        cu_seqlens = cu_seqlens.to("cpu")
        batch = cu_seqlens.numel() - 1
        if isinstance(seqlen_offsets, torch.Tensor):
            offsets = seqlen_offsets.to(device=out.device, dtype=torch.long)
            assert offsets.shape == (batch,), "seqlen_offsets tensor must have shape (batch,)"
        else:
            offsets = torch.full((batch,), int(seqlen_offsets), device=out.device, dtype=torch.long)
        for b in range(batch):
            start = int(cu_seqlens[b].item())
            end = int(cu_seqlens[b + 1].item())
            if end <= start:
                continue
            seq_len = end - start
            positions = torch.arange(seq_len, device=out.device, dtype=torch.long) + offsets[b]
            cos_sel, sin_sel = _select_positions(cos, sin, positions)
            cos_sel = cos_sel.unsqueeze(1).to(out.dtype)
            sin_sel = sin_sel.unsqueeze(1).to(out.dtype)
            _apply_rotary_slice(
                out[start:end, :, :rotary_dim],
                cos_sel,
                sin_sel,
                interleaved=interleaved,
            )

    return out


def apply_rotary_emb(
    x,
    cos,
    sin,
    interleaved=False,
    inplace=False,
    seqlen_offsets: Union[int, torch.Tensor] = 0,
    cu_seqlens: Optional[torch.Tensor] = None,
    max_seqlen: Optional[int] = None,
):
    """
    Arguments:
        x: (batch_size, seqlen, nheads, headdim) if cu_seqlens is None
            else (total_seqlen, nheads, headdim)
        cos, sin: (seqlen_rotary, rotary_dim / 2)
        interleaved: if True, rotate pairs of even and odd dimensions (GPT-J style) instead
            of 1st half and 2nd half (GPT-NeoX style).
        inplace: if True, apply rotary embedding in-place.
        seqlen_offsets: (batch_size,) or int. Each sequence in x is shifted by this amount.
            Most commonly used in inference when we have KV cache.
        cu_seqlens: (batch + 1,) or None
        max_seqlen: int
    Return:
        out: (batch_size, seqlen, nheads, headdim) if cu_seqlens is None
            else (total_seqlen, nheads, headdim)
    rotary_dim must be <= headdim
    Apply rotary embedding to the first rotary_dim of x.
    """
    return apply_rotary(
        x,
        cos,
        sin,
        seqlen_offsets=seqlen_offsets,
        cu_seqlens=cu_seqlens,
        max_seqlen=max_seqlen,
        interleaved=interleaved,
        inplace=inplace,
    )


apply_rotary_emb_func = apply_rotary_emb


def apply_rotary_emb_qkv_(
    qkv,
    cos,
    sin,
    cos_k=None,
    sin_k=None,
    interleaved=False,
    seqlen_offsets: Union[int, torch.Tensor] = 0,
):
    """
    Apply rotary embeddings inplace to q and k within a fused qkv tensor.
    """
    cos_k = cos if cos_k is None else cos_k
    sin_k = sin if sin_k is None else sin_k
    q = qkv[:, :, 0]
    k = qkv[:, :, 1]
    apply_rotary(q, cos, sin, seqlen_offsets=seqlen_offsets, interleaved=interleaved, inplace=True)
    apply_rotary(k, cos_k, sin_k, seqlen_offsets=seqlen_offsets, interleaved=interleaved, inplace=True)
    return qkv


def apply_rotary_emb_kv_(
    kv,
    cos,
    sin,
    interleaved=False,
    seqlen_offsets: Union[int, torch.Tensor] = 0,
):
    """
    Apply rotary embeddings inplace to the keys within a fused kv tensor.
    """
    k = kv[:, :, 0]
    apply_rotary(k, cos, sin, seqlen_offsets=seqlen_offsets, interleaved=interleaved, inplace=True)
    return kv


class RotaryEmbedding(torch.nn.Module):
    """
    The rotary position embeddings from RoFormer_ (Su et. al).
    A crucial insight from the method is that the query and keys are
    transformed by rotation matrices which depend on the relative positions.

    Other implementations are available in the Rotary Transformer repo_ and in
    GPT-NeoX_, GPT-NeoX was an inspiration

    .. _RoFormer: https://arxiv.org/abs/2104.09864
    .. _repo: https://github.com/ZhuiyiTechnology/roformer
    .. _GPT-NeoX: https://github.com/EleutherAI/gpt-neox

    If scale_base is not None, this implements XPos (Sun et al., https://arxiv.org/abs/2212.10554).
    A recommended value for scale_base is 512: https://github.com/HazyResearch/flash-attention/issues/96
    Reference: https://github.com/sunyt32/torchscale/blob/main/torchscale/component/xpos_relative_position.py
    """

    def __init__(
        self,
        dim: int,
        base=10000.0,
        interleaved=False,
        scale_base=None,
        pos_idx_in_fp32=True,
        device=None,
    ):
        """
        interleaved: if True, rotate pairs of even and odd dimensions (GPT-J style) instead
            of 1st half and 2nd half (GPT-NeoX style).
        pos_idx_in_fp32: if True, the position indices [0.0, ..., seqlen - 1] are in fp32,
            otherwise they might be in lower precision.
            This option was added because previously (before 2023-07-02), when we construct
            the position indices, we use the dtype of self.inv_freq. In most cases this would
            be fp32, but if the model is trained in pure bf16 (not mixed precision), then
            self.inv_freq would be bf16, and the position indices are also in bf16.
            Because of the limited precision of bf16 (e.g. 1995.0 is rounded to 2000.0), the
            embeddings for some positions will coincide.
            To maintain compatibility with models previously trained in pure bf16,
            we add this option.
        """
        super().__init__()
        self.dim = dim
        self.base = float(base)
        self.pos_idx_in_fp32 = pos_idx_in_fp32
        # Generate and save the inverse frequency buffer (non trainable)
        inv_freq = self._compute_inv_freq(device)
        self.register_buffer("inv_freq", inv_freq, persistent=False)
        self.interleaved = interleaved
        self.scale_base = scale_base
        scale = (
            (torch.arange(0, dim, 2, device=device, dtype=torch.float32) + 0.4 * dim) / (1.4 * dim)
            if scale_base is not None
            else None
        )
        self.register_buffer("scale", scale, persistent=False)

        self._seq_len_cached = 0
        self._var_seq_len_cached = None
        self._cos_cached = None
        self._sin_cached = None
        self._cos_k_cached = None
        self._sin_k_cached = None

    def _compute_inv_freq(self, device=None):
        return 1.0 / (
            self.base ** (torch.arange(0, self.dim, 2, device=device, dtype=torch.float32) / self.dim)
        )

    def _update_varlen_cos_sin_cache(self, seqlens, device=None, dtype=None):
        # Only update if total length changed or cache is invalid
        if (
            self._var_seq_len_cached is None
            or self._var_seq_len_cached != seqlens
            or self._cos_cached is None
            or self._cos_cached.device != device
            or self._cos_cached.dtype != dtype
            or (self.training and self._cos_cached.is_inference())
        ):
            self._var_seq_len_cached = seqlens
            self._seq_len_cached = sum(seqlens)
            cos_cached = []
            sin_cached = []
            cos_k_cached = []
            sin_k_cached = []
            for seqlen in seqlens:
                # We want fp32 here, not self.inv_freq.dtype, since the model could be loaded in bf16
                # And the output of arange can be quite large, so bf16 would lose a lot of precision.
                # However, for compatibility reason, we add an option to use the dtype of self.inv_freq.
                if self.pos_idx_in_fp32:
                    t = torch.arange(int(seqlen), device=device, dtype=torch.float32)
                    # We want fp32 here as well since inv_freq will be multiplied with t, and the output
                    # will be large. Having it in bf16 will lose a lot of precision and cause the
                    # cos & sin output to change significantly.
                    # We want to recompute self.inv_freq if it was not loaded in fp32
                    if self.inv_freq.dtype != torch.float32:
                        inv_freq = self._compute_inv_freq(device=device)
                    else:
                        inv_freq = self.inv_freq
                else:
                    t = torch.arange(seqlen, device=device, dtype=self.inv_freq.dtype)
                    inv_freq = self.inv_freq
                # Don't do einsum, it converts fp32 to fp16 under AMP
                # freqs = torch.einsum("i,j->ij", t, self.inv_freq)
                freqs = torch.outer(t, inv_freq)
                if self.scale is None:
                    cos_cached.append(torch.cos(freqs).to(dtype))
                    sin_cached.append(torch.sin(freqs).to(dtype))
                else:
                    power = (
                        torch.arange(seqlen, dtype=self.scale.dtype, device=self.scale.device)
                        - seqlen // 2
                    ) / self.scale_base
                    scale = self.scale.to(device=power.device) ** rearrange(power, "s -> s 1")
                    # We want the multiplication by scale to happen in fp32
                    cos_cached.append((torch.cos(freqs) * scale).to(dtype))
                    sin_cached.append((torch.sin(freqs) * scale).to(dtype))
                    cos_k_cached.append((torch.cos(freqs) / scale).to(dtype))
                    sin_k_cached.append((torch.sin(freqs) / scale).to(dtype))

            self._cos_cached = torch.cat(cos_cached, dim=0)
            self._sin_cached = torch.cat(sin_cached, dim=0)
            if self.scale is not None:
                self._cos_k_cached = torch.cat(cos_k_cached, dim=0)
                self._sin_k_cached = torch.cat(sin_k_cached, dim=0)

    def _update_cos_sin_cache(self, seqlen, device=None, dtype=None):
        # Reset the tables if the sequence length has changed,
        # if we're on a new device (possibly due to tracing for instance),
        # or if we're switching from inference mode to training
        if (
            seqlen > self._seq_len_cached
            or self._cos_cached is None
            or self._cos_cached.device != device
            or self._cos_cached.dtype != dtype
            or (self.training and self._cos_cached.is_inference())
        ):
            self._var_seq_lens = None
            self._seq_len_cached = seqlen
            # We want fp32 here, not self.inv_freq.dtype, since the model could be loaded in bf16
            # And the output of arange can be quite large, so bf16 would lose a lot of precision.
            # However, for compatibility reason, we add an option to use the dtype of self.inv_freq.
            if self.pos_idx_in_fp32:
                t = torch.arange(seqlen, device=device, dtype=torch.float32)
                # We want fp32 here as well since inv_freq will be multiplied with t, and the output
                # will be large. Having it in bf16 will lose a lot of precision and cause the
                # cos & sin output to change significantly.
                # We want to recompute self.inv_freq if it was not loaded in fp32
                if self.inv_freq.dtype != torch.float32:
                    inv_freq = self._compute_inv_freq(device=device)
                else:
                    inv_freq = self.inv_freq
            else:
                t = torch.arange(seqlen, device=device, dtype=self.inv_freq.dtype)
                inv_freq = self.inv_freq
            # Don't do einsum, it converts fp32 to fp16 under AMP
            # freqs = torch.einsum("i,j->ij", t, self.inv_freq)
            freqs = torch.outer(t, inv_freq)
            if self.scale is None:
                self._cos_cached = torch.cos(freqs).to(dtype)
                self._sin_cached = torch.sin(freqs).to(dtype)
            else:
                power = (
                    torch.arange(seqlen, dtype=self.scale.dtype, device=self.scale.device) - seqlen // 2
                ) / self.scale_base
                scale = self.scale.to(device=power.device) ** rearrange(power, "s -> s 1")
                # We want the multiplication by scale to happen in fp32
                self._cos_cached = (torch.cos(freqs) * scale).to(dtype)
                self._sin_cached = (torch.sin(freqs) * scale).to(dtype)
                self._cos_k_cached = (torch.cos(freqs) / scale).to(dtype)
                self._sin_k_cached = (torch.sin(freqs) / scale).to(dtype)

    def forward(
        self,
        qkv: torch.Tensor,
        kv: Optional[torch.Tensor] = None,
        seq_lens: Optional[Union[torch.Tensor, List[int]]] = None,
        seqlen_offset: Union[int, torch.Tensor] = 0,
        max_seqlen: Optional[int] = None,
    ) -> Union[torch.Tensor, Tuple[torch.Tensor, torch.Tensor]]:
        """
        qkv: (batch, seqlen, 3, nheads, headdim) if kv is none,
             else it's just q of shape (batch, seqlen, nheads, headdim)
        kv: (batch, seqlen, 2, nheads, headdim)
        seq_lens: packed list of sequence lengths for train
        seqlen_offset: (batch_size,) or int. Each sequence in x is shifted by this amount.
            Most commonly used in inference when we have KV cache.
            If it's a tensor of shape (batch_size,), then to update the cos / sin cache, one
            should pass in max_seqlen, which will update the cos / sin cache up to that length.
        Apply rotary embedding *inplace* to qkv and / or kv.
        """
        seqlen = qkv.shape[1]
        # Handle variable length sequences if provided
        if seq_lens is not None:
            self._update_varlen_cos_sin_cache(seq_lens, device=qkv.device, dtype=qkv.dtype)
        elif max_seqlen is not None:
            self._update_cos_sin_cache(max_seqlen, device=qkv.device, dtype=qkv.dtype)
        elif isinstance(seqlen_offset, int):
            self._update_cos_sin_cache(seqlen + seqlen_offset, device=qkv.device, dtype=qkv.dtype)
        if kv is None:
            if self.scale is None:
                return apply_rotary_emb_qkv_(
                    qkv,
                    self._cos_cached,
                    self._sin_cached,
                    interleaved=self.interleaved,
                    seqlen_offsets=seqlen_offset,
                )
            else:
                return apply_rotary_emb_qkv_(
                    qkv,
                    self._cos_cached,
                    self._sin_cached,
                    self._cos_k_cached,
                    self._sin_k_cached,
                    interleaved=self.interleaved,
                    seqlen_offsets=seqlen_offset,
                )
        else:
            q = qkv
            q = apply_rotary_emb_func(
                q,
                self._cos_cached,
                self._sin_cached,
                interleaved=self.interleaved,
                inplace=True,
                seqlen_offsets=seqlen_offset,
            )
            if self.scale is None:
                kv = apply_rotary_emb_kv_(
                    kv,
                    self._cos_cached,
                    self._sin_cached,
                    interleaved=self.interleaved,
                    seqlen_offsets=seqlen_offset,
                )
            else:
                kv = apply_rotary_emb_kv_(
                    kv,
                    self._cos_k_cached,
                    self._sin_k_cached,
                    interleaved=self.interleaved,
                    seqlen_offsets=seqlen_offset,
                )
            return q, kv