"""Render static design examples from one original TRAIN image/cloud.

This is a proposal illustration generator, not an annotation service. The
demonstration polygon, geometric face and ruler are not human-confirmed labels.
Only docs/assets/pallet_pose_annotation_design is written.
"""
from __future__ import annotations

import argparse
import hashlib
import json
from pathlib import Path
import sys

import cv2
import matplotlib
matplotlib.use('Agg')
import matplotlib.pyplot as plt
from matplotlib import font_manager
from matplotlib.lines import Line2D
from matplotlib.patches import Polygon, Rectangle, FancyBboxPatch
import numpy as np
from PIL import Image

PROJECT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(PROJECT))
from pallet.pose_model import fit_front_plane

SAMPLE = '09fc85ced43d7c6e5500596f'
OUT = PROJECT/'docs/assets/pallet_pose_annotation_design'
BG = '#101827'
PANEL = '#172338'
TEXT = '#ebf1fa'
MUTED = '#a5b7cd'
GRID = '#354861'
FACE = '#ffb454'
ROI = '#52bde8'
RULER = '#64f3bf'
GHOST = '#ee83dd'
W_RANGE = (-650., 650.)
D_RANGE = (-160., 340.)


def sha(path):
    return hashlib.sha256(Path(path).read_bytes()).hexdigest()


def load_example():
    root = PROJECT/'outputs/pallet_pose_dataset_v2/dataset'
    metadata_path = root/f'overlap/train/{SAMPLE}.json'
    row = json.loads(metadata_path.read_text())
    assert row['split'] == 'train' and row['sensor_policy']['depth_axis'] == 0
    scale = row['pointcloud_scale_to_m']
    assert scale is not None and scale > 0
    image_path, points_path = root/row['image'], root/row['points']
    hashes = {str(p.relative_to(PROJECT)): sha(p)
              for p in (metadata_path, image_path, points_path)}
    with np.load(points_path, allow_pickle=False) as cloud:
        all_points = cloud['points_xyzi'][:, :3].astype(np.float64)*scale
        mapping = cloud['pixel_to_source_index'].copy()
    image = np.array(Image.open(image_path).convert('L'))
    assert image.shape == mapping.shape
    # Illustrative coarse outline only, intentionally includes the visible side.
    polygon = np.array([[236, 251], [469, 245], [499, 251],
                        [497, 299], [448, 310], [237, 309]], dtype=np.int32)
    mask = np.zeros(image.shape, np.uint8)
    cv2.fillPoly(mask, [polygon], 1)
    good = np.isfinite(all_points).all(axis=1) & (np.linalg.norm(all_points, axis=1) > 1e-8)
    roi_ids = mapping[mask > 0]
    roi_ids = roi_ids[good[roi_ids]]
    roi = all_points[roi_ids]
    anchor, normal, _, ok = fit_front_plane(roi,
        {'geometry_points': 8192, 'ransac_iterations': 512, 'plane_threshold': .012}, seed=42)
    assert ok
    face_keep = np.abs((roi-anchor)@normal) < .012
    face = roi[face_keep]
    # Width is the horizontal direction of the selected fork face, not sensor Y.
    depth_axis = np.r_[normal[:2], 0.]
    depth_axis /= np.linalg.norm(depth_axis)
    width_axis = np.cross(depth_axis, [0., 0., 1.])  # image-left to image-right
    height_axis = np.array([0., 0., 1.])
    basis = np.stack((width_axis, depth_axis, height_axis), axis=1)
    assert np.allclose(basis.T@basis, np.eye(3), atol=1e-12)
    width_bounds = np.quantile(face@width_axis, [.01, .99])
    origin = (width_axis*width_bounds.mean() + depth_axis*np.median(face@depth_axis)
              + height_axis*np.median(face@height_axis))
    local = (all_points[good]-origin)@basis*1000
    context = local[(np.abs(local[:, 0]) <= 700) & (np.abs(local[:, 2]) <= 260)
                    & (local[:, 1] >= -200) & (local[:, 1] <= 550)]
    if len(context) > 16000:
        context = context[np.random.default_rng(42).choice(len(context), 16000, replace=False)]
    return dict(row=row, image=image, polygon=polygon, roi=roi, face=face,
        context=context, roi_local=(roi-origin)@basis*1000,
        face_local=(face-origin)@basis*1000, origin=origin, basis=basis,
        width_bounds=width_bounds, half_width=float(np.diff(width_bounds)[0]*500),
        theta=float(np.degrees(np.arctan2(depth_axis[1], depth_axis[0]))),
        source_hashes=hashes, face_point_count=int(face_keep.sum()), roi_ids=roi_ids)


def text(fig, x, y, value, size=13, color=TEXT, **kwargs):
    return fig.text(x, y, value, fontsize=size, color=color, **kwargs)


def style(ax, ylim, ylabel):
    ax.set_facecolor(PANEL)
    ax.set_xlim(*W_RANGE)
    ax.set_ylim(*ylim)
    ax.set_xticks(np.arange(-600, 601, 200))
    ax.tick_params(colors=MUTED, labelsize=11)
    for spine in ax.spines.values():
        spine.set_color(GRID)
    ax.grid(color=GRID, linewidth=.7, alpha=.6)
    ax.set_axisbelow(True)
    ax.set_xlabel('宽度 W（mm） · 沿叉取面长边', color=TEXT, fontsize=12, labelpad=6)
    ax.set_ylabel(ylabel, color=TEXT, fontsize=12, labelpad=8)
    ax.set_aspect('equal', adjustable='box')


def cloud(ax, example, vertical):
    for points, color, size, alpha in (
        (example['context'], '#9ca9bb', 1.3, .15),
        (example['roi_local'], ROI, 2.0, .23),
        (example['face_local'], FACE, 3.5, .90),
    ):
        ax.scatter(points[:, 0], points[:, vertical], s=size, c=color,
                   alpha=alpha, edgecolors='none', rasterized=True)


def ruler(ax, center=(0., 0.), angle=0., half=480., color=RULER,
          dashed=False, ticks=True, handles=True):
    phi = np.deg2rad(angle)
    along = np.array([np.cos(phi), np.sin(phi)])
    perpendicular = np.array([-np.sin(phi), np.cos(phi)])
    center = np.array(center)
    ends = np.array([center-half*along, center+half*along])
    ax.plot(ends[:, 0], ends[:, 1], color=color, lw=2.2,
            linestyle='--' if dashed else '-', zorder=8)
    if ticks:
        for distance in np.arange(-int(half//20)*20, int(half//20)*20+1, 20):
            start = center+distance*along
            length = 24 if distance % 100 == 0 else 11
            end = start+length*perpendicular
            ax.plot([start[0], end[0]], [start[1], end[1]], color=color, lw=.8, zorder=8)
            if distance % 200 == 0:
                label = start+45*perpendicular
                ax.text(*label, str(int(distance)), color=color, ha='center',
                        fontsize=9, rotation=angle, zorder=9)
    if handles:
        ax.scatter(*center, s=80, c=BG, edgecolors=color, linewidths=2, zorder=10)
        tip = center+(half+60)*along
        ax.plot([ends[1, 0], tip[0]], [ends[1, 1], tip[1]], color=color, lw=1, zorder=8)
        ax.scatter(*tip, s=65, c=color, marker='D', zorder=10)
        arrow = center+120*perpendicular
        ax.annotate('', xy=arrow, xytext=center+40*perpendicular,
                    arrowprops=dict(arrowstyle='->', color=color, lw=1.6), zorder=10)
    return center, along


def image_overlay(ax, example, zoom=False):
    ax.imshow(example['image'], cmap='gray', vmin=0, vmax=255)
    box = example['row']['box_xywh']
    ax.add_patch(Rectangle((box['x'], box['y']), box['width'], box['height'],
        fill=False, edgecolor='white', linestyle='--', linewidth=1.2))
    ax.add_patch(Polygon(example['polygon'], facecolor=ROI, alpha=.18))
    closed = np.vstack((example['polygon'], example['polygon'][0]))
    ax.plot(closed[:, 0], closed[:, 1], c=ROI, lw=1.8)
    ax.scatter(example['polygon'][:, 0], example['polygon'][:, 1],
               s=38 if zoom else 20, c=BG, edgecolors=ROI, linewidths=1.5, zorder=4)
    if zoom:
        ax.set_xlim(211, 522)
        ax.set_ylim(326, 227)
    ax.set_axis_off()


def workstation(example):
    fig = plt.figure(figsize=(18, 11.2), dpi=130, facecolor=BG)
    text(fig, .025, .957, '托盘位姿标注工作台 · 宽度轴对齐版', 23, weight='bold')
    text(fig, .975, .963, '静态需求示意 / 尚未上线', 12, MUTED, ha='right')
    text(fig, .025, .918, '真实 train 样本：2024-10-30 10:03:05', 12, MUTED)
    for x, label, width in ((.60, '上一条', .065), (.675, '撤销', .055),
                            (.74, '按标尺重对齐', .105), (.86, '保存并下一条', .115)):
        fig.add_artist(FancyBboxPatch((x, .904), width, .035,
            boxstyle='round,pad=0.003,rounding_size=0.004', transform=fig.transFigure,
            facecolor='#233d4d' if x == .86 else PANEL, edgecolor=GRID))
        text(fig, x+width/2, .921, label, 11, ha='center', va='center')

    text(fig, .025, .865, '① 图像区域：四角起步，可增点、拖点', 15, weight='bold')
    image_overlay(fig.add_axes([.025, .505, .34, .345]), example)
    text(fig, .025, .470, '② 区域放大：蓝色顶点与粗掩膜', 14, weight='bold')
    image_overlay(fig.add_axes([.025, .280, .34, .175]), example, zoom=True)
    text(fig, .025, .255, '白虚线：原始框   蓝多边形：编辑示例', 11, MUTED)
    for y, message, color in (
        (.209, '● 灰：原始点云上下文（局部视窗）', MUTED),
        (.179, '● 蓝：多边形区域点云', ROI),
        (.149, '● 橙：自动拟合的叉取面候选', FACE),
        (.119, '● 绿：可拖动、旋转的刻度标尺', RULER),
        (.072, '掩膜 / 中心 / 位姿均为演示，待人工确认', MUTED),
        (.043, f'示例法向角 θ ≈ {example["theta"]:.1f}°（原坐标系）', TEXT),
    ):
        text(fig, .025, y, message, 12, color)

    # The axes have exactly the same left edge, width and W range; each has
    # equal mm-per-pixel in its two directions, so ruler angles are undistorted.
    left, width = .435, .535
    front_h = width*18/11.2*440/1300
    top_h = width*18/11.2*500/1300
    front = fig.add_axes([left, .555, width, front_h])
    top = fig.add_axes([left, .090, width, top_h])
    style(front, (-220, 220), '高度 H（mm）')
    style(top, D_RANGE, '深度 D（mm）')
    cloud(front, example, 2)
    cloud(top, example, 1)
    text(fig, left, .875, '③ 正视图：W—H  |  用腿、叉孔和左右轮廓判断中心', 15, weight='bold')
    text(fig, left, .465, '④ 俯视图：W—D  |  标尺与叉取面平行，中心对准托盘', 15, weight='bold')
    for coordinate in (-example['half_width'], 0., example['half_width']):
        for ax in (front, top):
            ax.axvline(coordinate, color=RULER, alpha=.65, linestyle='--', linewidth=1)
    front.plot([0, 0], [-25, 25], c=RULER, lw=2)
    front.plot([-25, 25], [0, 0], c=RULER, lw=2)
    ruler(top, half=example['half_width'])
    top.text(80, 135, '叉入方向 / +D', color=RULER, fontsize=11)
    front.text(0, 175, '同一宽度中心', color=RULER, fontsize=11, ha='center')
    fig.canvas.draw()
    xf = front.transData.transform([[0, 0], [-400, 0], [400, 0]])[:, 0]
    xt = top.transData.transform([[0, 0], [-400, 0], [400, 0]])[:, 0]
    assert np.allclose(xf, xt, atol=1e-9)
    for coordinate in (-example['half_width'], 0., example['half_width']):
        pixel_x = front.transData.transform((coordinate, 0))[0]
        figure_x = fig.transFigure.inverted().transform((pixel_x, 0))[0]
        fig.add_artist(Line2D([figure_x, figure_x], [top.get_position().y1, front.get_position().y0],
            transform=fig.transFigure, color=RULER, alpha=.25, linestyle='--', lw=.8))
    fig.savefig(OUT/'workstation_real_sample.png', facecolor=BG)
    checks = {'shared_width_axis_max_pixel_error': float(np.max(np.abs(xf-xt))),
              'front_axis_box': list(front.get_position().bounds),
              'top_axis_box': list(top.get_position().bounds)}
    plt.close(fig)
    return checks


def coordinates_and_ruler(example):
    fig = plt.figure(figsize=(18, 7.4), dpi=130, facecolor=BG)
    text(fig, .025, .938, '同一倾斜托盘：宽度坐标怎么建立，标尺怎么操作', 22, weight='bold')
    text(fig, .025, .887, '三幅均使用同一份真实点云；右图粉色虚线是人为构造的操作示例。', 13, MUTED)
    centers = example['origin']
    axes = [fig.add_axes([x, .265, .285, .48]) for x in (.045, .365, .685)]
    for ax in axes:
        style(ax, (-250, 550), '深度（mm）')
        ax.tick_params(labelsize=10)
    raw = axes[0]
    for points, color, alpha in ((example['roi'], ROI, .25), (example['face'], FACE, .9)):
        delta = (points-centers)*1000
        raw.scatter(-delta[:, 1], delta[:, 0], s=3, c=color, alpha=alpha, edgecolors='none')
    raw.set_xlabel('传感器横向 −Y（mm）', color=TEXT, fontsize=12)
    raw.set_ylabel('传感器前向 X（mm）', color=TEXT, fontsize=12)
    raw.text(0, 440, '叉取面在此坐标中是倾斜的', color=FACE, fontsize=11, ha='center')
    raw_w = np.array([-example['basis'][1, 0], example['basis'][0, 0]])
    ruler(raw, angle=np.degrees(np.arctan2(raw_w[1], raw_w[0])),
          half=example['half_width'], ticks=False, handles=False)
    for ax in axes[1:]:
        cloud(ax, example, 1)
        ax.set_ylabel('深度 D（mm）', color=TEXT, fontsize=12)
    ruler(axes[1], half=example['half_width'])
    axes[1].text(0, 440, '同一面沿 W 展开，标尺线水平', color=RULER, fontsize=11, ha='center')
    ruler(axes[2], half=example['half_width'], handles=False)
    ruler(axes[2], center=(80, 100), angle=12, half=example['half_width'],
          color=GHOST, dashed=True, ticks=False)
    axes[2].annotate('', xy=(0, 0), xytext=(80, 100),
        arrowprops=dict(arrowstyle='->', color='white', lw=2), zorder=12)
    axes[2].annotate('拖中心', xy=(80, 100), xytext=(-440, 290), color=GHOST, fontsize=11,
        arrowprops=dict(arrowstyle='->', color=GHOST))
    axes[2].annotate('拖旋转手柄', xy=(580, 206), xytext=(80, 425), color=GHOST, fontsize=11,
        arrowprops=dict(arrowstyle='->', color=GHOST))
    for x, title in ((.045, 'A · 原始传感器坐标'), (.365, 'B · 叉取面宽度坐标'), (.685, 'C · 同一 W—D 视图中配准')):
        text(fig, x, .790, title, 15, weight='bold')
    for x, lines in (
        (.045, ('先由选定叉取面确定长边方向 W。', 'W 不是固定的传感器 Y。')),
        (.365, ('正视、俯视共用此 W 轴。', f'视图内水平，原坐标 θ 仍约 {example["theta"]:.1f}°。')),
        (.685, ('粉虚线 → 绿实线：平移并旋转标尺。', '操作期间锁定视图，结束后可重新对齐。')),
    ):
        for y, line in zip((.165, .112), lines):
            text(fig, x, y, line, 12, MUTED)
    text(fig, .025, .035, '方案示意：橙色为几何面候选；中心与位姿不是已确认真值。', 11, MUTED)
    fig.savefig(OUT/'width_coordinates_and_ruler.png', facecolor=BG)
    plt.close(fig)


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument('--font', type=Path,
        default=Path('/tmp/opencode/pallet_pose_design/NotoSansCJKsc-Regular.otf'))
    args = parser.parse_args()
    font_manager.fontManager.addfont(args.font)
    family = font_manager.FontProperties(fname=args.font).get_name()
    plt.rcParams.update({'font.family': family, 'axes.unicode_minus': False})
    OUT.mkdir(parents=True, exist_ok=True)
    example = load_example()
    layout = workstation(example)
    coordinates_and_ruler(example)
    for path, expected in example['source_hashes'].items():
        assert sha(PROJECT/path) == expected
    record = {'kind': 'static_design_examples_not_human_annotations',
        'sample_id': SAMPLE, 'frame_id': example['row']['frame_id'], 'source_split': 'train',
        'source_files_sha256_unchanged': example['source_hashes'],
        'illustrative_polygon_native_pixels': example['polygon'].tolist(),
        'geometric_candidate_points': example['face_point_count'],
        'roi_valid_points': len(example['roi']), 'source_scale_to_m': example['row']['pointcloud_scale_to_m'],
        'display_basis_columns_W_D_H_in_source_coordinates': example['basis'].tolist(),
        'display_origin_source_m': example['origin'].tolist(),
        'geometric_candidate_normal_theta_degrees': example['theta'],
        'layout_verification': layout,
        'human_annotation_written': False, 'ground_truth_pose_used_for_geometry': False,
        'service_started': False,
        'notes': ['Polygon and ruler are illustrative, not human-confirmed annotations.',
                  'Width is the horizontal long direction of the selected fork face.',
                  'H follows source +Z; this example does not establish gravity or robot calibration.',
                  'Context points are displayed in a local viewport; raw point coordinates are unchanged.'],
        'figures': {p.name: {'sha256': sha(p), 'size_pixels': list(Image.open(p).size)}
                    for p in sorted(OUT.glob('*.png'))}}
    (OUT/'example_provenance.json').write_text(json.dumps(record, ensure_ascii=False, indent=2)+'\n')
    print(json.dumps(record, ensure_ascii=False, indent=2))


if __name__ == '__main__':
    main()
