# Adopted from https://github.com/lm-sys/FastChat. Below is the original copyright:
# Adopted from tatsu-lab@stanford_alpaca. Below is the original copyright:
#    Copyright 2023 Rohan Taori, Ishaan Gulrajani, Tianyi Zhang, Yann Dubois, Xuechen Li
#
#    Licensed under the Apache License, Version 2.0 (the "License");
#    you may not use this file except in compliance with the License.
#    You may obtain a copy of the License at
#
#        http://www.apache.org/licenses/LICENSE-2.0
#
#    Unless required by applicable law or agreed to in writing, software
#    distributed under the License is distributed on an "AS IS" BASIS,
#    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#    See the License for the specific language governing permissions and
#    limitations under the License.
from piper_sdk import *
import os
import copy
from dataclasses import dataclass, field
import json
import logging
import pathlib
from typing import Dict, Optional, Sequence, List
# import librosa
from torch.utils.data import Subset
import torch
import math
import transformers
from transformers import AddedToken
import tokenizers

from llava.constants import IGNORE_INDEX, IMAGE_TOKEN_INDEX, DEFAULT_IMAGE_TOKEN, DEFAULT_IM_START_TOKEN, DEFAULT_IM_END_TOKEN, DEFAULT_AUDIO_TOKEN, DEFAULT_GOAL_TOKEN
from torch.utils.data import Dataset
from llava.train.llava_trainer import LLaVATrainer

from llava import conversation as conversation_lib
from llava.model import *
from llava.mm_utils import tokenizer_image_token, tokenizer_audio_token, tokenizer_image_audio_token
from llava.action_tokenizer import ActionTokenizer, encode_actions, encode_robot_obs,encode_robot_obs_forpipper,encode_actions_forpipper

from PIL import Image
from functools import partial
from torch.utils.data import ConcatDataset
######### real dataset  dependence
import contextlib
import logging
import shutil
from pathlib import Path
from typing import Callable

import datasets
import numpy as np
import packaging.version
import PIL.Image
import torch
import torch.utils
from datasets import concatenate_datasets, load_dataset
from huggingface_hub import HfApi, snapshot_download
from huggingface_hub.constants import REPOCARD_NAME
from huggingface_hub.errors import RevisionNotFoundError

from lerobot.common.constants import HF_LEROBOT_HOME
from lerobot.common.datasets.compute_stats import aggregate_stats, compute_episode_stats
from lerobot.common.datasets.image_writer import AsyncImageWriter, write_image
from lerobot.common.datasets.lerobot_dataset import LeRobotDataset, LeRobotDatasetMetadata
from lerobot.common.datasets.utils import (
    DEFAULT_FEATURES,
    DEFAULT_IMAGE_PATH,
    INFO_PATH,
    TASKS_PATH,
    append_jsonlines,
    backward_compatible_episodes_stats,
    check_delta_timestamps,
    check_timestamps_sync,
    check_version_compatibility,
    create_empty_dataset_info,
    create_lerobot_dataset_card,
    embed_images,
    get_delta_indices,
    get_episode_data_index,
    get_features_from_robot,
    get_hf_features_from_features,
    get_safe_version,
    hf_transform_to_torch,
    is_valid_version,
    load_episodes,
    load_episodes_stats,
    load_info,
    load_stats,
    load_tasks,
    validate_episode_buffer,
    validate_frame,
    write_episode,
    write_episode_stats,
    write_info,
    write_json,
)
from lerobot.common.datasets.video_utils import (
    VideoFrame,
    decode_video_frames,
    encode_video_frames,
    get_safe_default_codec,
    get_video_info,
)
from lerobot.common.robot_devices.robots.utils import Robot

CODEBASE_VERSION = "v2.1"


#########

TARGET_IMG_SIZE = 334


local_rank = None


def rank0_print(*args):
    if local_rank == 0:
        print(*args)


from packaging import version
IS_TOKENIZER_GREATER_THAN_0_14 = version.parse(tokenizers.__version__) >= version.parse('0.14')


@dataclass
class ModelArguments:
    model_name_or_path: Optional[str] = field(default="facebook/opt-125m")
    version: Optional[str] = field(default="v0")
    freeze_backbone: bool = field(default=False)
    tune_mm_mlp_adapter: bool = field(default=False)#视觉到文本模块
    vision_tower: Optional[str] = field(default=None)
    mm_vision_select_layer: Optional[int] = field(default=-1)   # default to the last layer
    pretrain_mm_mlp_adapter: Optional[str] = field(default=None)
    mm_projector_type: Optional[str] = field(default='linear')
    mm_use_im_start_end: bool = field(default=False)
    mm_use_im_patch_token: bool = field(default=True)
    mm_patch_merge_type: Optional[str] = field(default='flat')
    mm_vision_select_feature: Optional[str] = field(default="patch")

    # Add for audio modality
    tune_audio_adapter: bool = field(default=False)
    audio_tower: Optional[str] = field(default=None)
    mm_audio_select_layer: Optional[int] = field(default=-1)
    pretrain_mm_mlp_adapter_audio: Optional[str] = field(default=None)
    mm_projector_type_audio: Optional[str] = field(default='mlp2x_gelu')


@dataclass
class DataArguments:
    data_path: str = field(default=None,
                           metadata={"help": "Path to the training data."})
    lazy_preprocess: bool = False
    is_multimodal: bool = False
    image_folder: Optional[str] = field(default=None)
    image_aspect_ratio: str = 'square'

    # Add for audio modality
    is_multimodal_audio: bool = False
    audio_folder: Optional[str] = field(default=None)
    audio_folder_asr: Optional[str] = field(default=None)
    action_stat: str = field(default=None)


@dataclass
class TrainingArguments(transformers.TrainingArguments):
    cache_dir: Optional[str] = field(default=None)
    optim: str = field(default="adamw_torch")
    remove_unused_columns: bool = field(default=False)
    freeze_mm_mlp_adapter: bool = field(default=False)
    mpt_attn_impl: Optional[str] = field(default="triton")
    model_max_length: int = field(
        default=512,
        metadata={
            "help":
            "Maximum sequence length. Sequences will be right padded (and possibly truncated)."
        },
    )
    double_quant: bool = field(
        default=True,
        metadata={"help": "Compress the quantization statistics through double quantization."}
    )
    quant_type: str = field(
        default="nf4",
        metadata={"help": "Quantization data type to use. Should be one of `fp4` or `nf4`."}
    )
    bits: int = field(
        default=16,
        metadata={"help": "How many bits to use."}
    )
    lora_enable: bool = False
    lora_r: int = 64
    lora_alpha: int = 16
    lora_dropout: float = 0.05
    lora_weight_path: str = ""
    lora_bias: str = "none"
    mm_projector_lr: Optional[float] = None
    group_by_modality_length: bool = field(default=False)

    # Add for audio modality
    freeze_audio_adapter: bool = field(default=False)
    report_to_wandb_project: Optional[str] = field(default=None)
    report_to_wandb_run_name: Optional[str] = field(default=None)

    per_device_eval_batch_size: int = field(default=4)  # 每个 device 上评估 batch 大小
    eval_steps: int = field(default=1000)
    evaluation_strategy: str = field(default="steps")
    max_eval_samples: Optional[int] = field(default=10)


def maybe_zero_3(param, ignore_status=False, name=None):
    from deepspeed import zero
    from deepspeed.runtime.zero.partition_parameters import ZeroParamStatus
    if hasattr(param, "ds_id"):
        if param.ds_status == ZeroParamStatus.NOT_AVAILABLE:
            if not ignore_status:
                logging.warning(f"{name}: param.ds_status != ZeroParamStatus.NOT_AVAILABLE: {param.ds_status}")
        with zero.GatheredParameters([param]):
            param = param.data.detach().cpu().clone()
    else:
        param = param.detach().cpu().clone()
    return param


# Borrowed from peft.utils.get_peft_model_state_dict
def get_peft_state_maybe_zero_3(named_params, bias):
    if bias == "none":
        to_return = {k: t for k, t in named_params if "lora_" in k}
    elif bias == "all":
        to_return = {k: t for k, t in named_params if "lora_" in k or "bias" in k}
    elif bias == "lora_only":
        to_return = {}
        maybe_lora_bias = {}
        lora_bias_names = set()
        for k, t in named_params:
            if "lora_" in k:
                to_return[k] = t
                bias_name = k.split("lora_")[0] + "bias"
                lora_bias_names.add(bias_name)
            elif "bias" in k:
                maybe_lora_bias[k] = t
        for k, t in maybe_lora_bias:
            if bias_name in lora_bias_names:
                to_return[bias_name] = t
    else:
        raise NotImplementedError
    to_return = {k: maybe_zero_3(v, ignore_status=True) for k, v in to_return.items()}
    return to_return


def get_peft_state_non_lora_maybe_zero_3(named_params, require_grad_only=True):
    to_return = {k: t for k, t in named_params if "lora_" not in k}
    if require_grad_only:
        to_return = {k: t for k, t in to_return.items() if t.requires_grad}
    to_return = {k: maybe_zero_3(v, ignore_status=True).cpu() for k, v in to_return.items()}
    return to_return


def get_mm_adapter_state_maybe_zero_3(named_params, keys_to_match):
    to_return = {k: t for k, t in named_params if any(key_match in k for key_match in keys_to_match)}
    to_return = {k: maybe_zero_3(v, ignore_status=True).cpu() for k, v in to_return.items()}
    return to_return


def find_all_linear_names(model):
    cls = torch.nn.Linear
    lora_module_names = set()
    multimodal_keywords = ['mm_projector', 'vision_tower', 'vision_resampler']
    for name, module in model.named_modules():
        if any(mm_keyword in name for mm_keyword in multimodal_keywords):
            continue
        if isinstance(module, cls):
            names = name.split('.')
            lora_module_names.add(names[0] if len(names) == 1 else names[-1])

    if 'lm_head' in lora_module_names: # needed for 16-bit
        lora_module_names.remove('lm_head')
    return list(lora_module_names)


def safe_save_model_for_hf_trainer(trainer: transformers.Trainer,
                                   output_dir: str):
    """Collects the state dict and dump to disk."""

    if getattr(trainer.args, "tune_audio_adapter", False):
        # Only save Adapter
        keys_to_match = ['mm_projector_audio']
        if getattr(trainer.args, "use_im_start_end", False):
            keys_to_match.extend(['embed_tokens', 'embed_in'])

        weight_to_save = get_mm_adapter_state_maybe_zero_3(trainer.model.named_parameters(), keys_to_match)
        trainer.model.config.save_pretrained(output_dir)

        current_folder = output_dir.split('/')[-1]
        parent_folder = os.path.dirname(output_dir)
        if trainer.args.local_rank == 0 or trainer.args.local_rank == -1:
            if current_folder.startswith('checkpoint-'):
                mm_projector_folder = os.path.join(parent_folder, "mm_projector_audio")
                os.makedirs(mm_projector_folder, exist_ok=True)
                torch.save(weight_to_save, os.path.join(mm_projector_folder, f'{current_folder}.bin'))
            else:
                torch.save(weight_to_save, os.path.join(output_dir, f'mm_projector_audio.bin'))
        return

    if trainer.deepspeed:
        print("deepspeed_save")
        torch.cuda.synchronize()
        trainer.save_model(output_dir)
        return

    state_dict = trainer.model.state_dict()
    if trainer.args.should_save:
        cpu_state_dict = {
            key: value.cpu()
            for key, value in state_dict.items()
        }
        del state_dict
        trainer._save(output_dir, state_dict=cpu_state_dict)  # noqa


def smart_tokenizer_and_embedding_resize(
    special_tokens_dict: Dict,
    tokenizer: transformers.PreTrainedTokenizer,
    model: transformers.PreTrainedModel,
):
    """Resize tokenizer and embedding.

    Note: This is the unoptimized version that may make your embedding size not be divisible by 64.
    """
    num_new_tokens = tokenizer.add_special_tokens(special_tokens_dict)
    model.resize_token_embeddings(len(tokenizer))

    if num_new_tokens > 0:
        input_embeddings = model.get_input_embeddings().weight.data
        output_embeddings = model.get_output_embeddings().weight.data

        input_embeddings_avg = input_embeddings[:-num_new_tokens].mean(
            dim=0, keepdim=True)
        output_embeddings_avg = output_embeddings[:-num_new_tokens].mean(
            dim=0, keepdim=True)

        input_embeddings[-num_new_tokens:] = input_embeddings_avg
        output_embeddings[-num_new_tokens:] = output_embeddings_avg





def mask_target_labels(conversations, conv, targets, tokenizer):
    """Mask the target labels for autoregressive training.

    Args:
        conversations (List[Str]): The formatted dialogue.
        conv (_type_): The conversation template.
        targets (torch.Tensor): The unmasked training labels.
        tokenizer (_type_): The tokenizer.

    Returns:
        torch.Tensor: The masked training labels.
    """
    sep = conv.sep + conv.roles[1] + ": "
    for conversation, target in zip(conversations, targets):
        total_len = int(target.ne(tokenizer.pad_token_id).sum())

        rounds = conversation.split(conv.sep2)
        cur_len = 1
        target[:cur_len] = IGNORE_INDEX
        for i, rou in enumerate(rounds):
            if rou == "":
                break

            parts = rou.split(sep)
            if len(parts) != 2:
                break
            parts[0] += sep

            if '<image>' in rou and not '<audio>' in rou:
                round_len = len(tokenizer_image_token(rou, tokenizer))                   # 统计长度时多了<s>, 但少了</s>, 所以不变
                instruction_len = len(tokenizer_image_token(parts[0], tokenizer)) - 2    # 统计长度时多了<s>, 且': '在句尾时多了' ', 所以减2
            elif '<audio>' in rou and not '<image>' in rou:
                round_len = len(tokenizer_audio_token(rou, tokenizer))
                instruction_len = len(tokenizer_audio_token(parts[0], tokenizer)) - 2
            elif '<audio>' in rou and '<image>' in rou:
                round_len = len(tokenizer_audio_token(rou, tokenizer))
                instruction_len = len(tokenizer_image_audio_token(parts[0], tokenizer)) - 2
            else:
                round_len = len(tokenizer(rou).input_ids)
                instruction_len = len(tokenizer(parts[0]).input_ids) - 2

            if i != 0 and not tokenizer.legacy and IS_TOKENIZER_GREATER_THAN_0_14:       # USER正常tokenize为两个token，但其前有特殊字符，如</s>时会tokenize为1个token，所以减1
                round_len -= 1
                instruction_len -= 1

            target[cur_len : cur_len + instruction_len] = IGNORE_INDEX
            cur_len += round_len

        target[cur_len:] = IGNORE_INDEX
        if cur_len < tokenizer.model_max_length:
            if cur_len != total_len:
                target[:] = IGNORE_INDEX
                print(
                    f"WARNING: tokenization mismatch: {cur_len} vs. {total_len}."
                    f" (ignored)"
                )

    return targets


class LeRobotDataset_Llava(LeRobotDataset):
    """Dataset for supervised fine-tuning."""

    def __init__(
        self, 
        repo_id: str,
        root: str | Path | None = None,
        episodes: list[int] | None = None,
        image_transforms: Callable | None = None,
        delta_timestamps: dict[list[float]] | None = None,
        tolerance_s: float = 1e-4,
        revision: str | None = None,
        force_cache_sync: bool = False,
        download_videos: bool = True,
        video_backend: str | None = None,

        # your own fields
        tokenizer: transformers.PreTrainedTokenizer | None = None,
        action_tokenizer: ActionTokenizer | None = None,
        data_args: DataArguments | None = None,
    ):
        # 调用父类构造函数，传入所需参数
        super().__init__(
            repo_id=repo_id,
            root=root,
            episodes=episodes,
            image_transforms=image_transforms,
            delta_timestamps=delta_timestamps,
            tolerance_s=tolerance_s,
            revision=revision,
            force_cache_sync=force_cache_sync,
            download_videos=download_videos,
            video_backend=video_backend,
        )

        # 初始化子类特有的部分

        self.tokenizer = tokenizer
        self.action_tokenizer = action_tokenizer
        self.data_args = data_args
        self.fk_solver = C_PiperForwardKinematics()
    def __getitem__(self, idx) -> dict:
        item = self.hf_dataset[idx]
        ep_idx = item["episode_index"].item()

        query_indices = None
        if self.delta_indices is not None:
            query_indices, padding = self._get_query_indices(idx, ep_idx)
            query_result = self._query_hf_dataset(query_indices)
            item = {**item, **padding}
            for key, val in query_result.items():
                item[key] = val

        if len(self.meta.video_keys) > 0:
            current_ts = item["timestamp"].item()
            query_timestamps = self._get_query_timestamps(current_ts, query_indices)
            video_frames = self._query_videos(query_timestamps, ep_idx)
            item = {**video_frames, **item}
        def tensor_to_pil(img_tensor: torch.Tensor) -> Image.Image:
            # C x H x W → H x W x C
            if img_tensor.dim() == 3 and img_tensor.shape[0] == 3:
                img_tensor = img_tensor.permute(1, 2, 0)
            # print("img_tensor",img_tensor)
                # print("img_tensor.dim() == 3 and img_tensor.shape[0] == 3")
            # Tensor → NumPy (0-255) uint8
            img_np = (img_tensor * 255).clamp(0, 255).byte().cpu().numpy()
            return Image.fromarray(img_np)

        # === 改成这样 ===
        image_gripper = item["observation.images.one"]
        image_static = item["observation.images.two"]

        # print("image_gripper:", image_gripper.shape)

        img_static = tensor_to_pil(image_static)
        img_static = img_static.resize((TARGET_IMG_SIZE, TARGET_IMG_SIZE // 2), Image.LANCZOS)

        img_gripper = tensor_to_pil(image_gripper)
        img_gripper = img_gripper.resize((TARGET_IMG_SIZE, TARGET_IMG_SIZE // 2), Image.LANCZOS)

        img_concat = Image.new("RGB", (TARGET_IMG_SIZE, TARGET_IMG_SIZE))
        img_concat.paste(img_static, (0, 0))
        img_concat.paste(img_gripper, (0, TARGET_IMG_SIZE // 2))
        # img_concat.save("/data/user/user68/project/vlas/debug/img_concat.jpg")
        processor = self.data_args.image_processor
        image = processor.preprocess(img_concat, return_tensors='pt')['pixel_values'][0]

        if self.image_transforms is not None:
            print("self.image_transforms:",self.image_transforms)
            image_keys = self.meta.camera_keys
            for cam in image_keys:
                item[cam] = self.image_transforms(item[cam])

        # Add task as a string
        task_idx = item["task_index"].item()
        item["task"] = self.meta.tasks[task_idx]
        instruction= self.meta.tasks[task_idx]
        action= item["action"]#torch
        # print("action.shape:",action.shape)
        joint_angles_batch = (action[:, :6] * 0.001*(math.pi/180)).tolist() #转成弧度

        # 批量 forward kinematics
        act_pos = []
        for joint_angles, act_gripper in zip(joint_angles_batch, action[:, 6:]):
            j_pos = self.fk_solver.CalFK(joint_angles)  # returns list of 6 poses
            ee_pose = np.array(j_pos[5]) * 1000 #单位0.001
            act_combined = ee_pose.tolist() + act_gripper.tolist()
            act_pos.extend(act_combined)
        
        conv = conversation_lib.default_conversation.copy()
        observation=item["observation.state"]
        # print("observation:", observation)
        # print("act_pos:", act_pos)
        action=act_pos
        # print("len(action):",len(action))
        inputs=format_source_data_real(instruction,action,observation,conv,self.action_tokenizer)#action 是list torch str 可以
        conversations=[inputs]
        input_ids = torch.stack([tokenizer_image_token(inputs, self.tokenizer, return_tensors='pt') ], dim=0)
        # print("input_ids:",input_ids)
        targets = input_ids.clone()
        # print("input_ids.shape:",input_ids.shape)
        # print("tokenizer.pad_token_id:",self.tokenizer.pad_token_id)
        targets=mask_target_labels(conversations, conv, targets, self.tokenizer)
        # print("targets:",targets)
        data_dict = dict(input_ids=input_ids[0], labels=targets[0])
        data_dict['image'] = image
        # data_dict['action']=action
        # print( "targets.shape",targets.shape)
        # print( "input_ids.shape",input_ids.shape)
        # print( "image.shape",image.shape)
        
        return data_dict
    def get_action(self,idx):
        item=self.hf_dataset[idx]
        ep_idx = item["episode_index"].item()
        query_indices = None
        if self.delta_indices is not None:
            query_indices, padding = self._get_query_indices(idx, ep_idx)
            query_result = self._query_hf_dataset(query_indices)
            item = {**item, **padding}
            for key, val in query_result.items():
                item[key] = val
        action=item["action"]
        return action
def format_source_data_real(instruction,action, observation,conv,  action_tokenizer):

    roles = {"human": conv.roles[0], "gpt": conv.roles[1]}
    conv.system = "A chat between a curious user and an artificial intelligence robot. The robot provides actions to follow out the user's instructions."
    
    robot_obs = robot_obs_lang(observation, action_tokenizer)
    instruction=[DEFAULT_IMAGE_TOKEN,instruction,robot_obs]
    human_sent="\n".join(instruction)
    conv.append_message(conv.roles[0], human_sent)
    sent_value = action_to_lang(action, action_tokenizer)
    conv.append_message(conv.roles[1], sent_value)
    inputs=conv.get_prompt()
    return inputs
    


@dataclass
class DataCollatorForSupervisedDataset(object):
    """Collate examples for supervised fine-tuning."""

    tokenizer: transformers.PreTrainedTokenizer

    def __call__(self, instances: Sequence[Dict]) -> Dict[str, torch.Tensor]:
        input_ids, labels = tuple([instance[key] for instance in instances]
                                  for key in ("input_ids", "labels"))
        input_ids = torch.nn.utils.rnn.pad_sequence(
            input_ids,
            batch_first=True,
            padding_value=self.tokenizer.pad_token_id)
        labels = torch.nn.utils.rnn.pad_sequence(labels,
                                                 batch_first=True,
                                                 padding_value=IGNORE_INDEX)
        input_ids = input_ids[:, :self.tokenizer.model_max_length]
        labels = labels[:, :self.tokenizer.model_max_length]
        batch = dict(
            input_ids=input_ids,
            labels=labels,
            attention_mask=input_ids.ne(self.tokenizer.pad_token_id),
        )

        if 'image' in instances[0]:
            images = [instance['image'] for instance in instances]
            if all(x is not None and x.shape == images[0].shape for x in images):
                batch['images'] = torch.stack(images)
            else:
                batch['images'] = images
        
        if 'audio' in instances[0]:
            audios = [instance['audio'] for instance in instances]
            if all(x is not None and x.shape == audios[0].shape for x in audios):
                batch['audios'] = torch.stack(audios)
            else:
                batch['audios'] = audios

        return batch


def make_supervised_data_module(tokenizer: transformers.PreTrainedTokenizer,
                                action_tokenizer: ActionTokenizer,
                                data_args,
                                do_eval: bool) -> Dict:
    """Make dataset and collator for supervised fine-tuning."""
    delta_timestamps = {
    # loads 4 images: 1 second before current frame, 500 ms before, 200 ms before, and current frame
    # camera_key: [-0.20, 0],
    # loads 8 state vectors: 1.5 seconds before, 1 second before, ... 200 ms, 100 ms, and current frame
    # "observation.state": [0],
    # loads 64 action vectors: current frame, 1 frame in the future, 2 frames, ... 63 frames in the future
    "action": [t / 30 for t in range(5)],
    }
    if do_eval:
        action_not_static=[]
        dataset = LeRobotDataset_Llava(
            repo_id="real_pipper",  # 如果固定任务名，这里不变；否则你也可以根据目录名动态调整
            root=data_args.data_path,       # 每个子任务目录作为 root
            delta_timestamps=delta_timestamps,
            tokenizer=tokenizer,
            action_tokenizer=action_tokenizer,
            data_args=data_args,
            # episodes=train_episodes
        )
        print("dataset[0]:",dataset[0])
        print("len(dataset):",len(dataset))
        count=0
        for i in range(len(dataset)):
            # print("dataset.get_action(i):",dataset.get_action(i))
            action=dataset.get_action(i)
            # print("action.shape:",action.shape)
            if not torch.equal(action[0], action[1]) :
                # print("find not static action:",i)
                action_not_static.append(i)
                count=count+1
        print("after_clean_count:",count)
        spilt_num = len(action_not_static)
        train_num = int(spilt_num * 0.9)
        train_dataset=Subset(dataset,action_not_static[:train_num])
        eval_dataset=Subset(dataset,action_not_static[train_num:])

        # train_dataset = Subset(dataset, list(range(train_num)))
        # eval_dataset = Subset(dataset, list(range(train_num, spilt_num)))
        print("train_dataset[0]:",train_dataset[0])

    else:
        action_notstatic=[]
        train_dataset = LeRobotDataset_Llava(
            repo_id="real_pipper",  # 如果固定任务名，这里不变；否则你也可以根据目录名动态调整
            root="/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06",       # 每个子任务目录作为 root
            delta_timestamps=delta_timestamps,
            tokenizer=tokenizer,
            action_tokenizer=action_tokenizer,
            data_args=data_args
        )
        for i in range(len(train_dataset)):
            action_id=train_dataset[i]['labels'][-36:-1]
            if torch.equal(action_id[-35:-30], action_id[-30:-25]):
                action_notstatic.append(i)
        train_dataset=Subset(train_dataset,action_notstatic)
        eval_dataset=None

    # ✅ 合并所有数据集
  
    # ✅ 构造 collator
    print("len(train_dataset):",len(train_dataset))
    print("len(eval_dataset):",len(eval_dataset))
    data_collator = DataCollatorForSupervisedDataset(tokenizer=tokenizer)
    return dict(
        train_dataset=train_dataset,
        eval_dataset=eval_dataset,
        data_collator=data_collator
    )

# def make_supervised_data_module(tokenizer: transformers.PreTrainedTokenizer,
#                                 action_tokenizer: ActionTokenizer,
#                                 data_args,
#                                 do_eval: bool) -> Dict:
#     """Make dataset and collator for supervised fine-tuning."""
#     delta_timestamps = {
#     # loads 4 images: 1 second before current frame, 500 ms before, 200 ms before, and current frame
#     # camera_key: [-0.20, 0],
#     # loads 8 state vectors: 1.5 seconds before, 1 second before, ... 200 ms, 100 ms, and current frame
#     # "observation.state": [0],
#     # loads 64 action vectors: current frame, 1 frame in the future, 2 frames, ... 63 frames in the future
#     "action": [t / 30 for t in range(5)],
#     }
#     if do_eval:
#         info = load_info(Path("/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06"))
#         print("info:",info)
#         train_range = info["splits"]["train"]  # 例如 "18:20"
#         start, end = map(int, train_range.split(":"))
#         train_episodes = list(range(start, end))
#         train_dataset = LeRobotDataset_Llava(
#             repo_id="real_pipper",  # 如果固定任务名，这里不变；否则你也可以根据目录名动态调整
#             root="/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06",       # 每个子任务目录作为 root
#             delta_timestamps=delta_timestamps,
#             tokenizer=tokenizer,
#             action_tokenizer=action_tokenizer,
#             data_args=data_args,
#             episodes=train_episodes
#         )
#     else:
#         train_dataset = LeRobotDataset_Llava(
#             repo_id="real_pipper",  # 如果固定任务名，这里不变；否则你也可以根据目录名动态调整
#             root="/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06",       # 每个子任务目录作为 root
#             delta_timestamps=delta_timestamps,
#             tokenizer=tokenizer,
#             action_tokenizer=action_tokenizer,
#             data_args=data_args
#         )

#     # ✅ 合并所有数据集
  

#     #  
#     # eval_datasets =[]

#     if do_eval:
#         info = load_info(Path("/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06"))
#         val_range = info["splits"]["val"]  # 例如 "18:20"
#         start, end = map(int, val_range.split(":"))
#         val_episodes = list(range(start, end))
#         eval_dataset = LeRobotDataset_Llava(
#             repo_id="real_pipper",
#             root="/data/user/wsong890/user68/project/lerobot-piper/data_train/base_task06",
#             delta_timestamps=delta_timestamps,
#             tokenizer=tokenizer,
#             action_tokenizer=action_tokenizer,
#             data_args=data_args,
#             episodes=val_episodes
#         )  
#     else :
#         eval_dataset = None
#     # ✅ 构造 collator
#     print("len(train_dataset):",len(train_dataset))
#     print("len(eval_dataset):",len(eval_dataset))
#     data_collator = DataCollatorForSupervisedDataset(tokenizer=tokenizer)
#     return dict(
#         train_dataset=train_dataset,
#         eval_dataset=eval_dataset,
#         data_collator=data_collator
#     )

def make_supervised_data_module_all(tokenizer: transformers.PreTrainedTokenizer,
                                action_tokenizer: ActionTokenizer,
                                data_args,
                                do_eval: bool=True
                                ) -> Dict:
    """先切分再合并 每个子目录下各自过滤静态action并切分train/val 最后合并所有train和val子集。"""

    delta_timestamps = {
        "action": [t / 30 for t in range(5)],
    }

    data_root = Path(data_args.data_path)
    subdirs = [d for d in data_root.iterdir() if d.is_dir()]

    train_datasets = []
    eval_datasets = []

    for subdir in subdirs:
        print(f"📦 加载数据子目录: {subdir}")
        dataset = LeRobotDataset_Llava(
            repo_id="real_pipper",
            root=str(subdir),
            delta_timestamps=delta_timestamps,
            tokenizer=tokenizer,
            action_tokenizer=action_tokenizer,
            data_args=data_args
        )
        action_not_static = []
        for i in range(len(dataset)):
            action = dataset.get_action(i)
            if not torch.equal(action[0], action[1]):
                action_not_static.append(i)
        spilt_num = len(action_not_static)
        if spilt_num == 0:
            continue  # 跳过无有效样本的子集
        train_num = int(spilt_num * 0.9)
        train_dataset = Subset(dataset, action_not_static[:train_num])
        eval_dataset = Subset(dataset, action_not_static[train_num:])
        train_datasets.append(train_dataset)
        eval_datasets.append(eval_dataset)

    # 合并所有train和val子集
    if len(train_datasets) > 0:
        train_dataset_all = ConcatDataset(train_datasets)
    else:
        train_dataset_all = None
    if do_eval and len(eval_datasets) > 0:
        eval_dataset_all = ConcatDataset(eval_datasets)
    else:
        eval_dataset_all = None

    data_collator = DataCollatorForSupervisedDataset(tokenizer=tokenizer)
    print("len(train_dataset):", len(train_dataset_all) if train_dataset_all is not None else 0)
    print("len(eval_dataset):", len(eval_dataset_all) if eval_dataset_all is not None else 0)
    return dict(
        train_dataset=train_dataset_all,
        eval_dataset=eval_dataset_all,
        data_collator=data_collator
    )




def wandb_init_custom(training_args):
    if "wandb" in training_args.report_to:
        assert training_args.report_to_wandb_project is not None
        assert training_args.report_to_wandb_run_name is not None
        os.environ["WANDB_PROJECT"] = training_args.report_to_wandb_project
        os.environ["WANDB_RUN_NAME"] = training_args.report_to_wandb_run_name


def train(attn_implementation=None):
    global local_rank, action_to_lang, robot_obs_lang
    torch.serialization.weights_only = False
    # from builtins import slice
    # from numpy import dtype
    # from deepspeed.runtime.zero.config import ZeroStageEnum
    # from deepspeed.runtime.fp16.loss_scaler import LossScaler

    # torch.serialization.add_safe_globals([
    #     ZeroStageEnum,
    #     LossScaler,
    #     np.core.multiarray._reconstruct,
    #     np.ndarray,
    #     dtype,
    #     slice
    # ])

    parser = transformers.HfArgumentParser(
        (ModelArguments, DataArguments, TrainingArguments))
    model_args, data_args, training_args = parser.parse_args_into_dataclasses()
    # print(model_args)
    local_rank = training_args.local_rank
    compute_dtype = (torch.float16 if training_args.fp16 else (torch.bfloat16 if training_args.bf16 else torch.float32))
    action_to_lang = partial(encode_actions_forpipper, statistics=data_args.action_stat)
    robot_obs_lang = partial(encode_robot_obs_forpipper, statistics=data_args.action_stat)
    # print(training_args)
    # Set env vars for wandb
    wandb_init_custom(training_args)

    bnb_model_from_pretrained_args = {}
    if training_args.bits in [4, 8]:
        from transformers import BitsAndBytesConfig
        bnb_model_from_pretrained_args.update(dict(
            device_map={"": training_args.device},
            load_in_4bit=training_args.bits == 4,
            load_in_8bit=training_args.bits == 8,
            quantization_config=BitsAndBytesConfig(
                load_in_4bit=training_args.bits == 4,
                load_in_8bit=training_args.bits == 8,
                llm_int8_skip_modules=["mm_projector"],
                llm_int8_threshold=6.0,
                llm_int8_has_fp16_weight=False,
                bnb_4bit_compute_dtype=compute_dtype,
                bnb_4bit_use_double_quant=training_args.double_quant,
                bnb_4bit_quant_type=training_args.quant_type # {'fp4', 'nf4'}
            )
        ))

    if model_args.vision_tower is not None:
        if 'mpt' in model_args.model_name_or_path:
            config = transformers.AutoConfig.from_pretrained(model_args.model_name_or_path, trust_remote_code=True)
            config.attn_config['attn_impl'] = training_args.mpt_attn_impl
            model = LlavaMptForCausalLM.from_pretrained(
                model_args.model_name_or_path,
                config=config,
                cache_dir=training_args.cache_dir,
                **bnb_model_from_pretrained_args
            )
        else:
            model = LlavaLlamaForCausalLM.from_pretrained(#加载预训练的模型权重以及其配置。
                model_args.model_name_or_path,
                cache_dir=training_args.cache_dir,
                attn_implementation=attn_implementation,
                torch_dtype=(torch.bfloat16 if training_args.bf16 else None), 
                **bnb_model_from_pretrained_args
            )
    else:
        model = transformers.LlamaForCausalLM.from_pretrained(
            model_args.model_name_or_path,
            cache_dir=training_args.cache_dir,
            attn_implementation=attn_implementation,
            torch_dtype=(torch.bfloat16 if training_args.bf16 else None),
            **bnb_model_from_pretrained_args
        )
    model.config.use_cache = False

    if model_args.freeze_backbone:
        model.model.requires_grad_(False)

    if training_args.bits in [4, 8]:#低比特训练准备
        from peft import prepare_model_for_kbit_training
        model.config.torch_dtype=(torch.float32 if training_args.fp16 else (torch.bfloat16 if training_args.bf16 else torch.float32))
        model = prepare_model_for_kbit_training(model, use_gradient_checkpointing=training_args.gradient_checkpointing)

    if training_args.gradient_checkpointing:#在前向传播时不存储中间激活值。在反向传播时动态重新计算这些激活值
        if hasattr(model, "enable_input_require_grads"):
            model.enable_input_require_grads()
        else:
            def make_inputs_require_grad(module, input, output):
                output.requires_grad_(True)
            model.get_input_embeddings().register_forward_hook(make_inputs_require_grad)

    if training_args.lora_enable:
        from peft import LoraConfig, get_peft_model
        lora_config = LoraConfig(
            r=training_args.lora_r,
            lora_alpha=training_args.lora_alpha,
            target_modules=find_all_linear_names(model),
            lora_dropout=training_args.lora_dropout,
            bias=training_args.lora_bias,
            task_type="CAUSAL_LM",
        )
        if training_args.bits == 16:
            if training_args.bf16:
                model.to(torch.bfloat16)
            if training_args.fp16:
                model.to(torch.float16)
        rank0_print("Adding LoRA adapters...")
        model = get_peft_model(model, lora_config)

    if 'mpt' in model_args.model_name_or_path:
        tokenizer = transformers.AutoTokenizer.from_pretrained(
            model_args.model_name_or_path,
            cache_dir=training_args.cache_dir,
            model_max_length=training_args.model_max_length,
            padding_side="right"
        )
    else:
        tokenizer = transformers.AutoTokenizer.from_pretrained(
            model_args.model_name_or_path,
            cache_dir=training_args.cache_dir,
            model_max_length=training_args.model_max_length,
            padding_side="right",
            use_fast=False,
        )
        action_tokenizer = ActionTokenizer(tokenizer)


    if model_args.version == "v0":
        if tokenizer.pad_token is None:
            smart_tokenizer_and_embedding_resize(
                special_tokens_dict=dict(pad_token="[PAD]"),
                tokenizer=tokenizer,
                model=model,
            )
    elif model_args.version == "v0.5":
        tokenizer.pad_token = tokenizer.unk_token
    else:
        tokenizer.pad_token = tokenizer.unk_token
        if model_args.version in conversation_lib.conv_templates:
            conversation_lib.default_conversation = conversation_lib.conv_templates[model_args.version]
        else:
            conversation_lib.default_conversation = conversation_lib.conv_templates["vicuna_v1"]
    # print(model_args)
    if model_args.vision_tower is not None:
        
        model.get_model().initialize_vision_modules(
            model_args=model_args,
            fsdp=training_args.fsdp
        )
        
        vision_tower = model.get_vision_tower()
        vision_tower.to(dtype=torch.bfloat16 if training_args.bf16 else torch.float16, device=training_args.device)

        data_args.image_processor = vision_tower.image_processor
        data_args.is_multimodal = True

        model.config.image_aspect_ratio = data_args.image_aspect_ratio
        model.config.tokenizer_padding_side = tokenizer.padding_side
        model.config.tokenizer_model_max_length = tokenizer.model_max_length

        model.config.tune_mm_mlp_adapter = training_args.tune_mm_mlp_adapter = model_args.tune_mm_mlp_adapter
        if model_args.tune_mm_mlp_adapter:
            model.requires_grad_(False)
            for p in model.get_model().mm_projector.parameters():
                p.requires_grad = True

        model.config.freeze_mm_mlp_adapter = training_args.freeze_mm_mlp_adapter
        if training_args.freeze_mm_mlp_adapter:
            for p in model.get_model().mm_projector.parameters():
                p.requires_grad = False

        if training_args.bits in [4, 8]:
            model.get_model().mm_projector.to(dtype=compute_dtype, device=training_args.device)

        model.config.mm_use_im_start_end = data_args.mm_use_im_start_end = model_args.mm_use_im_start_end
        model.config.mm_projector_lr = training_args.mm_projector_lr
        training_args.use_im_start_end = model_args.mm_use_im_start_end
        model.config.mm_use_im_patch_token = model_args.mm_use_im_patch_token
        model.initialize_vision_tokenizer(model_args, tokenizer=tokenizer)

    # NOTE: Add tower for audio processing
    if model_args.audio_tower is not None:
        model.get_model().initialize_audio_modules(model_args=model_args)
        
        audio_tower = model.get_audio_tower()
        audio_tower.to(dtype=torch.bfloat16 if training_args.bf16 else torch.float16, device=training_args.device)

        data_args.audio_processor = audio_tower.audio_processor
        data_args.is_multimodal_audio = True

        model.config.tune_audio_adapter = training_args.tune_audio_adapter = model_args.tune_audio_adapter
        if model_args.tune_audio_adapter:
            model.requires_grad_(False)
            for p in model.get_model().mm_projector_audio.parameters():
                p.requires_grad = True
        
        model.config.freeze_audio_adapter = training_args.freeze_audio_adapter
        if training_args.freeze_audio_adapter:
            for p in model.get_model().mm_projector_audio.parameters():
                p.requires_grad = False

    if training_args.bits in [4, 8]:
        from peft.tuners.lora import LoraLayer
        for name, module in model.named_modules():
            if isinstance(module, LoraLayer):
                if training_args.bf16:
                    module = module.to(torch.bfloat16)
            if 'norm' in name:
                module = module.to(torch.float32)
            if 'lm_head' in name or 'embed_tokens' in name:
                if hasattr(module, 'weight'):
                    if training_args.bf16 and module.weight.dtype == torch.float32:
                        module = module.to(torch.bfloat16)
    # print("tokenizer",tokenizer)
    do_eval = training_args.evaluation_strategy != "no"   
    data_module = make_supervised_data_module(tokenizer=tokenizer,
                                              action_tokenizer=action_tokenizer,
                                              data_args=data_args,
                                              do_eval=do_eval)
    # data_module = make_supervised_data_module_all(tokenizer=tokenizer,
    #                                           action_tokenizer=action_tokenizer,
    #                                           data_args=data_args)
    trainer = LLaVATrainer(model=model,
                    tokenizer=tokenizer,
                    args=training_args,
                    **data_module)

    if list(pathlib.Path(training_args.output_dir).glob("checkpoint-*")):
        trainer.train(resume_from_checkpoint=True)
    else:
        trainer.train()
    trainer.save_state()

    model.config.use_cache = True

    if training_args.lora_enable:
        state_dict = get_peft_state_maybe_zero_3(
            model.named_parameters(), training_args.lora_bias
        )
        non_lora_state_dict = get_peft_state_non_lora_maybe_zero_3(
            model.named_parameters()
        )
        if training_args.local_rank == 0 or training_args.local_rank == -1:
            model.config.save_pretrained(training_args.output_dir)
            model.save_pretrained(training_args.output_dir, state_dict=state_dict)
            torch.save(non_lora_state_dict, os.path.join(training_args.output_dir, 'non_lora_trainables.bin'))
    else:
        safe_save_model_for_hf_trainer(trainer=trainer,
                                       output_dir=training_args.output_dir)


if __name__ == "__main__":
    # train(attn_implementation="flash_attention_2")
    train()

