import json
import numpy as np
import matplotlib.pyplot as plt

# ==== 配置 ====
instance_id = "48"  # 你要对比的物体id

# ==== Occupancy参数：世界边界 ====
with open('test/839920/occupancy.json') as f:
    meta = json.load(f)
x_min, y_min = meta['min'][:2]
x_max, y_max = meta['max'][:2]

# ==== 读入3DGS labels.json ====
with open('test/839920/labels.json') as f:
    labels = json.load(f)
label_obj = next(obj for obj in labels if obj["ins_id"] == instance_id)
# 提取其底面四个点
poly3d = label_obj["bounding_box"]
corners3d = np.array([[v['x'], v['y']] for v in poly3d[:4]]) # 3D底面四点

# ==== 3DGS投影翻转基准 ====
def flip_x(x):
    return x_min + x_max - x
def flip_y(y):
    return y_min + y_max - y

corners3d_flip = np.column_stack([flip_x(corners3d[:,0]), flip_y(corners3d[:,1])])

# ==== 读入2D Map.json ====
with open('2D_Semantic_Map_839920_Physic.json') as f:
    data = json.load(f)
map_obj = next(obj for obj in data if str(obj["instance_id"]) == instance_id)
mask_coords = np.array([[float(y), float(x)] for y, x in map_obj['mask_coords_m']])  # 注意格式[y, x]

bbox2d = np.array([
    [float(map_obj['bbox_m'][1]), float(map_obj['bbox_m'][0])],     # 左下
    [float(map_obj['bbox_m'][3]), float(map_obj['bbox_m'][0])],     # 左上
    [float(map_obj['bbox_m'][3]), float(map_obj['bbox_m'][2])],     # 右上
    [float(map_obj['bbox_m'][1]), float(map_obj['bbox_m'][2])]      # 右下
])

# ==== 可视化/数值对比 ====
plt.figure(figsize=(8,6))
# 2D mask coords散点（蓝色）
plt.scatter(mask_coords[:,1], mask_coords[:,0], marker='.', color='blue', s=10, label='2D Map mask_coords')
# 2D bounding box（红框）
plt.plot(np.append(bbox2d[:,1], bbox2d[0,1]), np.append(bbox2d[:,0], bbox2d[0,0]), 'r-', lw=2, label="2D Map BBox")

# 3D label底面四点（绿色实线）
plt.scatter(corners3d[:,0], corners3d[:,1], c='green', marker='o', s=60, label='3D Label Bottom Corners (raw)')
plt.plot(np.append(corners3d[:,0], corners3d[0,0]), np.append(corners3d[:,1], corners3d[0,1]), 'g--', label='3DGS Bottom Poly (raw)')

# 3D flip后的底面（紫色虚线）
plt.scatter(corners3d_flip[:,0], corners3d_flip[:,1], c='purple', marker='*', s=80, label='3D Label Bottom Corners (flip)')
plt.plot(np.append(corners3d_flip[:,0], corners3d_flip[0,0]), np.append(corners3d_flip[:,1], corners3d_flip[0,1]), 'm:', lw=2, label='3DGS Bottom Poly (flip)')

plt.xlabel('x (meter)')
plt.ylabel('y (meter)')
plt.title(f'Instance_id={instance_id}: 3DGS label.json vs 2D Map.json\n(含左右+上下flip对比)')
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()

# ==== 数值分析 ====
print(f'3D label底面xy范围 (原始): x [{corners3d[:,0].min():.2f}, {corners3d[:,0].max():.2f}], '
      f'y [{corners3d[:,1].min():.2f}, {corners3d[:,1].max():.2f}]')
print(f'3D label底面xy范围 (flip):   x [{corners3d_flip[:,0].min():.2f}, {corners3d_flip[:,0].max():.2f}], '
      f'y [{corners3d_flip[:,1].min():.2f}, {corners3d_flip[:,1].max():.2f}]')
print(f'2D Map mask_coords_m:       x [{mask_coords[:,1].min():.2f}, {mask_coords[:,1].max():.2f}], '
      f'y [{mask_coords[:,0].min():.2f}, {mask_coords[:,0].max():.2f}]')