#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
双手机支架几何模型（纯标准库，无第三方依赖）

坐标约定
    x : 从妈妈指向 B 手机（B 在 +x 方向）
    y : 妈妈面向 B 时的左侧（横向错位方向，+y 为左）
    z : 桌面上方
    原点 O : B 手机背板下边缘的桌面投影点

单位: mm / 度（输出 JSON 时角度转为度）
"""

from __future__ import annotations

import json
import math
from dataclasses import dataclass, asdict, field
from pathlib import Path

# ----------------------------------------------------------------------------
# 1. 基准参数（可调）
# ----------------------------------------------------------------------------

W = 72.0          # 手机宽 (mm)
H = 160.0         # 手机高 (mm)
T = 8.3           # 手机厚 (mm)
CAM_PROUD = 4.0   # 后摄模组凸出背板的厚度 (mm)
CAM_FROM_TOP = 25.0  # 后摄光心距手机上边缘 (mm) -> 距下边缘 lc = H - 25
CAM_LATERAL = 0.0    # 后摄光心相对手机中线的横向偏移 (mm)，0 = 简化模型

THETA_B = 60.0    # B 背板与桌面的夹角 (deg)
H_B0 = 12.0       # B 下边缘（背板基准）离桌高度 = 托口唇高 (mm)

D = 300.0         # B/A 两个背板下边缘在 x 方向的水平距离 (mm)
E = 140.0         # A 相对 B 的横向错位（y 方向，+ 表示 A 偏向妈妈左侧）(mm)
H_A0 = 12.0       # A 下边缘离桌高度 (mm)

# 后摄主摄：26mm 等效，4:3 传感器
HFOV_4_3 = 67.0   # 4:3 水平视场 (deg)
VFOV_4_3 = 53.0   # 4:3 垂直视场 (deg)
# 16:9 视频裁切后的垂直视场，由同一个 4:3 传感器高度决定
VFOV_16_9 = math.degrees(2 * math.atan(math.tan(math.radians(HFOV_4_3) / 2) * 9 / 16))

# 妈妈的眼睛位置（用于校核观看角）
EYE = (-700.0, 200.0, 370.0)

# 设计目标
TARGET_FILL = (0.55, 0.75)   # B 屏幕高度占画面高度的比例
MIN_MARGIN = 0.08            # 画面四边最小余量（占整幅比例）


# ----------------------------------------------------------------------------
# 2. 小向量库
# ----------------------------------------------------------------------------

def vadd(a, b): return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
def vsub(a, b): return (a[0] - b[0], a[1] - b[1], a[2] - b[2])
def vmul(a, s): return (a[0] * s, a[1] * s, a[2] * s)
def vdot(a, b): return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
def vlen(a): return math.sqrt(vdot(a, a))
def vnorm(a):
    n = vlen(a)
    return (a[0] / n, a[1] / n, a[2] / n)
def vcross(a, b):
    return (a[1] * b[2] - a[2] * b[1],
            a[2] * b[0] - a[0] * b[2],
            a[0] * b[1] - a[1] * b[0])
def clamp(x, lo, hi): return max(lo, min(hi, x))
def deg(x): return math.degrees(x)


# ----------------------------------------------------------------------------
# 3. 位姿与几何
# ----------------------------------------------------------------------------

@dataclass
class PhonePose:
    """一台手机的位姿。背板下边缘中点为 origin（其上再叠加离桌高度）。"""
    origin: tuple
    theta: float          # 背板与桌面夹角 (rad)
    psi: float            # 绕 z 轴偏航 (rad)，0 = 手机朝向 -x（面向妈妈）
    d: tuple = field(init=False)   # 沿手机上边缘方向（从下到上）
    n: tuple = field(init=False)   # 屏幕外法线
    c: tuple = field(init=False)   # 后摄朝向（= -n）
    w: tuple = field(init=False)   # 手机宽度方向

    def __post_init__(self):
        ct, st = math.cos(self.theta), math.sin(self.theta)
        cp, sp = math.cos(self.psi), math.sin(self.psi)
        self.d = (ct * cp, ct * sp, st)
        self.n = (-st * cp, -st * sp, ct)
        self.c = (-self.n[0], -self.n[1], -self.n[2])
        self.w = (-sp, cp, 0.0)


def phi(theta_deg: float, psi_deg: float = 0.0) -> PhonePose:
    return PhonePose((0.0, 0.0, 0.0), math.radians(theta_deg), math.radians(psi_deg))


def b_pose() -> PhonePose:
    p = phi(THETA_B, 0.0)
    p.origin = (0.0, 0.0, H_B0)
    return p


def b_center() -> tuple:
    p = b_pose()
    # 屏幕中心：背板下边缘 + H/2 * d + T * n
    return vadd(vadd(p.origin, vmul(p.d, H / 2)), vmul(p.n, T))


def a_base() -> tuple:
    return (-D, E, H_A0)


def a_camera(pose: PhonePose) -> tuple:
    lc = H - CAM_FROM_TOP
    p = vadd(vadd(pose.origin, vmul(pose.d, lc)), vmul(pose.c, T / 2 + CAM_PROUD))
    p = vadd(p, vmul(pose.w, CAM_LATERAL))
    return p


def solve_a(target=None, theta0_deg=80.0, psi0_deg=-20.0, iters=100):
    """求 A 的倾角/偏航，使后摄光轴穿过 target（默认 B 屏幕中心）。"""
    if target is None:
        target = b_center()
    base = a_base()
    theta, psi = math.radians(theta0_deg), math.radians(psi0_deg)
    for _ in range(iters):
        p = phi(deg(theta), deg(psi))
        p.origin = base
        cam = a_camera(p)
        u = vnorm(vsub(target, cam))
        theta_new = math.pi / 2 - math.asin(clamp(-u[2], -1, 1))
        psi_new = math.atan2(u[1], u[0])
        if abs(theta_new - theta) < 1e-12 and abs(psi_new - psi) < 1e-12:
            theta, psi = theta_new, psi_new
            break
        theta, psi = theta_new, psi_new
    p = phi(deg(theta), deg(psi))
    p.origin = base
    return p


def aim_ray(pose: PhonePose):
    cam = a_camera(pose)
    u = vnorm(vsub(b_center(), cam))
    return cam, u


# ----------------------------------------------------------------------------
# 4. 投影与取景校核
# ----------------------------------------------------------------------------

def b_corners():
    p = b_pose()
    c = b_center()
    out = []
    for sv in (-1, 1):          # 下/上
        for su in (-1, 1):      # 左/右（y 方向）
            out.append(vadd(vadd(c, vmul(p.d, sv * H / 2)), vmul(p.w, su * W / 2)))
    return out  # [bl, br, tl, tr]


def project(cam, u, up_hint, hfov_deg, vfov_deg, safety=1.0, points=None):
    """把世界点投到归一化像平面：u,v ∈ [-1,1]，中心为 0。"""
    if points is None:
        points = b_corners()
    r = vnorm(vcross(u, up_hint))
    up = vcross(r, u)
    th = math.tan(math.radians(hfov_deg) * safety / 2)
    tv = math.tan(math.radians(vfov_deg) * safety / 2)
    out = []
    for q in points:
        v = vsub(q, cam)
        z = vdot(v, u)
        if z <= 1e-9:
            raise ValueError("point behind camera")
        out.append((vdot(v, r) / z / th, vdot(v, up) / z / tv, z))
    return out, r, up


def frame_metrics(pose, hfov_deg, vfov_deg, safety=1.0):
    cam, u = aim_ray(pose)
    pts, r, up = project(cam, u, pose.d, hfov_deg, vfov_deg, safety)
    us = [p[0] for p in pts]
    vs = [p[1] for p in pts]
    bb = (min(us), max(us), min(vs), max(vs))
    # 投影边长（用于梯形畸变）
    def seg(a, b): return math.hypot(a[0] - b[0], a[1] - b[1])
    top = seg(pts[2], pts[3])
    bottom = seg(pts[0], pts[1])
    left = seg(pts[0], pts[2])
    right = seg(pts[1], pts[3])
    return {
        "corners_uv": [(round(a, 4), round(b, 4), round(c, 2)) for a, b, c in pts],
        "bbox": tuple(round(x, 4) for x in bb),
        "fill_w": round((bb[1] - bb[0]) / 2, 4),
        "fill_h": round((bb[3] - bb[2]) / 2, 4),
        "margin_left": round((1 + bb[0]) / 2, 4),
        "margin_right": round((1 - bb[1]) / 2, 4),
        "margin_bottom": round((1 + bb[2]) / 2, 4),
        "margin_top": round((1 - bb[3]) / 2, 4),
        "keystone_top_over_bottom": round(top / bottom, 4),
        "keystone_left_over_right": round(left / right, 4),
    }


def off_axis_angle(view_dir, normal):
    return deg(math.acos(clamp(vdot(vnorm(view_dir), vnorm(normal)), -1, 1)))


def screen_view(center, normal, eye):
    v = vsub(eye, center)
    e = deg(math.atan2(v[2], math.hypot(v[0], v[1])))
    az = deg(math.atan2(v[1], v[0]))
    return {
        "off_normal": round(off_axis_angle(v, normal), 2),
        "elevation": round(e, 2),
        "azimuth": round(az, 2),
        "distance": round(vlen(v), 1),
    }


def line_hits_a(target, eye=EYE):
    """视线 E->target 是否被 A 手机本体挡住。返回相交参数与手机局部坐标。"""
    p = solve_a()
    origin = p.origin
    denom = vdot(vsub(target, eye), p.n)
    if abs(denom) < 1e-9:
        return None
    t = vdot(vsub(origin, eye), p.n) / denom
    if not (0 < t < 1):
        return {"occluded": False, "t": round(t, 3)}
    x = vadd(eye, vmul(vsub(target, eye), t))
    rel = vsub(x, origin)
    s = vdot(rel, p.d)
    q = vdot(rel, p.w)
    occluded = (0 <= s <= H) and (-W / 2 <= q <= W / 2)
    return {"occluded": occluded, "t": round(t, 3), "s": round(s, 1),
            "q": round(q, 1), "point": tuple(round(v, 1) for v in x)}


def height_profile(pose):
    p = b_pose()
    top_b = vadd(p.origin, vmul(p.d, H))
    top_a = vadd(pose.origin, vmul(pose.d, H))
    return {
        "B_screen_center_z": round(b_center()[2], 1),
        "B_top_z": round(top_b[2], 1),
        "A_top_z": round(top_a[2], 1),
        "A_camera_z": round(a_camera(pose)[2], 1),
    }


# ----------------------------------------------------------------------------
# 5. 汇总
# ----------------------------------------------------------------------------

def evaluate(D_=D, E_=E, theta_B_deg=THETA_B, H_=H, cam_top=CAM_FROM_TOP,
             hfov=HFOV_4_3, vfov=VFOV_4_3, safety=1.0, eye=None):
    global D, E, THETA_B, H, CAM_FROM_TOP
    old = (D, E, THETA_B, H, CAM_FROM_TOP)
    D, E, THETA_B, H, CAM_FROM_TOP = D_, E_, theta_B_deg, H_, cam_top
    try:
        pose = solve_a()
        cam, u = aim_ray(pose)
        pB = b_center()
        dist = vlen(vsub(pB, cam))
        beta = off_axis_angle(u, vmul(b_pose().n, -1))
        met = frame_metrics(pose, hfov, vfov, safety)
        eye = EYE if eye is None else eye
        a_screen = vadd(vadd(pose.origin, vmul(pose.d, H / 2)), vmul(pose.n, T / 2))
        b_screen = b_center()
        return {
            "D": D, "E": E, "theta_B": theta_B_deg,
            "theta_A": round(deg(pose.theta), 2),
            "psi_A": round(deg(pose.psi), 2),
            "alpha_cam": round(90 - deg(pose.theta), 2),
            "cam": tuple(round(v, 1) for v in cam),
            "dist_cam_screen": round(dist, 1),
            "beta_off_normal": round(beta, 2),
            "frame": met,
            "a_view": screen_view(a_screen, pose.n, eye),
            "b_view": screen_view(b_screen, b_pose().n, eye),
            "occlusion_B_center": line_hits_a(b_screen, eye),
            "occlusion_B_top": line_hits_a(vadd(b_screen, vmul(b_pose().d, H / 2)), eye),
            "heights": height_profile(pose),
            "pB": tuple(round(v, 1) for v in pB),
            "a_screen_center": tuple(round(v, 1) for v in a_screen),
        }
    finally:
        D, E, THETA_B, H, CAM_FROM_TOP = old


def main():
    rec = evaluate()
    # 16:9 视频
    rec169 = evaluate(vfov=VFOV_16_9)
    rec169s = evaluate(vfov=VFOV_16_9, safety=0.92)
    rec43 = evaluate(vfov=VFOV_4_3)

    out = {
        "constants": {
            "W": W, "H": H, "T": T, "CAM_PROUD": CAM_PROUD,
            "CAM_FROM_TOP": CAM_FROM_TOP, "CAM_LATERAL": CAM_LATERAL,
            "THETA_B": THETA_B, "H_B0": H_B0, "D": D, "E": E, "H_A0": H_A0,
            "HFOV_4_3": HFOV_4_3, "VFOV_4_3": VFOV_4_3, "VFOV_16_9": round(VFOV_16_9, 2),
            "EYE": EYE,
        },
        "recommended": {
            "4:3": rec,
            "16:9": rec169,
            "16:9_safety0.92": rec169s,
        },
        "recommended_43": rec43,
    }

    # 扫描表
    sweep_D = []
    for d_ in (240, 260, 280, 300, 320, 340, 360):
        r = evaluate(D_=d_, vfov=VFOV_16_9, safety=0.92)
        sweep_D.append({
            "D": d_, "theta_A": r["theta_A"], "psi_A": r["psi_A"],
            "cam_z": r["heights"]["A_camera_z"], "dist": r["dist_cam_screen"],
            "beta": r["beta_off_normal"],
            "fill_h": r["frame"]["fill_h"],
            "margin_bottom": r["frame"]["margin_bottom"],
            "margin_top": r["frame"]["margin_top"],
            "keystone": r["frame"]["keystone_top_over_bottom"],
            "a_view": r["a_view"]["off_normal"],
        })
    sweep_E = []
    for e_ in (0, 60, 100, 140, 180):
        eye = (-700.0, e_ + 60.0, 370.0)
        r = evaluate(E_=e_, vfov=VFOV_16_9, safety=0.92, eye=eye)
        sweep_E.append({
            "E": e_, "theta_A": r["theta_A"], "psi_A": r["psi_A"],
            "beta": r["beta_off_normal"],
            "fill_h": r["frame"]["fill_h"],
            "keystone": r["frame"]["keystone_top_over_bottom"],
            "a_view": r["a_view"]["off_normal"],
            "occluded": r["occlusion_B_center"]["occluded"],
        })
    # 手机尺寸敏感性
    sweep_H = []
    for h_ in (147.6, 152.8, 160.0, 165.0):
        r = evaluate(H_=h_, vfov=VFOV_16_9, safety=0.92)
        sweep_H.append({
            "H": h_, "theta_A": r["theta_A"], "cam_z": r["heights"]["A_camera_z"],
            "fill_h": r["frame"]["fill_h"], "dist": r["dist_cam_screen"],
        })
    out["sweep_D"] = sweep_D
    out["sweep_E"] = sweep_E
    out["sweep_H"] = sweep_H

    path = Path(__file__).with_name("derived.json")
    path.write_text(json.dumps(out, ensure_ascii=False, indent=2), encoding="utf-8")

    # 控制台摘要
    r = rec169
    print("== 推荐配置（16:9 视频）==")
    print(f"theta_B = {r['theta_B']}°, theta_A = {r['theta_A']}°, psi_A = {r['psi_A']}°")
    print(f"camera = {r['cam']} mm, dist = {r['dist_cam_screen']} mm, beta = {r['beta_off_normal']}°")
    f = r["frame"]
    print(f"fill_h = {f['fill_h']*100:.1f}%  margins L/R/T/B = "
          f"{f['margin_left']*100:.1f}/{f['margin_right']*100:.1f}/{f['margin_top']*100:.1f}/{f['margin_bottom']*100:.1f}%")
    print(f"keystone top/bottom = {f['keystone_top_over_bottom']}")
    print(f"A screen view off-normal = {r['a_view']['off_normal']}°, "
          f"B screen view off-normal = {r['b_view']['off_normal']}°")
    print(f"heights = {r['heights']}")
    print(f"occlusion B center = {r['occlusion_B_center']}")
    print(f"4:3 fill_h = {rec43['frame']['fill_h']*100:.1f}%, "
          f"16:9 w/ 0.92 crop fill_h = {rec169s['frame']['fill_h']*100:.1f}%, "
          f"bottom margin = {rec169s['frame']['margin_bottom']*100:.1f}%")
    print("\n== D 扫描 (16:9, 0.92 crop) ==")
    print("D    theta_A  psi_A   cam_z  dist   beta  fill_h  mBot  mTop  keystone")
    for s in sweep_D:
        print(f"{s['D']:4.0f} {s['theta_A']:7.1f} {s['psi_A']:7.1f} {s['cam_z']:6.1f} "
              f"{s['dist']:6.1f} {s['beta']:6.1f} {s['fill_h']*100:6.1f}% "
              f"{s['margin_bottom']*100:5.1f}% {s['margin_top']*100:5.1f}% {s['keystone']:.3f}")
    print("\n== E 扫描 ==")
    print("E    theta_A  psi_A   beta  fill_h  keystone  a_view  occl")
    for s in sweep_E:
        print(f"{s['E']:4.0f} {s['theta_A']:7.1f} {s['psi_A']:7.1f} {s['beta']:6.1f} "
              f"{s['fill_h']*100:6.1f}% {s['keystone']:.3f} {s['a_view']:6.1f}° {s['occluded']}")
    print("\n== H 扫描 ==")
    for s in sweep_H:
        print(f"H={s['H']:6.1f} theta_A={s['theta_A']:5.1f} cam_z={s['cam_z']:6.1f} "
              f"fill={s['fill_h']*100:5.1f}% dist={s['dist']:6.1f}")
    print(f"\nJSON -> {path}")


if __name__ == "__main__":
    main()
