#!/usr/bin/env python3
from html import escape


W = 4800
H = 7200
FONT = "Noto Sans CJK SC, DejaVu Sans, sans-serif"


def text_width_units(s):
    units = 0
    for ch in s:
        units += 2 if ord(ch) > 127 else 1
    return units


def wrap_text(text, max_units):
    words = []
    for part in text.split(" "):
        if text_width_units(part) <= max_units:
            words.append(part)
            continue
        current = ""
        for ch in part:
            if text_width_units(current + ch) > max_units and current:
                words.append(current)
                current = ch
            else:
                current += ch
        if current:
            words.append(current)

    lines = []
    current = ""
    for word in words:
        candidate = word if not current else current + " " + word
        if text_width_units(candidate) <= max_units:
            current = candidate
        else:
            if current:
                lines.append(current)
            current = word
    if current:
        lines.append(current)
    return lines


class Svg:
    def __init__(self):
        self.items = []

    def add(self, s):
        self.items.append(s)

    def section(self, x, y, w, h, title, color="#eef5ff"):
        self.add(
            f'<rect x="{x}" y="{y}" width="{w}" height="{h}" rx="36" '
            f'fill="{color}" stroke="#b8c7dc" stroke-width="4"/>'
        )
        self.add(
            f'<text x="{x + 34}" y="{y + 70}" class="section-title">{escape(title)}</text>'
        )

    def box(self, node_id, x, y, w, h, title, body=None, fill="#ffffff", stroke="#416788"):
        self.add(
            f'<rect id="{node_id}" x="{x}" y="{y}" width="{w}" height="{h}" rx="18" '
            f'fill="{fill}" stroke="{stroke}" stroke-width="4"/>'
        )
        lines = wrap_text(title, max(10, int(w / 24)))
        ty = y + 44
        for i, line in enumerate(lines):
            klass = "box-title" if i == 0 else "box-text"
            self.add(f'<text x="{x + w / 2}" y="{ty}" class="{klass}" text-anchor="middle">{escape(line)}</text>')
            ty += 42
        if body:
            ty += 8
            for line in wrap_text(body, max(10, int(w / 20))):
                self.add(f'<text x="{x + 28}" y="{ty}" class="box-small">{escape(line)}</text>')
                ty += 34

    def note(self, x, y, w, h, title, lines, fill="#fff7e6"):
        self.add(
            f'<rect x="{x}" y="{y}" width="{w}" height="{h}" rx="18" '
            f'fill="{fill}" stroke="#d49b32" stroke-width="4"/>'
        )
        self.add(f'<text x="{x + 28}" y="{y + 46}" class="note-title">{escape(title)}</text>')
        ty = y + 90
        for line in lines:
            for wrapped in wrap_text(line, max(10, int((w - 56) / 19))):
                self.add(f'<text x="{x + 28}" y="{ty}" class="note-text">{escape(wrapped)}</text>')
                ty += 32

    def arrow(self, x1, y1, x2, y2, label=None, color="#1f3b57", dashed=False):
        dash = ' stroke-dasharray="18 12"' if dashed else ""
        self.add(
            f'<path d="M {x1} {y1} L {x2} {y2}" fill="none" stroke="{color}" '
            f'stroke-width="6" marker-end="url(#arrow)"{dash}/>'
        )
        if label:
            lx = (x1 + x2) / 2
            ly = (y1 + y2) / 2 - 16
            self.add(
                f'<rect x="{lx - 150}" y="{ly - 32}" width="300" height="46" rx="12" '
                f'fill="#ffffff" opacity="0.92"/>'
            )
            self.add(f'<text x="{lx}" y="{ly}" class="arrow-label" text-anchor="middle">{escape(label)}</text>')

    def poly_arrow(self, points, label=None, color="#1f3b57", dashed=False):
        dash = ' stroke-dasharray="18 12"' if dashed else ""
        d = "M " + " L ".join(f"{x} {y}" for x, y in points)
        self.add(
            f'<path d="{d}" fill="none" stroke="{color}" stroke-width="6" '
            f'marker-end="url(#arrow)"{dash}/>'
        )
        if label:
            x, y = points[len(points) // 2]
            self.add(
                f'<rect x="{x - 170}" y="{y - 54}" width="340" height="46" rx="12" '
                f'fill="#ffffff" opacity="0.92"/>'
            )
            self.add(f'<text x="{x}" y="{y - 22}" class="arrow-label" text-anchor="middle">{escape(label)}</text>')

    def render(self):
        defs = f"""
<defs>
  <marker id="arrow" markerWidth="18" markerHeight="18" refX="14" refY="6" orient="auto" markerUnits="strokeWidth">
    <path d="M2,2 L14,6 L2,10 Z" fill="#1f3b57"/>
  </marker>
  <style>
    svg {{ background: #ffffff; }}
    text {{ font-family: {FONT}; fill: #17212b; }}
    .title {{ font-size: 76px; font-weight: 800; }}
    .subtitle {{ font-size: 34px; fill: #52606d; }}
    .section-title {{ font-size: 44px; font-weight: 800; fill: #24415f; }}
    .box-title {{ font-size: 34px; font-weight: 800; }}
    .box-text {{ font-size: 30px; font-weight: 650; }}
    .box-small {{ font-size: 27px; fill: #35495e; }}
    .note-title {{ font-size: 34px; font-weight: 800; fill: #7a4b00; }}
    .note-text {{ font-size: 27px; fill: #5c440b; }}
    .arrow-label {{ font-size: 26px; font-weight: 700; fill: #24415f; }}
    .legend {{ font-size: 28px; fill: #34495e; }}
  </style>
</defs>
"""
        return (
            f'<svg xmlns="http://www.w3.org/2000/svg" width="{W}" height="{H}" viewBox="0 0 {W} {H}">\n'
            + defs
            + "\n".join(self.items)
            + "\n</svg>\n"
        )


svg = Svg()

svg.add(f'<rect x="0" y="0" width="{W}" height="{H}" fill="#ffffff"/>')
svg.add('<text x="2400" y="120" class="title" text-anchor="middle">5axis-xyzbc-trt-sim 执行流程图</text>')
svg.add('<text x="2400" y="178" class="subtitle" text-anchor="middle">LinuxCNC: xyzbc-trt.ini / switchkins / Vismach / PyVCP / M428-M429-M430</text>')

# Section 1: startup
svg.section(120, 260, 4560, 780, "1. 总体启动链路", "#eef6ff")
startup = [
    ("s1", 210, 390, 500, 150, "xyzbc-trt.desktop", "快捷方式入口"),
    ("s2", 820, 390, 520, 150, "rip-environment", "设置 RIP 环境"),
    ("s3", 1450, 390, 560, 150, "scripts/linuxcnc", "读取 xyzbc-trt.ini"),
    ("s4", 2120, 390, 520, 150, "linuxcncsvr", "NML 通道"),
    ("s5", 2750, 390, 560, 150, "realtime / HAL", "加载 RTAPI/HAL"),
    ("s6", 3420, 390, 500, 150, "milltask / halui", "任务与 MDI"),
    ("s7", 4030, 390, 520, 150, "AXIS GUI", "前台显示"),
]
for args in startup:
    svg.box(*args)
for i in range(len(startup) - 1):
    x1 = startup[i][1] + startup[i][3]
    y1 = startup[i][2] + startup[i][4] / 2
    x2 = startup[i + 1][1]
    y2 = startup[i + 1][2] + startup[i + 1][4] / 2
    svg.arrow(x1, y1, x2, y2)

svg.box("ini", 450, 690, 1050, 230, "INI 核心项", "[KINS] xyzbc-trt-kins sparm=identityfirst; [TRAJ] XYZBC; [DISPLAY] axis + PyVCP; [HAL] basic_sim.tcl")
svg.box("halcmd", 1760, 690, 1050, 230, "HAL 加载", "basic_sim.tcl 建立仿真闭环; HALCMD 启动 Vismach 并连接 pins")
svg.box("postgui", 3070, 690, 1050, 230, "GUI 后置 HAL", "AXIS 创建 PyVCP 后执行 switchkins_postgui.hal")
svg.arrow(1500, 805, 1760, 805)
svg.arrow(2810, 805, 3070, 805)

# Section 2: HAL and kins
svg.section(120, 1140, 2200, 1490, "2. basic_sim.tcl / 仿真 HAL", "#f2fbf2")
svg.box("b1", 230, 1280, 520, 150, "basic_sim.tcl", "读取 coordinates/joints/servo period", "#ffffff", "#3f7d4a")
svg.box("b2", 910, 1280, 560, 150, "setup_kins", "loadrt xyzbc-trt-kins sparm=identityfirst", "#ffffff", "#3f7d4a")
svg.box("b3", 1590, 1280, 560, 150, "motmod", "num_joints=5 servo=1ms", "#ffffff", "#3f7d4a")
svg.arrow(750, 1355, 910, 1355)
svg.arrow(1470, 1355, 1590, 1355)

for i, label in enumerate(["pid J0..J4", "mux2 J0..J4", "sim_home_switch", "sim_spindle", "hal_manualtoolchange"]):
    x = 250 + (i % 2) * 870
    y = 1600 + (i // 2) * 230
    svg.box(f"bc{i}", x, y, 720, 150, label, "basic_sim.tcl 创建/连接", "#ffffff", "#3f7d4a")
svg.poly_arrow([(1870, 1430), (1870, 1530), (610, 1530), (610, 1600)])
svg.poly_arrow([(1870, 1430), (1870, 1530), (1480, 1530), (1480, 1600)])

svg.box("loop", 250, 2300, 1720, 190, "理想伺服仿真闭环", "joint.N.motor-pos-cmd -> JN_pid.command -> JN_mux.in1 -> joint.N.motor-pos-fb", "#ffffff", "#3f7d4a")
svg.arrow(1110, 2210, 1110, 2300)

svg.section(2480, 1140, 2200, 1490, "3. switchkins 初始化", "#f9f3ff")
svg.box("k1", 2590, 1280, 620, 150, "xyzbc-trt-kins", "switchkinsSetup()", "#ffffff", "#7246a3")
svg.box("k2", 3350, 1280, 620, 150, "sparm=identityfirst", "改变 type 顺序", "#ffffff", "#7246a3")
svg.arrow(3210, 1355, 3350, 1355)

svg.box("t0", 2630, 1620, 520, 150, "type 0", "identity kinematics", "#ffffff", "#7246a3")
svg.box("t1", 3280, 1620, 520, 150, "type 1", "xyzbc TRT kinematics", "#ffffff", "#7246a3")
svg.box("t2", 3930, 1620, 520, 150, "type 2", "userk kinematics", "#ffffff", "#7246a3")
svg.poly_arrow([(3660, 1430), (3660, 1530), (2890, 1530), (2890, 1620)])
svg.poly_arrow([(3660, 1430), (3660, 1530), (3540, 1530), (3540, 1620)])
svg.poly_arrow([(3660, 1430), (3660, 1530), (4190, 1530), (4190, 1620)])

svg.box("pins", 2700, 1940, 1560, 180, "HAL pins", "kinstype.is-0/1/2; x/y/z-rot-point; x/y/z-offset; tool-offset; conventional-directions", "#ffffff", "#7246a3")
svg.arrow(3540, 1770, 3480, 1940)
svg.box("default", 2920, 2280, 1120, 160, "启动默认状态", "switchkins_type = 0 -> identity", "#ffffff", "#7246a3")
svg.arrow(3480, 2120, 3480, 2280)

# Section 4: switching
svg.section(120, 2730, 4560, 1400, "4. PyVCP / M-code / motion.switchkins-type 切换链路", "#fff8ee")
svg.box("p1", 240, 2880, 680, 160, "PyVCP SWITCHKINS 面板", "IDENTITY / TCP:XYZBC / userk", "#ffffff", "#ba7a20")
svg.box("p2", 1100, 2880, 620, 160, "switchkins_postgui.hal", "按钮接入 halui.mdi-command", "#ffffff", "#ba7a20")
svg.box("p3", 1900, 2880, 580, 160, "halui MDI", "执行 M429 / M428 / M430", "#ffffff", "#ba7a20")
svg.box("p4", 2660, 2880, 640, 160, "remap 子程序", "429/428/430remap.ngc", "#ffffff", "#ba7a20")
svg.box("p5", 3480, 2880, 520, 160, "M68 E3 Qn", "设置 analog out", "#ffffff", "#ba7a20")
svg.box("p6", 4160, 2880, 420, 160, "M66 E0 L0", "同步解释器与 motion", "#ffffff", "#ba7a20")
for x1, x2 in [(920, 1100), (1720, 1900), (2480, 2660), (3300, 3480), (4000, 4160)]:
    svg.arrow(x1, 2960, x2, 2960)

svg.box("m429", 520, 3260, 660, 150, "M429", "type 0: identity", "#ffffff", "#ba7a20")
svg.box("m428", 1480, 3260, 660, 150, "M428", "type 1: xyzbc TRT", "#ffffff", "#ba7a20")
svg.box("m430", 2440, 3260, 660, 150, "M430", "type 2: userk", "#ffffff", "#ba7a20")
svg.box("aout", 3400, 3260, 720, 150, "motion.analog-out-03", "HAL net :kinstype-select", "#ffffff", "#ba7a20")
svg.arrow(850, 3040, 850, 3260, "M429")
svg.arrow(2200, 3040, 1810, 3260, "M428")
svg.arrow(2200, 3040, 2770, 3260, "M430")
svg.arrow(1180, 3335, 3400, 3335, "Q0")
svg.arrow(2140, 3335, 3400, 3335, "Q1")
svg.arrow(3100, 3335, 3400, 3335, "Q2")

svg.box("swpin", 1580, 3700, 760, 170, "motion.switchkins-type", "float HAL input, 被截断为整数 type", "#ffffff", "#ba7a20")
svg.box("handle", 2580, 3700, 820, 170, "handle_kinematicsSwitch()", "servo-thread 每周期检测并调用 kinematicsSwitch(type)", "#ffffff", "#ba7a20")
svg.arrow(3760, 3410, 1960, 3700)
svg.arrow(2340, 3785, 2580, 3785)

# Section 5: motion data
svg.section(120, 4230, 4560, 1520, "5. 运行时运动数据流与 Vismach 显示", "#eef9f8")
svg.box("gcode", 240, 4390, 520, 150, "G-code XYZBC", "程序指令", "#ffffff", "#2b7a78")
svg.box("interp", 920, 4390, 520, 150, "interpreter", "RS274NGC", "#ffffff", "#2b7a78")
svg.box("task", 1600, 4390, 520, 150, "milltask", "任务层", "#ffffff", "#2b7a78")
svg.box("tp", 2280, 4390, 620, 150, "trajectory planner", "生成 carte_pos_cmd", "#ffffff", "#2b7a78")
svg.box("inv", 3060, 4390, 760, 150, "kinematicsInverse()", "按当前 type 分派", "#ffffff", "#2b7a78")
svg.box("joint", 3980, 4390, 560, 150, "joint targets", "X/Y/Z/B/C joint 目标", "#ffffff", "#2b7a78")
for x1, x2 in [(760, 920), (1440, 1600), (2120, 2280), (2900, 3060), (3820, 3980)]:
    svg.arrow(x1, 4465, x2, 4465)

svg.box("idinv", 540, 4840, 760, 150, "type 0: identity", "一一映射", "#ffffff", "#2b7a78")
svg.box("trtinv", 1510, 4840, 1040, 210, "type 1: xyzbcKinematicsInverse", "使用 B/C 角度、x-offset=-20、z-offset=-15、tool-offset、旋转中心计算 joint", "#ffffff", "#2b7a78")
svg.box("userkinv", 2760, 4840, 760, 150, "type 2: userk", "模板示例", "#ffffff", "#2b7a78")
svg.poly_arrow([(3440, 4540), (3440, 4720), (920, 4720), (920, 4840)])
svg.poly_arrow([(3440, 4540), (3440, 4720), (2030, 4720), (2030, 4840)])
svg.poly_arrow([(3440, 4540), (3440, 4720), (3140, 4720), (3140, 4840)])

svg.box("fb", 820, 5260, 1000, 160, "仿真反馈闭环", "joint.N.motor-pos-cmd -> pid/mux2 -> joint.N.motor-pos-fb", "#ffffff", "#2b7a78")
svg.box("vis", 2140, 5260, 1000, 160, "Vismach", "joint feedback 驱动 table/saddle/spindle/B/C 模型", "#ffffff", "#2b7a78")
svg.box("panel", 3460, 5260, 780, 160, "PyVCP 状态", "kinstype.is-N 显示当前运动学", "#ffffff", "#2b7a78")
svg.arrow(4260, 4540, 1320, 5260)
svg.arrow(1820, 5340, 2140, 5340)
svg.arrow(2980, 3870, 3850, 5260, "kinstype.is-N", dashed=True)

# Section 6: demo
svg.section(120, 5860, 4560, 1060, "6. 自动打开的演示 G-code 流程", "#f7f7f7")
svg.box("d1", 250, 6020, 660, 150, "xyzbc_switchkins.ngc", "AXIS OPEN_FILE", "#ffffff", "#5b6570")
svg.box("d2", 1080, 6020, 760, 150, "xyzbc_switchkins_sub", "四个象限重复", "#ffffff", "#5b6570")
svg.box("d3", 2010, 6020, 620, 150, "M429 identity", "安全定位 / 重设 G54", "#ffffff", "#5b6570")
svg.box("d4", 2800, 6020, 620, 150, "helix_bc", "准备螺旋插补", "#ffffff", "#5b6570")
svg.box("d5", 3590, 6020, 700, 150, "M428 xyzbc TRT", "B/C 倾斜后加工", "#ffffff", "#5b6570")
for x1, x2 in [(910, 1080), (1840, 2010), (2630, 2800), (3420, 3590)]:
    svg.arrow(x1, 6095, x2, 6095)

svg.note(
    360,
    6380,
    1780,
    330,
    "演示循环",
    [
        "每个象限先 M429 切回 identity。",
        "G53 回机床安全位置，G10 L20 P0 重设 G54。",
        "移动到象限中心后调用 helix_bc。",
    ],
)
svg.note(
    2540,
    6380,
    1780,
    330,
    "helix_bc 核心",
    [
        "M428 切换到 xyzbc TRT。",
        "G0 B#<b> C#<c> 设置转台角度。",
        "G2 I#<r> Z#<zmin> P#<n> 执行螺旋插补。",
    ],
)

svg.add('<text x="2400" y="7080" class="legend" text-anchor="middle">输出文件: 项目分析/5axis-xyzbc-trt-sim高清流程图.png；源文件: 项目分析/5axis-xyzbc-trt-sim高清流程图.svg</text>')

with open("项目分析/5axis-xyzbc-trt-sim高清流程图.svg", "w", encoding="utf-8") as f:
    f.write(svg.render())