# Copyright (c) 2015-present, Facebook, Inc.
# All rights reserved.
import os
import random

import torch
torch.fx.wrap('len')

import torch.nn as nn
from functools import partial
from torch import Tensor
from typing import Optional
import torch.utils.checkpoint as checkpoint
import time
from einops import rearrange
from timm.models.vision_transformer import _cfg
from timm.models.registry import register_model
from timm.models.layers import trunc_normal_

from timm.models.layers import DropPath, to_2tuple
from timm.models.vision_transformer import _load_weights


import math
import torch.nn.functional as F
from utils import pi_resize_patch_embed, interpolate_resize_patch_embed
from mamba_ssm.modules.mamba_simple import Mamba

try:
    from mamba_ssm.ops.triton.layernorm import RMSNorm, layer_norm_fn, rms_norm_fn
except ImportError:
    RMSNorm, layer_norm_fn, rms_norm_fn = None, None, None


MODEL_PATH = '/home/VideoMamba/videomamba/video_sm/'
_MODELS = {
    "videomamba_t16_in1k": os.path.join(MODEL_PATH, "videomamba_t16_in1k_res224.pth"),
    "videomamba_s16_in1k": os.path.join(MODEL_PATH, "videomamba_s16_in1k_res224.pth"),
    "videomamba_m16_in1k": os.path.join(MODEL_PATH, "videomamba_m16_in1k_res224.pth"),
}


class Block(nn.Module):
    def __init__(
        self, dim, mixer_cls, norm_cls=nn.LayerNorm, fused_add_norm=False, residual_in_fp32=False,drop_path=0.,
    ):
        """
        Simple block wrapping a mixer class with LayerNorm/RMSNorm and residual connection"

        This Block has a slightly different structure compared to a regular
        prenorm Transformer block.
        The standard block is: LN -> MHA/MLP -> Add.
        [Ref: https://arxiv.org/abs/2002.04745]
        Here we have: Add -> LN -> Mixer, returning both
        the hidden_states (output of the mixer) and the residual.
        This is purely for performance reasons, as we can fuse add and LayerNorm.
        The residual needs to be provided (except for the very first block).
        """
        super().__init__()
        self.residual_in_fp32 = residual_in_fp32
        self.fused_add_norm = fused_add_norm
        self.mixer = mixer_cls(dim)
        # print(self.mixer)
        # exit()
        self.norm = norm_cls(dim)
        self.drop_path = DropPath(drop_path) if drop_path > 0. else nn.Identity()
        if self.fused_add_norm:
            assert RMSNorm is not None, "RMSNorm import fails"
            assert isinstance(
                self.norm, (nn.LayerNorm, RMSNorm)
            ), "Only LayerNorm and RMSNorm are supported for fused_add_norm"

    def forward(
        self, hidden_states: Tensor, residual: Optional[Tensor] = None, inference_params=None,
        use_checkpoint=False
    ):
        r"""Pass the input through the encoder layer.

        Args:
            hidden_states: the sequence to the encoder layer (required).
            residual: hidden_states = Mixer(LN(residual))
        """
        if not self.fused_add_norm:
            residual = (residual + self.drop_path(hidden_states)) if residual is not None else hidden_states
            hidden_states = self.norm(residual.to(dtype=self.norm.weight.dtype))
            if self.residual_in_fp32:
                residual = residual.to(torch.float32)
        else:
            fused_add_norm_fn = rms_norm_fn if isinstance(self.norm, RMSNorm) else layer_norm_fn
            hidden_states, residual = fused_add_norm_fn(
                hidden_states if residual is None else self.drop_path(hidden_states),
                self.norm.weight,
                self.norm.bias,
                residual=residual,
                prenorm=True,
                residual_in_fp32=self.residual_in_fp32,
                eps=self.norm.eps,
            )
        if use_checkpoint:
            hidden_states = checkpoint.checkpoint(self.mixer, hidden_states, inference_params)
        else:
            hidden_states = self.mixer(hidden_states, inference_params=inference_params)
        return hidden_states, residual

    def allocate_inference_cache(self, batch_size, max_seqlen, dtype=None, **kwargs):
        return self.mixer.allocate_inference_cache(batch_size, max_seqlen, dtype=dtype, **kwargs)


def create_block(
    d_model,
    ssm_cfg=None,
    norm_epsilon=1e-5,
    drop_path=0.,
    rms_norm=True,
    residual_in_fp32=True,
    fused_add_norm=True,
    layer_idx=None,
    bimamba=True,
    device=None,
    dtype=None,
):
    factory_kwargs = {"device": device, "dtype": dtype}
    if ssm_cfg is None:
        ssm_cfg = {}
    # print(ssm_cfg, dtype)
    # exit()
    mixer_cls = partial(Mamba, layer_idx=layer_idx, bimamba=bimamba, **ssm_cfg, **factory_kwargs)
    norm_cls = partial(nn.LayerNorm if not rms_norm else RMSNorm, eps=norm_epsilon)
    block = Block(
        d_model,
        mixer_cls,
        norm_cls=norm_cls,
        drop_path=drop_path,
        fused_add_norm=fused_add_norm,
        residual_in_fp32=residual_in_fp32,
    )
    block.layer_idx = layer_idx
    return block


# https://github.com/huggingface/transformers/blob/c28d04e9e252a1a099944e325685f14d242ecdcd/src/transformers/models/gpt2/modeling_gpt2.py#L454
def _init_weights(
    module,
    n_layer,
    initializer_range=0.02,  # Now only used for embedding layer.
    rescale_prenorm_residual=True,
    n_residuals_per_layer=1,  # Change to 2 if we have MLP
):
    if isinstance(module, nn.Linear):
        if module.bias is not None:
            if not getattr(module.bias, "_no_reinit", False):
                nn.init.zeros_(module.bias)
    elif isinstance(module, nn.Embedding):
        nn.init.normal_(module.weight, std=initializer_range)

    if rescale_prenorm_residual:
        # Reinitialize selected weights subject to the OpenAI GPT-2 Paper Scheme:
        #   > A modified initialization which accounts for the accumulation on the residual path with model depth. Scale
        #   > the weights of residual layers at initialization by a factor of 1/√N where N is the # of residual layers.
        #   >   -- GPT-2 :: https://openai.com/blog/better-language-models/
        #
        # Reference (Megatron-LM): https://github.com/NVIDIA/Megatron-LM/blob/main/megatron/model/gpt_model.py
        for name, p in module.named_parameters():
            if name in ["out_proj.weight", "fc2.weight"]:
                # Special Scaled Initialization --> There are 2 Layer Norms per Transformer Block
                # Following Pytorch init, except scale by 1/sqrt(2 * n_layer)
                # We need to reinit p since this code could be called multiple times
                # Having just p *= scale would repeatedly scale it down
                nn.init.kaiming_uniform_(p, a=math.sqrt(5))
                with torch.no_grad():
                    p /= math.sqrt(n_residuals_per_layer * n_layer)


def segm_init_weights(m):
    if isinstance(m, nn.Linear):
        trunc_normal_(m.weight, std=0.02)
        if isinstance(m, nn.Linear) and m.bias is not None:
            nn.init.constant_(m.bias, 0)
    elif isinstance(m, nn.LayerNorm):
        nn.init.constant_(m.bias, 0)
        nn.init.constant_(m.weight, 1.0)


class PatchEmbed(nn.Module):
    """ Image to Patch Embedding
    """
    def __init__(self, img_size=224, patch_size=16, kernel_size=1, in_chans=3, embed_dim=768):
        super().__init__()
        img_size = to_2tuple(img_size)
        patch_size = to_2tuple(patch_size)
        num_patches = (img_size[1] // patch_size[1]) * (img_size[0] // patch_size[0])
        self.img_size = img_size
        self.patch_size = patch_size
        self.num_patches = num_patches
        self.tubelet_size = kernel_size

        self.proj = nn.Conv3d(
            in_chans, embed_dim, 
            kernel_size=(kernel_size, patch_size[0], patch_size[1]),
            stride=(kernel_size, patch_size[0], patch_size[1])
        )

    def forward(self, x):
        x = self.proj(x)
        return x


class VisionMamba(nn.Module):
    def __init__(
            self, 
            img_size=224, 
            patch_size=16, 
            depth=24, 
            embed_dim=192, 
            channels=3, 
            num_classes=1000,
            drop_rate=0.,
            drop_path_rate=0.1,
            ssm_cfg=None, 
            norm_epsilon=1e-5, 
            initializer_cfg=None,
            fused_add_norm=True,
            rms_norm=True,
            residual_in_fp32=True,
            bimamba=True,
            # video
            kernel_size=1, 
            num_frames=8, 
            fc_drop_rate=0., 
            device=None,
            dtype=None,
            # checkpoint
            use_checkpoint=False,
            checkpoint_num=0,
            flexible=False,
            spatial_flex=False,
            flexivit=False,
            flex_all=False,
            static_tokens=False
        ):
        factory_kwargs = {"device": device, "dtype": dtype} # follow MambaLMHeadModel
        super().__init__()
        rms_norm = False
        fused_add_norm = False

        self.residual_in_fp32 = residual_in_fp32
        self.fused_add_norm = fused_add_norm
        self.use_checkpoint = use_checkpoint
        self.checkpoint_num = checkpoint_num
        # print('change rms and fused back to ture')
        # time.sleep(2)
        self.flexible = flexible
        self.spatial_flex = spatial_flex
        self.flexivit = flexivit
        self.flex_all = flex_all
        self.static_tokens = static_tokens
        
        print(f'Use checkpoint: {use_checkpoint}')
        print(f'Checkpoint number: {checkpoint_num}')
        print('Flexibility (temporal, spatial, flexivit, all, static_tokens): ', self.flexible, self.spatial_flex, self.flexivit, self.flex_all, self.static_tokens)

        # pretrain parameters
        self.num_classes = num_classes
        self.d_model = self.num_features = self.embed_dim = embed_dim  # num_features for consistency with other models

        self.patch_embed = PatchEmbed(
            img_size=img_size, patch_size=patch_size, 
            kernel_size=kernel_size,
            in_chans=channels, embed_dim=embed_dim
        )
        num_patches = self.patch_embed.num_patches

        self.cls_token = nn.Parameter(torch.zeros(1, 1, self.embed_dim))
        self.pos_embed = nn.Parameter(torch.zeros(1, num_patches + 1, self.embed_dim))
        self.temporal_pos_embedding = nn.Parameter(torch.zeros(1, num_frames // kernel_size, embed_dim))
        self.pos_drop = nn.Dropout(p=drop_rate)

        self.head_drop = nn.Dropout(fc_drop_rate) if fc_drop_rate > 0 else nn.Identity()
        self.head = nn.Linear(self.num_features, num_classes) if num_classes > 0 else nn.Identity()

        dpr = [x.item() for x in torch.linspace(0, drop_path_rate, depth)]  # stochastic depth decay rule
        inter_dpr = [0.0] + dpr
        self.drop_path = DropPath(drop_path_rate) if drop_path_rate > 0. else nn.Identity()
        # mamba blocks
        self.layers = nn.ModuleList(
            [
                create_block(
                    embed_dim,
                    ssm_cfg=ssm_cfg,
                    norm_epsilon=norm_epsilon,
                    rms_norm=rms_norm,
                    residual_in_fp32=residual_in_fp32,
                    fused_add_norm=fused_add_norm,
                    layer_idx=i,
                    bimamba=bimamba,
                    drop_path=inter_dpr[i],
                    **factory_kwargs,
                )
                for i in range(depth)
            ]
        )
        
        # output head
        self.norm_f = (nn.LayerNorm if not rms_norm else RMSNorm)(embed_dim, eps=norm_epsilon, **factory_kwargs)

        # original init
        self.apply(segm_init_weights)
        self.head.apply(segm_init_weights)
        trunc_normal_(self.pos_embed, std=.02)

        # mamba init
        self.apply(
            partial(
                _init_weights,
                n_layer=depth,
                **(initializer_cfg if initializer_cfg is not None else {}),
            )
        )

        self.divisors = {96: [12, 16, 24, 32, 48], 128: [16, 32], 224: [14, 16, 28, 32], \
                          384: [16, 24, 32, 48]} # Pre-compute all divsisors for each of these flexible resolutions. Randomly select one of these divisosrs as a patch size during flex_all training (needs to be double digit to fit on gpu)
        self.mode = 'test'
        self.patch_ablation = {96: 7, 98:7, 112: 8, 128: 9, 192: 14, 224: 16, 288: 21, 384: 27, 448: 32}
        print(f'####### {self.mode} ######')
    def allocate_inference_cache(self, batch_size, max_seqlen, dtype=None, **kwargs):
        return {
            i: layer.allocate_inference_cache(batch_size, max_seqlen, dtype=dtype, **kwargs)
            for i, layer in enumerate(self.layers)
        }

    @torch.jit.ignore
    def no_weight_decay(self):
        return {"pos_embed", "cls_token", "temporal_pos_embedding"}
    
    def get_num_layers(self):
        return len(self.layers)

    @torch.jit.ignore()
    def load_pretrained(self, checkpoint_path, prefix=""):
        _load_weights(self, checkpoint_path, prefix)

    def forward_features(self, x, inference_params=None, vos=False, viz=False):
        input = x.shape
        # print('input: ', input)
        # print('pre: ', self.patch_embed.proj, self.patch_embed.proj.weight.shape)
        # print('flex: ', self.flexivit)
        if self.flexivit:
            reso = x.shape[-1]
            if self.mode == 'test':
                new_size = self.patch_ablation[reso]
            else:
                new_size = random.choice([8, 10, 12, 15, 16, 20, 24, 30, 40, 48])
            
            flex_weight = interpolate_resize_patch_embed(patch_embed=self.patch_embed.proj.weight, new_patch_size=[new_size, new_size])
            # print('flex weight shape: ', flex_weight.shape)
            x = F.conv3d(x, flex_weight, self.patch_embed.proj.bias, stride=[1, new_size, new_size])
            
        elif self.flex_all:
            reso = x.shape[-1]
            if self.mode == 'test':
                new_size = self.patch_ablation[reso]
            else:
                new_size = random.choice(self.divisors[reso])
            # print(reso, new_size)
            flex_weight = interpolate_resize_patch_embed(patch_embed=self.patch_embed.proj.weight,
                                                         new_patch_size=[new_size, new_size])
            # print('flex weight shape: ', flex_weight.shape)
            x = F.conv3d(x, flex_weight, self.patch_embed.proj.bias, stride=[1, new_size, new_size])

        elif self.static_tokens:
            reso = x.shape[-1]
            new_size = reso // 14
            # print(reso, new_size)
            flex_weight = interpolate_resize_patch_embed(patch_embed=self.patch_embed.proj.weight,
                                                         new_patch_size=[new_size, new_size])
            # print('flex weight shape: ', flex_weight)
            x = F.conv3d(x, flex_weight, self.patch_embed.proj.bias, stride=[1, new_size, new_size])
            # print('post static: ', x.shape)
        else:
            x = self.patch_embed(x) # B, D, T, H, W (default 8 x 576 x 16 x 15 x 15)
        # print('post patch embed: ', x.shape)
        # exit()
        B, C, T, H, W = x.shape
        x = x.permute(0, 2, 3, 4, 1).reshape(B * T, H * W, C)
        # print('permute reshape: ', x.shape)

        cls_token = self.cls_token.expand(x.shape[0], -1, -1)  # stole cls_tokens impl from Phil Wang, thanks
        x = torch.cat((cls_token, x), dim=1)
        # print('cls + input: ', x.shape)
        # print('pos embed: ', self.pos_embed.shape)


        if self.flex_all:
            num_extra_tokens = 1  # appended cls token  = +1
            orig_size = int((self.pos_embed.shape[-2] - num_extra_tokens) ** 0.5)  # num spatial tokens
            # height (== width) for the new position embedding
            new_size = int(reso / flex_weight.shape[-1])  # new num spatial tokens, hardcoded 240 img size
            # print(orig_size, new_size)
            embedding_size = self.pos_embed.shape[-1]


        elif self.spatial_flex:
            if vos:
                _B, _C, _T, _H, _W = input
                orig_H = orig_W = 14
                new_size_H = _H // 16
                new_size_W = _W // 16
                # print(orig_H, new_size_H, orig_W, new_size_W)
                num_extra_tokens = 1
                embedding_size = self.pos_embed.shape[-1]
                orig_size = 0
                new_size = 1 # this is just to enter the conditional below for spatial interp

            else:
                orig_size = 14
                # height (== width) for the new position embedding
                new_size = int((x.shape[1] - 1) ** 0.5)
                num_extra_tokens = 1  #appended cls token  = +1
                # print(orig_size, new_size)
                embedding_size = self.pos_embed.shape[-1]

        elif self.flexivit:
            num_extra_tokens = 1  #appended cls token  = +1
            orig_size = int((self.pos_embed.shape[-2] - num_extra_tokens) ** 0.5) # num spatial tokens
            # orig_size = 16 ######################################################################################################################################### CHANGE ########################

            # height (== width) for the new position embedding
            # print(reso, flex_weight.shape[-1])
            new_size = int(reso / flex_weight.shape[-1])  # new num spatial tokens, hardcoded 240 img size
            # print(orig_size, new_size)
            embedding_size = self.pos_embed.shape[-1]
        else:
            orig_size, new_size = -1, -1

        if orig_size != new_size:
            if vos:
                extra_tokens = self.pos_embed[:, :num_extra_tokens]
                # only the position tokens are interpolated
                pos_tokens = self.pos_embed[:, num_extra_tokens:]
                # B, L, C -> B, H, W, C -> B, C, H, W
                pos_tokens = pos_tokens.reshape(-1, orig_H, orig_W, embedding_size).permute(0, 3, 1, 2)
                pos_tokens = torch.nn.functional.interpolate(
                    pos_tokens, size=(new_size_H, new_size_W), mode='bicubic', align_corners=False)
                # B, C, H, W -> B, H, W, C ->  B, H, W, C
                pos_tokens = pos_tokens.permute(0, 2, 3, 1).reshape(-1, new_size_H, new_size_W, embedding_size)
                pos_tokens = pos_tokens.flatten(1, 2)  # B, L, C
                new_pos_embed = torch.cat((extra_tokens, pos_tokens), dim=1)
                # print(x.shape, new_pos_embed.shape, self.pos_embed.shape)
                x = x + new_pos_embed
                # exit()
            else:
                # print("Position interpolate from %d to %d" % (orig_size, new_size))
                extra_tokens = self.pos_embed[:, :num_extra_tokens]
                # only the position tokens are interpolated
                pos_tokens = self.pos_embed[:, num_extra_tokens:]
                # B, L, C -> B, H, W, C -> B, C, H, W
                pos_tokens = pos_tokens.reshape(-1, orig_size, orig_size, embedding_size).permute(0, 3, 1, 2)
                pos_tokens = torch.nn.functional.interpolate(
                    pos_tokens, size=(new_size, new_size), mode='bicubic', align_corners=False)
                # B, C, H, W -> B, H, W, C ->  B, H, W, C
                pos_tokens = pos_tokens.permute(0, 2, 3, 1).reshape(-1, new_size, new_size, embedding_size)
                pos_tokens = pos_tokens.flatten(1, 2)  # B, L, C
                new_pos_embed = torch.cat((extra_tokens, pos_tokens), dim=1)
                # print(x.shape, new_pos_embed)
                x = x + new_pos_embed
        else:
            # print('no spatial interp needed, spatial_flex true', flush=True)
            # print(x.shape, self.pos_embed.shape)
            x = x + self.pos_embed

        # print('post pos embed: ', x.shape)

        # temporal pos
        cls_tokens = x[:B, :1, :]
        x = x[:, 1:]
        # print('post pos embed processed: ', x.shape)
        # print('pos embed shapes: ', self.pos_embed.shape, self.temporal_pos_embedding.shape)
        x = rearrange(x, '(b t) n m -> (b n) t m', b=B, t=T)
        # print('rearranged: ', x.shape)


        if self.flexible:
            flexbile_temporal_pos_embedding = torch.nn.functional.interpolate(
                self.temporal_pos_embedding.permute(0, 2, 1), size=(T,), mode='linear', align_corners=False
            )
            flexbile_temporal_pos_embedding = flexbile_temporal_pos_embedding.permute(0, 2, 1)

            x = x + flexbile_temporal_pos_embedding
        else:
            x = x + self.temporal_pos_embedding

        # print('post temp embed: ', x.shape)
        # exit()
        x = rearrange(x, '(b n) t m -> b (t n) m', b=B, t=T) # B x T * H * W x D
        # print('rearranged: ', x.shape)
        x = torch.cat((cls_tokens, x), dim=1)
        x = self.pos_drop(x)
        # print('final shape before mamba input: ', x.shape)
        # exit()
        # mamba impl
        residual = None
        hidden_states = x
        for idx, layer in enumerate(self.layers):
            # print('idx: ', idx)
            if self.use_checkpoint and idx < self.checkpoint_num:
                hidden_states, residual = layer(
                    hidden_states, residual, inference_params=inference_params,
                    use_checkpoint=True
                )
            else:
                hidden_states, residual = layer(
                    hidden_states, residual, inference_params=inference_params
                )
            # print(f"{idx}=")
            if idx == 31 and vos:
                return hidden_states[:, 1:, :]

        if not self.fused_add_norm:
            if residual is None:
                residual = hidden_states
            else:
                residual = residual + self.drop_path(hidden_states)
            hidden_states = self.norm_f(residual.to(dtype=self.norm_f.weight.dtype))
        else:
            # Set prenorm=False here since we don't need the residual
            fused_add_norm_fn = rms_norm_fn if isinstance(self.norm_f, RMSNorm) else layer_norm_fn
            hidden_states = fused_add_norm_fn(
                self.drop_path(hidden_states),
                self.norm_f.weight,
                self.norm_f.bias,
                eps=self.norm_f.eps,
                residual=residual,
                prenorm=False,
                residual_in_fp32=self.residual_in_fp32,
            )
        if viz:
            return hidden_states
        return hidden_states[:, 0, :]

    # def forward_dense_features(self, x, inference_params=None):

    def forward_image_features(self, x, inference_params=None):
        if self.static_tokens:
            reso = x.shape[-1]
            # new_size = int(reso / 14)
            new_size = 16
            flex_weight = interpolate_resize_patch_embed(patch_embed=self.patch_embed.proj.weight,
                                                         new_patch_size=[new_size, new_size])
            # print('flex weight shape: ', flex_weight)
            x = F.conv3d(x, flex_weight, self.patch_embed.proj.bias, stride=[1, new_size, new_size])
            # print(x.shape)
        else:
            x = self.patch_embed(x)  # B, D, T, H, W (default 8 x 576 x 16 x 15 x 15)


        # exit()

        B, C, T, H, W = x.shape
        x = x.permute(0, 2, 3, 4, 1).reshape(B * T, H * W, C)
        # print('permute reshape: ', x.shape)
        return x

        cls_token = self.cls_token.expand(x.shape[0], -1, -1)  # stole cls_tokens impl from Phil Wang, thanks
        x = torch.cat((cls_token, x), dim=1)
        # print('cls + input: ', x.shape)
        # print('pos embed: ', self.pos_embed.shape)

        if self.flex_all and self.mode == 'train':
            num_extra_tokens = 1  # appended cls token  = +1
            orig_size = int((self.pos_embed.shape[-2] - num_extra_tokens) ** 0.5)  # num spatial tokens
            # height (== width) for the new position embedding
            new_size = int(reso / flex_weight.shape[-1])  # new num spatial tokens, hardcoded 240 img size
            # print(orig_size, new_size)
            embedding_size = self.pos_embed.shape[-1]


        elif self.spatial_flex:
            orig_size = 14
            # height (== width) for the new position embedding
            new_size = int((x.shape[1] - 1) ** 0.5)
            num_extra_tokens = 1  # appended cls token  = +1
            # print(orig_size, new_size)
            embedding_size = self.pos_embed.shape[-1]

        elif self.flexivit:
            num_extra_tokens = 1  # appended cls token  = +1
            orig_size = int((self.pos_embed.shape[-2] - num_extra_tokens) ** 0.5)  # num spatial tokens
            # orig_size = 16 ######################################################################################################################################### CHANGE ########################

            # height (== width) for the new position embedding
            # print(reso, flex_weight.shape[-1])
            new_size = int(reso / flex_weight.shape[-1])  # new num spatial tokens, hardcoded 240 img size
            # print(orig_size, new_size)
            embedding_size = self.pos_embed.shape[-1]
        else:
            orig_size, new_size = -1, -1

        if orig_size != new_size:
            # print("Position interpolate from %d to %d" % (orig_size, new_size))
            extra_tokens = self.pos_embed[:, :num_extra_tokens]
            # only the position tokens are interpolated
            pos_tokens = self.pos_embed[:, num_extra_tokens:]
            # B, L, C -> B, H, W, C -> B, C, H, W
            pos_tokens = pos_tokens.reshape(-1, orig_size, orig_size, embedding_size).permute(0, 3, 1, 2)
            pos_tokens = torch.nn.functional.interpolate(
                pos_tokens, size=(new_size, new_size), mode='bicubic', align_corners=False)
            # B, C, H, W -> B, H, W, C ->  B, H, W, C
            pos_tokens = pos_tokens.permute(0, 2, 3, 1).reshape(-1, new_size, new_size, embedding_size)
            pos_tokens = pos_tokens.flatten(1, 2)  # B, L, C
            new_pos_embed = torch.cat((extra_tokens, pos_tokens), dim=1)
            # print(x.shape, new_pos_embed)
            x = x + new_pos_embed
        else:
            # print('no spatial interp needed, spatial_flex true', flush=True)
            # print(x.shape, self.pos_embed.shape)
            x = x + self.pos_embed

        # temporal pos
        cls_tokens = x[:B, :1, :]
        x = x[:, 1:]
        # print(x.shape)
        x = rearrange(x, '(b t) n m -> (b n) t m', b=B, t=T)

        if self.flexible:
            flexbile_temporal_pos_embedding = torch.nn.functional.interpolate(
                self.temporal_pos_embedding.permute(0, 2, 1), size=(T,), mode='linear', align_corners=False
            )
            flexbile_temporal_pos_embedding = flexbile_temporal_pos_embedding.permute(0, 2, 1)

            x = x + flexbile_temporal_pos_embedding
        else:
            x = x + self.temporal_pos_embedding

        # print(x.shape)
        x = rearrange(x, '(b n) t m -> b (t n) m', b=B, t=T)
        x = torch.cat((cls_tokens, x), dim=1)

        x = self.pos_drop(x)

        # mamba impl
        residual = None
        hidden_states = x
        for idx, layer in enumerate(self.layers):
            if self.use_checkpoint and idx < self.checkpoint_num:
                hidden_states, residual = layer(
                    hidden_states, residual, inference_params=inference_params,
                    use_checkpoint=True
                )
            else:
                hidden_states, residual = layer(
                    hidden_states, residual, inference_params=inference_params
                )

        if not self.fused_add_norm:
            if residual is None:
                residual = hidden_states
            else:
                residual = residual + self.drop_path(hidden_states)
            hidden_states = self.norm_f(residual.to(dtype=self.norm_f.weight.dtype))
        else:
            # Set prenorm=False here since we don't need the residual
            fused_add_norm_fn = rms_norm_fn if isinstance(self.norm_f, RMSNorm) else layer_norm_fn
            hidden_states = fused_add_norm_fn(
                self.drop_path(hidden_states),
                self.norm_f.weight,
                self.norm_f.bias,
                eps=self.norm_f.eps,
                residual=residual,
                prenorm=False,
                residual_in_fp32=self.residual_in_fp32,
            )


        return hidden_states[:, 1:, :]

    def forward(self, x, inference_params=None):
        x = self.forward_features(x, inference_params)
        x = self.head(self.head_drop(x))
        return x


def inflate_weight(weight_2d, time_dim, center=True):
    print(f'Init center: {center}')
    if center:
        weight_3d = torch.zeros(*weight_2d.shape)
        weight_3d = weight_3d.unsqueeze(2).repeat(1, 1, time_dim, 1, 1)
        middle_idx = time_dim // 2
        weight_3d[:, :, middle_idx, :, :] = weight_2d
    else:
        weight_3d = weight_2d.unsqueeze(2).repeat(1, 1, time_dim, 1, 1)
        weight_3d = weight_3d / time_dim
    return weight_3d


def load_state_dict(model, state_dict, center=True):
    state_dict_3d = model.state_dict()
    for k in state_dict.keys():
        if k in state_dict_3d.keys() and state_dict[k].shape != state_dict_3d[k].shape:
            if len(state_dict_3d[k].shape) <= 3:
                print(f'Ignore: {k}')
                continue
            print(f'Inflate: {k}, {state_dict[k].shape} => {state_dict_3d[k].shape}')
            time_dim = state_dict_3d[k].shape[2]
            state_dict[k] = inflate_weight(state_dict[k], time_dim, center=center)
    
    if 'head.weight' in state_dict.keys():
      del state_dict['head.weight']
      del state_dict['head.bias']
    msg = model.load_state_dict(state_dict, strict=False)
    print(msg)


@register_model
def videomamba_tiny(pretrained=False, **kwargs):
    model = VisionMamba(
        patch_size=16, 
        embed_dim=192, 
        depth=24, 
        rms_norm=True, 
        residual_in_fp32=True, 
        fused_add_norm=True, 
        **kwargs
    )
    model.default_cfg = _cfg()
    if pretrained:
        print('load pretrained weights')
        state_dict = torch.load(_MODELS["videomamba_t16_in1k"], map_location='cpu')
        load_state_dict(model, state_dict, center=True)
    return model


@register_model
def videomamba_small(pretrained=False, **kwargs):
    model = VisionMamba(
        patch_size=16, 
        embed_dim=384, 
        depth=24, 
        rms_norm=True, 
        residual_in_fp32=True, 
        fused_add_norm=True, 
        **kwargs
    )
    model.default_cfg = _cfg()
    if pretrained:
        print('load pretrained weights')
        state_dict = torch.load(_MODELS["videomamba_s16_in1k"], map_location='cpu')
        load_state_dict(model, state_dict, center=True)
    return model


@register_model
def videomamba_middle(pretrained=False, **kwargs):
    model = VisionMamba(
        patch_size=16, 
        embed_dim=576, 
        depth=32, 
        rms_norm=True,
        residual_in_fp32=True, 
        fused_add_norm=True,
        **kwargs
    )
    model.default_cfg = _cfg()
    if pretrained:
        print('load pretrained weights')
        state_dict = torch.load(_MODELS["videomamba_m16_in1k"], map_location='cpu')
        load_state_dict(model, state_dict, center=True)
    return model


if __name__ == '__main__':
    import time
    from fvcore.nn import FlopCountAnalysis
    from fvcore.nn import flop_count_table
    import numpy as np

    seed = 4217
    np.random.seed(seed)
    torch.manual_seed(seed)
    torch.cuda.manual_seed(seed)
    torch.cuda.manual_seed_all(seed)
    num_frames = 32
    img_size = 288

    # To evaluate GFLOPs, pleaset set `rms_norm=False` and `fused_add_norm=False`
    spatial_flex = False if img_size == 224 else True
    model = videomamba_middle(num_frames=num_frames, flexible=True, static_tokens=False, spatial_flex=spatial_flex).cuda()
    feat = model(torch.rand(1, 3, num_frames, img_size, img_size).cuda())
    print(feat.shape)
    s = time.time()
    flops = FlopCountAnalysis(model, torch.rand(1, 3, num_frames, img_size, img_size).cuda())
    print(time.time()-s)
    print(flop_count_table(flops, max_depth=1))
    print(num_frames, img_size)

