# note for davis dataloader later: temporally consistent depth estimator: https://github.com/yu-li/TCMonoDepth
# note for cool idea of not even downloading data and just streaming from youtube:https://gist.github.com/Mxhmovd/41e7690114e7ddad8bcd761a76272cc3
import matplotlib.pyplot as plt; 
import cv2
import os
import statistics 
import multiprocessing as mp
import torch.nn.functional as F
import torch
import random
import imageio
import numpy as np
from glob import glob
from collections import defaultdict
from pdb import set_trace as pdb
from itertools import combinations
from random import choice
import matplotlib.pyplot as plt
import imageio.v3 as iio

from torchvision import transforms

import sys

from glob import glob
import os
import gzip
import json
import numpy as np

import torchvision.transforms as transforms
from PIL import Image
import pickle
class TmpTraj(torch.utils.data.Dataset):

    def __init__( self, path=".",val=False):
        #self.demonstration_traj=torch.load("/home/cameronsmith/demonstration_traj.pt")
        #self.demonstration_rgb=np.array(torch.load("/home/cameronsmith/demonstration_frames.pt",weights_only=False))[:,0]
        #padded_traj=np.vstack([self.demonstration_traj, np.tile(self.demonstration_traj[-1], (11, 1))])
        #self.output_pairs=[padded_traj[i:i+10] for i,obs in enumerate(self.demonstration_rgb)]
        #self.coin_obs=[torch.load(x) for x in sorted(glob("/home/cameronsmith/cube_picking_real_test_data/data/*/*_packed_obs.pt"))]
        self.coin_obs=[torch.load(x) for x in sorted(glob("/data/cameron/robot_datasets/multimodal_cube_test/data/data/*/*multi*.pt"))]
        #self.coin_obs=[torch.load(x) for x in sorted(glob("/data/cameron/robot_datasets/multimodal_cube_test/data/data/*/*obs*.pt"))]
        self.coin_trajs=[torch.load(x) for x in sorted(glob("/data/cameron/robot_datasets/multimodal_cube_test/data/data/*/*_joint_state.pt"))]
        #"/home/cameronsmith/coin_task/data/06_24_2025_17_03_27/0979_joint_state.pt"
        #"/home/cameronsmith/coin_task/data/06_24_2025_17_03_27/0979_packed_obs.pt"
    
    def __len__(self): return 100000000
    def __getitem__(self, idx):
        idx=np.random.randint(len(self.coin_obs))
        coin_obs,coin_traj=self.coin_obs[idx],self.coin_trajs[idx]
        t=np.random.randint(len(coin_obs))
        padded_traj=np.vstack([coin_traj, np.tile(coin_traj[-1], (5, 1))])

        targ_traj=torch.from_numpy(padded_traj[t:t+5])
        #rgb=F.interpolate(torch.from_numpy(coin_obs[t][0]).permute(2,0,1)[:3][None],(256,256))[0]/255
        #return {"rgb_img":rgb,"joint_state":targ_traj}
        
        rgb=F.interpolate(torch.from_numpy(coin_obs[t]["rgb"]).permute(2,0,1)[:3][None],(256,256))[0].float()#/255
        pc_render=F.interpolate(torch.from_numpy(coin_obs[t]["pc_render"]).permute(2,0,1)[:3][None],(256,256))[0].float()#/255
        return {"rgb_img":rgb,"pc_render_img":pc_render,"rob_pc":torch.from_numpy(coin_obs[t]["rob_pc"]).float(),"cam_pc":torch.from_numpy(coin_obs[t]["cam_pc"]).float(),"joint_state":targ_traj}
class MultiModalFolder(torch.utils.data.Dataset):

    def __init__( self, path=".",val=False):
        self.scene_paths=glob("/data/cameron/robot_datasets/simple_cube_grasp_infront_real_data/data/*/*multi*")
    
    def __len__(self): return 100000000
    def format_sample(self,data_sample_):
        data_sample = {k:torch.from_numpy(v).float() for k,v in data_sample_.items()}
        #for k,v in data_sample_[0][time_i]
        for k in ["rgb","pc_render"]:data_sample[k+"_img"]=F.interpolate(data_sample.pop(k).permute(2,0,1)[:3][None],(480, 640))[0]
        return data_sample
    def __getitem__(self, idx):
        data_sample=torch.load(random.choice(self.scene_paths))
        data_sample=self.format_sample(data_sample)
            
        return data_sample

class MultiModalFolderSim(torch.utils.data.Dataset):

    def __init__( self, path=".",val=False):
        self.scene_paths=glob("/data/cameron/robot_datasets/study/sim_reach/*")
    
    def __len__(self): return 100000000
    def format_sample(self,data_sample):
        if "joint_state" in data_sample:data_sample["joint_state"]=np.array(data_sample["joint_state"])
        if "end_link" in data_sample:data_sample["end_link"]=np.array(list(data_sample["end_link"]))
        data_sample = {k:torch.from_numpy(v).float() for k,v in data_sample.items()}
        for k in ["rgb","rerender_rgb", "pointmap_cam","pointmap_rob"]:data_sample[k+"_img"]=F.interpolate(data_sample.pop(k).permute(2,0,1)[:3][None],(480, 640))[0]
        return data_sample
    def __getitem__(self, idx):
        #try: 
        data_sample=pickle.load(open(random.choice(self.scene_paths),"rb"))
        #except: return self[0]
        data_sample=self.format_sample(data_sample)
            
        return data_sample


class ImageFolder(torch.utils.data.Dataset):

    def __init__( self, path=".",val=False):
        self.scene_paths=glob("/data/cameron/robot_datasets/smaller_real_pickup/*/*.png")
    
    def __len__(self): return 100000000
    def __getitem__(self, idx):
        path=random.choice(self.scene_paths)
        img=torch.from_numpy(plt.imread(path))[...,:3].permute(2,0,1)
        joint_states=torch.from_numpy(np.load(path.replace(".png",".npy"))).float()
        datas={ "joint_states":joint_states, "img":img }
            
        return datas