import torch
from flamby.utils import evaluate_model_on_tests

# 2 lines of code to change to switch to another dataset
from flamby.datasets.fed_tcga_brca import (
    BATCH_SIZE,
    LR,
    NUM_EPOCHS_POOLED,
    Baseline,
    BaselineLoss,
    metric,
    NUM_CLIENTS,
    Optimizer,
)
from flamby.datasets.fed_tcga_brca import FedTcgaBrca as FedDataset

# Instantiation of local train set (and data loader)), baseline loss function, baseline model, default optimizer
train_dataset = FedDataset(center=0, train=True, pooled=False)
train_dataloader = torch.utils.data.DataLoader(train_dataset, batch_size=BATCH_SIZE, shuffle=True, num_workers=0)
lossfunc = BaselineLoss()
model = Baseline()
optimizer = Optimizer(model.parameters(), lr=LR)

# Traditional pytorch training loop
for epoch in range(0, NUM_EPOCHS_POOLED):
    for idx, (X, y) in enumerate(train_dataloader):
        optimizer.zero_grad()
        outputs = model(X)
        loss = lossfunc(outputs, y)
        loss.backward()
        optimizer.step()

# Evaluation
# Instantiation of a list of the local test sets
test_dataloaders = [
            torch.utils.data.DataLoader(
                FedDataset(center=i, train=False, pooled=False),
                batch_size=BATCH_SIZE,
                shuffle=False,
                num_workers=0,
            )
            for i in range(NUM_CLIENTS)
        ]
# Function performing the evaluation
dict_cindex = evaluate_model_on_tests(model, test_dataloaders, metric)
print(dict_cindex)

import torch
from flamby.utils import evaluate_model_on_tests

# 2 lines of code to change to switch to another dataset
from flamby.datasets.fed_tcga_brca import (
    BATCH_SIZE,
    LR,
    NUM_EPOCHS_POOLED,
    Baseline,
    BaselineLoss,
    metric,
    NUM_CLIENTS,
    get_nb_max_rounds
)
from flamby.datasets.fed_tcga_brca import FedTcgaBrca as FedDataset

# 1st line of code to change to switch to another strategy
from flamby.strategies.fed_avg import FedAvg as strat

# We loop on all the clients of the distributed dataset and instantiate associated data loaders
train_dataloaders = [
            torch.utils.data.DataLoader(
                FedDataset(center = i, train = True, pooled = False),
                batch_size = BATCH_SIZE,
                shuffle = True,
                num_workers = 0
            )
            for i in range(NUM_CLIENTS)
        ]

lossfunc = BaselineLoss()
m = Baseline()

# Federated Learning loop
# 2nd line of code to change to switch to another strategy (feed the FL strategy the right HPs)
args = {
            "training_dataloaders": train_dataloaders,
            "model": m,
            "loss": lossfunc,
            "optimizer_class": torch.optim.SGD,
            "learning_rate": LR / 10.0,
            "num_updates": 100,
# This helper function returns the number of rounds necessary to perform approximately as many
# epochs on each local dataset as with the pooled training
            "nrounds": get_nb_max_rounds(100),
        }
s = strat(**args)
m = s.run()[0]

# Evaluation
# We only instantiate one test set in this particular case: the pooled one
test_dataloaders = [
            torch.utils.data.DataLoader(
                FedDataset(train = False, pooled = True),
                batch_size = BATCH_SIZE,
                shuffle = False,
                num_workers = 0,
            )
        ]
dict_cindex = evaluate_model_on_tests(m, test_dataloaders, metric)
print(dict_cindex)

class MeshDataBase:
    def __init__(self, obj_list):
        self.infos = {obj['label']: obj for obj in obj_list}
        self.meshes = {l: trimesh.load(obj['mesh_path']) for l, obj in self.infos.items()}

    @staticmethod
    def from_object_ds(object_ds):
        obj_list = [object_ds[n] for n in range(len(object_ds))]
        return MeshDataBase(obj_list)
...

object_ds = BOPObjectDataset(scenario_dir / 'models')
mesh_db = MeshDataBase.from_object_ds(object_ds)

predictor, mesh_db = load_models(coarse_run_id, refiner_run_id, n_workers=n_plotters, object_set=object_set)

[
 {'objects': 
  [
   {'label': 'obj_000001',
    'name': 'obj_000001',
    'TWO': array([[-0.02062261, -0.99870347, -0.04654345, -0.05380909],
           [ 0.99854439, -0.022895  ,  0.04883047,  0.00189095],
           [-0.04983272, -0.04546878,  0.9977229 ,  0.07060698],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'T0O': array([[-0.02062261, -0.99870347, -0.04654345, -0.05380909],
           [ 0.99854439, -0.022895  ,  0.04883047,  0.00189095],
           [-0.04983272, -0.04546878,  0.9977229 ,  0.07060698],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'visib_fract': 0.7769277845777234,
    'id_in_segm': 1,
    'bbox': [347, 210, 467, 374]},
   {'label': 'obj_000006',
    'name': 'obj_000006',
    'TWO': array([[-0.40056693,  0.91475543, -0.05262471,  0.03103553],
           [-0.91622629, -0.39934108,  0.03248866, -0.02365388],
           [ 0.00870386,  0.06123014,  0.9980863 ,  0.01391488],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'T0O': array([[-0.40056693,  0.91475543, -0.05262471,  0.03103553],
           [-0.91622629, -0.39934108,  0.03248866, -0.02365388],
           [ 0.00870386,  0.06123014,  0.9980863 ,  0.01391488],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'visib_fract': 0.9990349353406678,
    'id_in_segm': 2,
    'bbox': [328, 343, 422, 405]},
   {'label': 'obj_000014',
    'name': 'obj_000014',
    'TWO': array([[ 0.24178672, -0.96941339, -0.04215706, -0.05206396],
           [ 0.96977496,  0.2399519 ,  0.0442575 ,  0.0179453 ],
           [-0.03278805, -0.05158388,  0.99813144,  0.16636215],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'T0O': array([[ 0.24178672, -0.96941339, -0.04215706, -0.05206396],
           [ 0.96977496,  0.2399519 ,  0.0442575 ,  0.0179453 ],
           [-0.03278805, -0.05158388,  0.99813144,  0.16636215],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'visib_fract': 0.9938250428816466,
    'id_in_segm': 3,
    'bbox': [372, 143, 490, 241]},
   {'label': 'obj_000019',
    'name': 'obj_000019',
    'TWO': array([[-0.69888905,  0.1926738 , -0.68878937,  0.01412755],
           [ 0.711967  ,  0.27928957, -0.64428215,  0.05127768],
           [ 0.06823575, -0.94067797, -0.33237011,  0.06472594],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'T0O': array([[-0.69888905,  0.1926738 , -0.68878937,  0.01412755],
           [ 0.711967  ,  0.27928957, -0.64428215,  0.05127768],
           [ 0.06823575, -0.94067797, -0.33237011,  0.06472594],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'visib_fract': 0.9890470974808324,
    'id_in_segm': 4,
    'bbox': [419, 222, 527, 410]},
   {'label': 'obj_000020',
    'name': 'obj_000020',
    'TWO': array([[-0.74512542, -0.66691536,  0.00352083,  0.07854437],
           [-0.6669148 ,  0.74507458, -0.00940455, -0.15283599],
           [ 0.00364864, -0.00935569, -0.99995023,  0.01854317],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'T0O': array([[-0.74512542, -0.66691536,  0.00352083,  0.07854437],
           [-0.6669148 ,  0.74507458, -0.00940455, -0.15283599],
           [ 0.00364864, -0.00935569, -0.99995023,  0.01854317],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]),
    'visib_fract': 0.9953060637992145,
    'id_in_segm': 5,
    'bbox': [92, 328, 288, 442]}],
  'camera': 
  {'T0C': array([[-0.0792652 ,  0.241296  , -0.967209  ,  0.946419  ],
          [ 0.996102  ,  0.0568396 , -0.0674529 , -0.02116569],
          [ 0.0386997 , -0.968786  , -0.244861  ,  0.36645836],
          [ 0.        ,  0.        ,  0.        ,  1.        ]]),
   'K': array([[1.066778e+03, 0.000000e+00, 3.129869e+02],
          [0.000000e+00, 1.067487e+03, 2.413109e+02],
          [0.000000e+00, 0.000000e+00, 1.000000e+00]]),
   'TWC': array([[-0.0792652 ,  0.241296  , -0.967209  ,  0.946419  ],
          [ 0.996102  ,  0.0568396 , -0.0674529 , -0.02116569],
          [ 0.0386997 , -0.968786  , -0.244861  ,  0.36645836],
          [ 0.        ,  0.        ,  0.        ,  1.        ]]),
   'resolution': torch.Size([480, 640])},
  'frame_info': 
  {
   'scene_id': 48,
   'cam_id': 'cam',
   'view_id': 1626,
   'cam_name': 'cam',
   'group_id': 0
   }
  },
  ... # other views
]

dataloader = DataLoader(scene_ds, batch_size=batch_size,
						num_workers=n_workers,
						sampler=sampler,
						collate_fn=self.collate_fn)

def collate_fn(self, batch):
	batch_im_id = -1

	cam_infos, K = [], []
	det_infos, bboxes = [], []
	for n, data in enumerate(batch): # normally only one batch
		assert n == 0
		images, masks, obss = data
		for c, obs in enumerate(obss): # iterate along different views
			batch_im_id += 1
			frame_info = obs['frame_info']
			im_info = {k: frame_info[k] for k in ('scene_id', 'view_id', 'group_id')} # info for the image
			im_info.update(batch_im_id=batch_im_id)
			cam_info = im_info.copy() # info for camera

			K.append(obs['camera']['K']) # info for 相机内参
			cam_infos.append(cam_info)

			for o, obj in enumerate(obs['objects']):
				obj_info = dict(
					label=obj['name'],
					score=1.0,
				)
				obj_info.update(im_info) # add key-value pair from im_info to obj_info
				bboxes.append(obj['bbox'])
				det_infos.append(obj_info)

	gt_detections = tc.PandasTensorCollection(
		infos=pd.DataFrame(det_infos),
		bboxes=torch.as_tensor(np.stack(bboxes)),
	) # 包括每一个ground truthdetection的的基本info,和检测框 
	cameras = tc.PandasTensorCollection(
		infos=pd.DataFrame(cam_infos),
		K=torch.as_tensor(np.stack(K)),
	)# 包括每一view 相机的基本info（和detection info相同）,和内参
	data = dict(
		images=images,
		cameras=cameras,
		gt_detections=gt_detections,
	)
	return data

def get_predictions(self, pose_predictor, mv_predictor,
					detections=None,
					n_coarse_iterations=1, n_refiner_iterations=1,
					sv_score_th=0.0, skip_mv=True,
					use_detections_TCO=False):

mv_predictor = MultiviewScenePredictor(mesh_db)

problem = MultiviewRefinement(candidates=candidates_n,
                    cameras=cameras,
	                pairs_TC1C2=pairs_TC1C2,
	                mesh_db=self.mesh_db_ba)
ba_outputs = problem.solve(
	n_iterations=ba_n_iter,
	optimize_cameras=not use_known_camera_poses,
)

def solve(self, sample_n_init=1, **lm_kwargs):
	timer_init = Timer()
	timer_opt = Timer()
	timer_misc = Timer()

	timer_init.start()
	TWO_9d_init, TCW_9d_init = self.robust_initialization_TWO_TCW(n_init=sample_n_init)
	timer_init.pause()

	timer_opt.start()
	TWO_9d_opt, TCW_9d_opt, history = self.optimize_lm(
		TWO_9d_init, TCW_9d_init, **lm_kwargs)
	timer_opt.pause()

	timer_misc.start()
	objects, cameras = self.make_scene_infos(TWO_9d_opt, TCW_9d_opt)
	objects_init, cameras_init = self.make_scene_infos(TWO_9d_init, TCW_9d_init)
	history = self.convert_history(history)
	timer_misc.pause()

	outputs = dict(
		objects_init=objects_init,
		cameras_init=cameras_init,
		objects=objects,
		cameras=cameras,
		history=history,
		time_init=timer_init.stop(),
		time_opt=timer_opt.stop(),
		time_misc=timer_misc.stop(),
	)
	return outputs

python -m cosypose.scripts.run_custom_scenario --scenario=example

def read_list_candidates_cameras(self, data_list, cameras_K_list):
	"""
	Creates a PandasTensorCollection from a list of candidates information.

	Args:
		data_list (list): Each element is a dictionary with keys:
			- "candidates" (list of dict): Each candidate dictionary includes:
				- "label" (str): The label of the object.
				- "score" (float): The confidence score of the object.
				- "pose" (torch.Tensor): A [4, 4] torch.Tensor representing the pose matrix.

	Returns:
		PandasTensorCollection: Contains poses and infos.
	"""
	all_poses = []
	all_infos = []
	all_K = []

	# Initialize view_id to be assigned automatically
	view_id = 0
	scene_id = 0  # Fixed value for scene_id

	for view, K in zip(data_list, cameras_K_list):
		all_K.append(K)
		for candidate in view["candidates"]:
			label = candidate["label"]
			score = candidate["score"]
			pose = candidate["pose"]

			# Append the pose tensor
			all_poses.append(pose)

			# Append the metadata
			all_infos.append({
				"view_id": view_id,
				"scene_id": scene_id,
				"score": score,
				"label": label
			})

		# Increment view_id for the next set of candidates
		view_id += 1

	K_tensor = torch.stack(all_K).to(dtype=torch.float32, device="cuda:0")

	# Stack poses into a single tensor
	poses_tensor = torch.stack(all_poses).to(dtype=torch.float32, device="cuda:0")

	# Create a Pandas DataFrame for infos
	infos_df = pd.DataFrame(all_infos)
	# Return the PandasTensorCollection-like structure
	ptc_candidate = tc.PandasTensorCollection(poses=poses_tensor, infos=infos_df)
	cam_info = infos_df.loc[:,["view_id"]]
	cam_info = cam_info.drop_duplicates()
	ptc_cam = tc.PandasTensorCollection(K=K_tensor, infos=cam_info)
	return ptc_candidate, ptc_cam

# Example usage:
example_data = [
    {
        "candidates": [
            {"label": "obj_000017", "score": 0.829675, "pose": torch.eye(4)},
            {"label": "obj_000010", "score": 0.820436, "pose": torch.eye(4) * 2},
        ]
    },
    {
        "candidates": [
            {"label": "obj_000005", "score": 0.104478, "pose": torch.eye(4) * 3},
        ]
    }
]
example_cameras_K = [
    torch.eye(3),
    torch.eye(3) * 2,
]

cd, cam= read_list_candidates(example_data, example_cameras_K)
cd, cam

predictions = self.mv_predictor.predict_scene_state(candidates, cameras,
									   score_th=self.sv_score_th,
									   use_known_camera_poses=False,
									   ransac_n_iter= self.ransac_n_iter,
									   ransac_dist_threshold= self.ransac_dist_threshold,
									   ba_n_iter= self.ba_n_iter)

objects = predictions['scene/objects']
cameras = predictions['scene/cameras']
reproj = predictions['ba_output']
#print(predictions)
for view_group in np.unique(objects.infos['view_group']):
	objects_ = objects[np.where(objects.infos['view_group'] == view_group)[0]]
	cameras_ = cameras[np.where(cameras.infos['view_group'] == view_group)[0]]
	reproj_ = reproj[np.where(reproj.infos['view_group'] == view_group)[0]]
	objects_ = nms3d(objects_, th= self.nms_th, poses_attr='TWO')

def right_to_left_hand_pose_R(R):
    # 定义反射矩阵
    M = torch.tensor([
        [ 1,  0,  0],
        [ 0, -1,  0],
        [ 0,  0,  1]
    ], dtype=R.dtype)
    
    # 转换旋转矩阵
    R_prime = M @ R @ M
    
    return R_prime

def right_to_left_hand_pose_t(t):
    # 定义反射矩阵
    M = torch.tensor([
        [ 1,  0,  0],
        [ 0, -1,  0],
        [ 0,  0,  1]
    ], dtype=t.dtype)

    # 转换位移向量
    t_prime = M @ t

    return t_prime

import pyRobotiqGripper
import time
from flask import Flask, request

app = Flask(__name__)
gripper = pyRobotiqGripper.RobotiqGripper()
gripper.activate()

@app.route('/open_gripper', methods=['POST'])
def open_gripper():
    # 执行脚本
    # import subprocess
    # subprocess.call(['/path/to/your/script.sh'])
    gripper.open()
    return 'gripper open'
@app.route('/close_gripper', methods=['POST'])
def close_gripper():
    # 执行脚本
    # import subprocess
    # subprocess.call(['/path/to/your/script.sh'])
    gripper.close()
    return 'gripper closed'

if __name__ == '__main__':
    app.run(host='0.0.0.0', port=5000)

# gripper = pyRobotiqGripper.RobotiqGripper()
# gripper.activate()
# gripper.close()
# gripper.open()
# time.sleep(3)
# gripper.close()

# 定义要发送的命令和URL
laptop_ip = "192.168.2.111"
close_url = "http://"+laptop_ip+":5000/close_gripper"
open_url = "http://"+laptop_ip+":5000/open_gripper"

# 使用curl命令通过POST请求发送命令
curl_close_command = [
    'curl', 
    '-X', 'POST', 
    close_url, 
]

curl_open_command = [
    'curl', 
    '-X', 'POST', 
    open_url, 
]

print(subprocess.run(curl_open_command, capture_output=True, text=True))