import json
import numpy as np
import matplotlib.pyplot as plt
from PIL import Image

scale = 0.05
occ_img = Image.open('test/839920/occupancy.png')
occupancy = np.array(occ_img)
h, w = occupancy.shape
with open('test/839920/occupancy.json') as f:
    meta = json.load(f)
min_x = meta['min'][0]
min_y = meta['min'][1]
with open('2D_Semantic_Map_839920_Physic.json') as f:
    data = json.load(f)
img = np.zeros((h, w), dtype=int)
def world2pix(x, y):
    px = int(round((float(x) - min_x) / scale))
    py = int(round((float(y) - min_y) / scale))
    return py, px
for inst in data:
    cid = inst['category_id']
    for y, x in inst['mask_coords_m']:
        py, px = world2pix(x, y)
        # 注意判定在画布边界内
        if 0 <= py < h and 0 <= px < w:
            img[py, px] = cid
plt.figure(figsize=(12,12))
img_extent = [min_x, min_x + w * scale, min_y, min_y + h * scale]
plt.imshow(img, cmap='tab20', extent=img_extent, origin='lower')
# bbox直接用world坐标
for inst in data:
    bbox = inst['bbox_m']
    x0, y0, x1, y1 = map(float, bbox)
    plt.gca().add_patch(
        plt.Rectangle((x0, y0), x1 - x0, y1 - y0,
                      edgecolor='red', fill=False, linewidth=1)
    )
plt.title('2D Semantic Map (Restored from Physical JSON)')
plt.xlabel('X (meters)')
plt.ylabel('Y (meters)')
plt.savefig('2D_Semantic_Map_839920_Physic_w_Occupancy.png', bbox_inches='tight', dpi=300)
plt.show()