#!/usr/bin/env python3
"""MuJoCo simulation and technical visualisation of a coffee grinder hopper."""
from pathlib import Path
import os
import math
import numpy as np
import mujoco
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
from matplotlib.collections import PolyCollection
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from matplotlib.patches import Polygon, Ellipse, Rectangle, FancyArrowPatch
from matplotlib.lines import Line2D

# ------------------------------- declared model -------------------------------
N_BEANS = 50
BEAN_LENGTH, BEAN_WIDTH, BEAN_THICKNESS = 10.0, 7.0, 5.2  # mm, overall envelope
BEAN_MASS_G = 0.13
THROAT_DIAMETER_MM = 18.0
DT = 0.001
DURATION = 5.0
GATE_TIME = 1.0
R_BOTTOM, R_TOP = 0.009, 0.045
Z_BOTTOM, Z_TOP = 0.042, 0.105
THROAT_PLANE = 0.039
OUT = Path(os.environ.get("OUT", "hopper.png"))
XML_PATH = Path(__file__).resolve().with_name("model.xml")
RNG = np.random.default_rng(20250308)

BG = "#f4f0e7"
INK = "#243235"
MUTED = "#6d7774"
HOPPER = "#477d7a"
COPPER = "#c87842"
COFFEE = ["#704329", "#7d4a2b", "#875333", "#633b27", "#925b35"]
DOME = ["#a66d42", "#ad7548", "#9a6039", "#b17a4c", "#a3693e"]


def initial_beans():
    """Six close-packed layers; long axes initially approximately horizontal."""
    counts = [1, 5, 6, 10, 12, 16]
    radii = [0.0, 0.0090, 0.0135, 0.0180, 0.0220, 0.0260]
    result = []
    for layer, (count, radius) in enumerate(zip(counts, radii)):
        z = 0.050 + layer * 0.007
        phase = 0.43 * layer
        for j in range(count):
            a = phase + 2 * np.pi * j / count
            x, y = radius * np.cos(a), radius * np.sin(a)
            # modest tilts preserve separation while breaking symmetry
            tilt_x, tilt_y = RNG.uniform(-0.20, 0.20, 2)
            yaw = a + RNG.uniform(-0.8, 0.8)
            result.append((x, y, z, tilt_x, tilt_y, yaw))
    assert len(result) == N_BEANS
    return result


def rotation_matrix(x_axis, y_axis, z_axis):
    return np.column_stack((x_axis, y_axis, z_axis))


def quat_from_matrix(R):
    q = np.empty(4)
    mujoco.mju_mat2Quat(q, R.ravel())
    return " ".join(f"{v:.10f}" for v in q)


def make_xml():
    wall_geoms = []
    nwall = 64
    dz = Z_TOP - Z_BOTTOM
    dr = R_TOP - R_BOTTOM
    slant = math.hypot(dz, dr)
    rmid = (R_TOP + R_BOTTOM) / 2
    zmid = (Z_TOP + Z_BOTTOM) / 2
    for i in range(nwall):
        a = 2 * np.pi * i / nwall
        radial = np.array([np.cos(a), np.sin(a), 0.0])
        tangent = np.array([-np.sin(a), np.cos(a), 0.0])
        up = (dr * radial + dz * np.array([0., 0., 1.])) / slant
        outward = np.cross(tangent, up)
        # Inner tangent surface lies on the nominal cone; 0.6 mm wall thickness.
        pos = rmid * radial + 0.00030 * outward + np.array([0., 0., zmid])
        quat = quat_from_matrix(rotation_matrix(outward, tangent, up))
        half_width = R_TOP * np.tan(np.pi / nwall) + 0.00008
        wall_geoms.append(
            f'<geom name="wall_{i:02d}" class="hopper_wall" type="box" '
            f'size="0.00030 {half_width:.8f} {slant/2 + 0.0002:.8f}" '
            f'pos="{pos[0]:.8f} {pos[1]:.8f} {pos[2]:.8f}" quat="{quat}"/>')

    bean_bodies = []
    for i, (x, y, z, rx, ry, rz) in enumerate(initial_beans()):
        # XYZ intrinsic Euler angles are sufficient for the initial orientations.
        bean_bodies.append(f'''
    <body name="bean_{i:02d}" pos="{x:.7f} {y:.7f} {z:.7f}" euler="{rx:.5f} {ry:.5f} {rz:.5f}">
      <freejoint name="bean_free_{i:02d}"/>
      <geom name="bean_{i:02d}_flat" class="bean" type="ellipsoid"
            size="0.0048 0.0034 0.00125" pos="0 0 -0.00055"
            rgba="0.30 0.13 0.065 1"/>
      <geom name="bean_{i:02d}_dome" class="bean" type="ellipsoid"
            size="0.0050 0.0035 0.00250" pos="0 0 0.00090"
            rgba="0.56 0.29 0.13 1"/>
      <inertial pos="0 0 0" mass="{BEAN_MASS_G/1000:.8f}"
                diaginertia="1.4e-8 2.2e-8 3.1e-8"/>
    </body>''')

    xml = f'''<?xml version="1.0" encoding="utf-8"?>
<mujoco model="50 roasted beans | 18 mm grinder hopper">
  <compiler angle="radian" coordinate="local" autolimits="true"/>
  <option timestep="{DT}" gravity="0 0 -9.81" integrator="implicitfast"
          solver="Newton" iterations="80" tolerance="1e-9"
          cone="elliptic" jacobian="sparse" impratio="1.4">
    <flag warmstart="enable" multiccd="enable"/>
  </option>
  <default>
    <default class="bean">
      <!-- Roasted coffee: 10 x 7 x 5.2 mm, 0.13 g; shallow flat + tall dome. -->
      <geom contype="1" conaffinity="1" friction="0.48 0.015 0.002"
            solref="0.003 1" solimp="0.94 0.99 0.001" margin="0.00005"/>
    </default>
    <default class="hopper_wall">
      <geom contype="1" conaffinity="1" friction="0.28 0.01 0.001"
            solref="0.003 1" solimp="0.96 0.995 0.001" rgba="0.20 0.43 0.42 0.32"/>
    </default>
    <default class="tray">
      <geom friction="0.68 0.02 0.003" solref="0.004 1" rgba="0.72 0.73 0.68 1"/>
    </default>
  </default>
  <worldbody>
    <light directional="true" pos="0.1 -0.2 0.3" dir="-0.2 0.3 -1" diffuse="0.8 0.8 0.8"/>
    <geom name="tray_floor" class="tray" type="box" size="0.056 0.049 0.002" pos="0 0 0"/>
    <geom name="tray_front" class="tray" type="box" size="0.056 0.0015 0.005" pos="0 -0.049 0.004"/>
    <geom name="tray_back" class="tray" type="box" size="0.056 0.0015 0.005" pos="0 0.049 0.004"/>
    <geom name="tray_left" class="tray" type="box" size="0.0015 0.049 0.005" pos="-0.056 0 0.004"/>
    <geom name="tray_right" class="tray" type="box" size="0.0015 0.049 0.005" pos="0.056 0 0.004"/>
    {''.join(wall_geoms)}
    <body name="sliding_gate" mocap="true" pos="0 0 0.0405">
      <geom name="gate" type="box" size="0.015 0.015 0.0015" pos="0 0 0"
            friction="0.32 0.01 0.001" solref="0.003 1" rgba="0.78 0.35 0.12 1"/>
    </body>
  </worldbody>
  <worldbody>
    {''.join(bean_bodies)}
  </worldbody>
</mujoco>
'''
    # MJCF permits only one worldbody: merge the two construction blocks.
    xml = xml.replace("  </worldbody>\n  <worldbody>\n", "\n")
    XML_PATH.write_text(xml)
    return xml


def simulate():
    model = mujoco.MjModel.from_xml_string(make_xml())
    data = mujoco.MjData(model)
    mujoco.mj_forward(model, data)
    gate_id = model.body("sliding_gate").id
    bean_ids = [model.body(f"bean_{i:02d}").id for i in range(N_BEANS)]
    bean_jids = [model.joint(f"bean_free_{i:02d}").id for i in range(N_BEANS)]
    wall_gid = {model.geom(f"wall_{i:02d}").id for i in range(64)}
    wanted = {0.0, 1.0, 2.0, 5.0}
    snapshots = {}
    crossings = np.full(N_BEANS, np.nan)
    prev_z = np.array([data.xpos[b, 2] for b in bean_ids])
    max_penetration = 0.0
    max_penetration_time = 0.0
    diagnostic = None
    diagnostic_score = -1
    escaped = set()
    total_steps = int(DURATION / DT)

    def capture(t):
        snapshots[t] = {
            "xpos": data.xpos[bean_ids].copy(),
            "xmat": data.xmat[bean_ids].reshape(N_BEANS, 3, 3).copy(),
            "gate": data.mocap_pos[model.body_mocapid[gate_id]].copy(),
        }

    capture(0.0)
    for step in range(1, total_steps + 1):
        t = step * DT
        # A 120 mm/s sliding stroke clears the throat in 0.15 s.
        if t <= GATE_TIME:
            opening = 0.0
        else:
            u = min(1.0, (t - GATE_TIME) / 0.25)
            opening = 0.030 * (u * u * (3 - 2 * u))
        data.mocap_pos[model.body_mocapid[gate_id]] = [opening, 0, 0.0405]
        mujoco.mj_step(model, data)

        z = data.xpos[bean_ids, 2]
        newly = np.where(np.isnan(crossings) & (prev_z >= THROAT_PLANE) & (z < THROAT_PLANE))[0]
        for i in newly:
            frac = (prev_z[i] - THROAT_PLANE) / max(prev_z[i] - z[i], 1e-12)
            crossings[i] = t - DT + frac * DT
        prev_z = z.copy()

        for c in data.contact:
            if c.geom1 in wall_gid or c.geom2 in wall_gid:
                pen = max(0.0, -float(c.dist))
                if pen > max_penetration:
                    max_penetration, max_penetration_time = pen, t

        # Preserve a busy post-opening frame with forces throughout the hopper.
        if 1.10 <= t <= 1.65 and step % 5 == 0:
            contacts = []
            for contact_index, c in enumerate(data.contact):
                force = np.zeros(6)
                mujoco.mj_contactForce(model, data, contact_index, force)
                vec = force[:3] @ np.asarray(c.frame).reshape(3, 3)
                if np.linalg.norm(vec) > 1e-7:
                    contacts.append((c.pos.copy(), vec.copy(), float(np.linalg.norm(vec)), c.geom1, c.geom2))
            score = len(contacts)
            if score > diagnostic_score:
                diagnostic_score = score
                diagnostic = {
                    "time": t, "contacts": contacts,
                    "xpos": data.xpos[bean_ids].copy(),
                    "xmat": data.xmat[bean_ids].reshape(N_BEANS, 3, 3).copy(),
                    "gate": np.array([opening, 0, 0.0405]),
                }

        for i, b in enumerate(bean_ids):
            p = data.xpos[b]
            if (not np.all(np.isfinite(p))) or abs(p[0]) > 0.15 or abs(p[1]) > 0.15 or p[2] < -0.03 or p[2] > 0.20:
                escaped.add(i)
        if any(abs(t - target) < DT / 2 for target in wanted):
            capture(round(t, 3))

    if diagnostic is None:
        raise RuntimeError("No diagnostic contacts recorded")
    times = np.arange(0, DURATION + DT / 2, DT)
    cumulative = np.array([np.sum(np.isfinite(crossings) & (crossings <= t)) for t in times])
    count_at = {t: int(np.sum(np.isfinite(crossings) & (crossings <= t + 1e-9))) for t in wanted}
    print(f"Maximum penetration into hopper walls: {max_penetration*1000:.4f} mm (t = {max_penetration_time:.3f} s)")
    print("Beans through throat: " + ", ".join(f"t={t:g}s: {count_at[t]}/50" for t in sorted(count_at)))
    print("Beans that left the model: " + (", ".join(f"bean_{i:02d}" for i in sorted(escaped)) if escaped else "none"))
    print(f"Diagnostic frame: t={diagnostic['time']:.3f}s, {len(diagnostic['contacts'])} contacts")
    return snapshots, diagnostic, times, cumulative, crossings, count_at


# --------------------------------- drawing ---------------------------------
U, V = np.meshgrid(np.linspace(0, 2*np.pi, 13), np.linspace(0, np.pi, 8))
SPHERE = np.array([np.cos(U)*np.sin(V), np.sin(U)*np.sin(V), np.cos(V)])


def setup_3d(ax):
    ax.set_facecolor(BG)
    ax.set_xlim(-0.058, 0.058)
    ax.set_ylim(-0.052, 0.052)
    ax.set_zlim(-0.002, 0.111)
    ax.set_box_aspect((1.05, 0.95, 1.10))
    ax.view_init(elev=17, azim=-62)
    ax.set_proj_type("ortho")
    ax.set_axis_off()
    ax.dist = 9


def draw_hopper_3d(ax, gate_x=0.0):
    # Tray as a shallow open box.
    xx, yy = np.meshgrid(np.linspace(-.056, .056, 2), np.linspace(-.049, .049, 2))
    # Keep the floor translucent: mplot3d otherwise depth-sorts it over the beans.
    ax.plot_surface(xx, yy, np.full_like(xx, .002), color="#d9d8ca", alpha=.18, shade=False, linewidth=0)
    for x in [-.056, .056]:
        ax.plot([x, x], [-.049, .049], [.006, .006], color="#9ca7a0", lw=.8)
    for y in [-.049, .049]:
        ax.plot([-.056, .056], [y, y], [.006, .006], color="#9ca7a0", lw=.8)
    # Cone: transparent silhouette and regular fabrication rings.
    theta = np.linspace(0, 2*np.pi, 65)
    zrows = np.linspace(Z_BOTTOM, Z_TOP, 6)
    for z in zrows:
        r = R_BOTTOM + (z-Z_BOTTOM)/(Z_TOP-Z_BOTTOM)*(R_TOP-R_BOTTOM)
        ax.plot(r*np.cos(theta), r*np.sin(theta), np.full_like(theta, z), color=HOPPER, lw=.55, alpha=.46)
    for a in np.linspace(0, 2*np.pi, 17)[:-1]:
        ax.plot([R_BOTTOM*np.cos(a), R_TOP*np.cos(a)], [R_BOTTOM*np.sin(a), R_TOP*np.sin(a)], [Z_BOTTOM, Z_TOP], color=HOPPER, lw=.55, alpha=.42)
    ax.plot(R_TOP*np.cos(theta), R_TOP*np.sin(theta), np.full_like(theta, Z_TOP), color=HOPPER, lw=1.6)
    ax.plot(R_BOTTOM*np.cos(theta), R_BOTTOM*np.sin(theta), np.full_like(theta, Z_BOTTOM), color=HOPPER, lw=1.1)
    # Sliding gate, shown as a copper plate below the throat.
    gx = gate_x
    verts = [[(gx-.015,-.015,.0405),(gx+.015,-.015,.0405),(gx+.015,.015,.0405),(gx-.015,.015,.0405)]]
    p = Poly3DCollection(verts, facecolor=COPPER, edgecolor="#9f582c", alpha=.78, linewidth=.6)
    ax.add_collection3d(p)


def draw_beans_3d(ax, positions, rotations):
    # Back-to-front sorting reduces transparency artefacts in the small views.
    order = np.argsort(positions[:, 1])
    for i in order:
        color, dome = COFFEE[i % len(COFFEE)], DOME[i % len(DOME)]
        for size, offset, col in [
            (np.array([.0048,.0034,.00125]), np.array([0,0,-.00055]), color),
            (np.array([.0050,.0035,.00250]), np.array([0,0,.00090]), dome),
        ]:
            pts = SPHERE * size[:, None, None]
            shape = pts.shape
            pts = rotations[i] @ pts.reshape(3, -1)
            pts = pts.reshape(shape) + (positions[i] + rotations[i] @ offset)[:, None, None]
            ax.plot_surface(pts[0], pts[1], pts[2], color=col, linewidth=0, antialiased=False, shade=True, rcount=8, ccount=13)


def draw_snapshot(ax, snap, t, count):
    setup_3d(ax)
    draw_hopper_3d(ax, snap["gate"][0])
    draw_beans_3d(ax, snap["xpos"], snap["xmat"])
    ax.text2D(.01, .96, f"{t:g} s", transform=ax.transAxes, fontsize=15, fontweight="bold", color=INK, va="top")
    ax.text2D(.01, .865, f"{count:02d} through", transform=ax.transAxes, fontsize=9.5, color=MUTED, va="top")
    if t == 0:
        ax.text2D(.01, .06, "GATE CLOSED", transform=ax.transAxes, fontsize=7.5, fontweight="bold", color=COPPER)
    elif t == 1:
        ax.text2D(.01, .06, "GATE STARTS", transform=ax.transAxes, fontsize=7.5, fontweight="bold", color=COPPER)
    elif t == 5:
        ax.text2D(.01, .06, "TRAY COLLECTION", transform=ax.transAxes, fontsize=7.5, fontweight="bold", color=HOPPER)


def draw_diagnostic(ax, diag):
    ax.set_facecolor("#eceee7")
    # x-z section through the actual 3-D contact set.
    cone = np.array([[-R_TOP,Z_TOP],[-R_BOTTOM,Z_BOTTOM],[R_BOTTOM,Z_BOTTOM],[R_TOP,Z_TOP]])
    ax.add_patch(Polygon(cone, closed=True, fill=False, ec=HOPPER, lw=1.5))
    for z in [Z_BOTTOM, Z_TOP]:
        r = R_BOTTOM if z == Z_BOTTOM else R_TOP
        ax.plot([-r,r],[z,z], color=HOPPER, lw=.7, alpha=.5)
    ax.add_patch(Rectangle((diag["gate"][0]-.015,.039),.030,.003,fc=COPPER,ec="#9f582c",lw=.7,alpha=.85))
    ax.add_patch(Rectangle((-.056,0),.112,.002,fc="#c9cdc3",ec="none"))
    # Projected bean silhouettes; each body remains two overlapping ellipses.
    for i in range(N_BEANS):
        p, R = diag["xpos"][i], diag["xmat"][i]
        if p[2] < .003:
            continue
        for size, off, col in [(.0048,.00125,COFFEE[i%5]),(.0050,.0025,DOME[i%5])]:
            c = p + R @ np.array([0,0,off-.00055 if off < .002 else .0009])
            ax.add_patch(Ellipse((c[0],c[2]),2*size,2*off,fc=col,ec="none",alpha=.90,zorder=3))
    contacts = sorted(diag["contacts"], key=lambda c:c[2], reverse=True)
    shown = contacts[:26]
    for p, f, mag, g1, g2 in reversed(shown):
        # Projected physical force, common scale; arrowhead is kept legible.
        scale = 2.0  # metres of arrow per newton; common scale for every vector
        dx, dz = scale*f[0], scale*f[2]
        if abs(dx)+abs(dz) < 1e-7:
            continue
        ax.arrow(p[0],p[2],dx,dz,width=.00010,head_width=.0013,head_length=.0017,
                 length_includes_head=True,color="#d4553c",alpha=.82,zorder=6)
    pts = np.array([c[0] for c in shown])
    if len(pts):
        ax.scatter(pts[:,0],pts[:,2],s=14,fc="#f9d35b",ec=INK,lw=.35,zorder=7)
    ax.set_xlim(-.060,.060)
    ax.set_ylim(-.004,.125)
    ax.set_aspect("equal")
    ax.set_xticks([]); ax.set_yticks([])
    for s in ax.spines.values(): s.set_visible(False)
    ax.text(.02,.985,"CONTACT DIAGNOSTIC",transform=ax.transAxes,fontsize=10.5,fontweight="bold",color=INK,va="top")
    ax.text(.02,.915,f"x–z projection  ·  t = {diag['time']:.3f} s",transform=ax.transAxes,fontsize=8.5,color=MUTED,va="top")
    ax.text(.02,.07,f"{len(contacts)} active contacts",transform=ax.transAxes,fontsize=8,color=INK)
    ax.text(.02,.025,"strongest 26 vectors shown",transform=ax.transAxes,fontsize=7.2,color=MUTED)
    ax.arrow(.012,.012,.020,0,width=.00012,head_width=.0015,head_length=.0018,length_includes_head=True,color="#d4553c")
    ax.text(.022,.017,"10 mN",fontsize=7.2,color="#a34432",ha="center")
    ax.scatter([.047],[.012],s=18,fc="#f9d35b",ec=INK,lw=.4)
    ax.text(.047,.017,"contact",fontsize=7.2,color=INK,ha="center")


def draw_throughput(ax, times, cumulative, crossings, count_at):
    ax.set_facecolor(BG)
    ax.step(times,cumulative,where="post",color=HOPPER,lw=2.4,zorder=3)
    ax.fill_between(times,cumulative,step="post",color=HOPPER,alpha=.12,zorder=2)
    ax.set_xlim(0,5); ax.set_ylim(0,55)
    ax.set_xticks([0,1,2,3,4,5]); ax.set_yticks([0,10,20,30,40,50])
    ax.grid(axis="y",color="#cdd2c9",lw=.6)
    ax.set_xlabel("SIMULATION TIME  (s)",fontsize=8,color=MUTED,labelpad=7)
    ax.set_ylabel("CUMULATIVE BEANS THROUGH THROAT",fontsize=8,color=MUTED,labelpad=7)
    ax.tick_params(labelsize=8,colors=MUTED,length=0)
    for s in ax.spines.values(): s.set_visible(False)
    ax.axvline(1,color=COPPER,lw=1,ls=(0,(3,3)))
    ax.text(1.06,49,"gate opens",fontsize=7.8,color=COPPER,va="top")
    for t in [0,1,2,5]:
        n=count_at[t]
        ax.scatter([t],[n],s=34,fc=BG,ec=HOPPER,lw=1.6,zorder=4)
        xytext = (8,9) if t < 5 else (-8,10)
        ax.annotate(f"{n} @ {t:g}s",(t,n),xytext=xytext,textcoords="offset points",fontsize=8,color=INK,fontweight="bold",ha="left" if t<5 else "right")
    valid=crossings[np.isfinite(crossings)]
    if len(valid):
        bins=np.arange(0,5.0001,.25)
        hist,_=np.histogram(valid,bins)
        # Quiet lower-band bars show instantaneous batches without a second axis.
        ax.bar(bins[:-1]+.125,hist*.34,width=.19,bottom=0,color=COPPER,alpha=.52,zorder=1)
        ax.text(4.94,4.5,"orange bars: crossings / 0.25 s",ha="right",fontsize=7,color=MUTED)
    ax.set_title("THROAT THROUGHPUT",loc="left",fontsize=10.5,fontweight="bold",color=INK,pad=12)


def make_figure(snapshots, diagnostic, times, cumulative, crossings, count_at):
    fig = plt.figure(figsize=(15.6,10.2),facecolor=BG)
    gs = fig.add_gridspec(2,3,left=.035,right=.975,top=.855,bottom=.095,wspace=.08,hspace=.23)
    for ax,t in zip([fig.add_subplot(gs[0,i],projection="3d") for i in range(3)],[0.0,1.0,2.0]):
        draw_snapshot(ax,snapshots[t],t,count_at[t])
    draw_snapshot(fig.add_subplot(gs[1,0],projection="3d"),snapshots[5.0],5.0,count_at[5.0])
    draw_diagnostic(fig.add_subplot(gs[1,1]),diagnostic)
    draw_throughput(fig.add_subplot(gs[1,2]),times,cumulative,crossings,count_at)
    fig.text(.04,.955,"GRAVITY, IN SMALL BATCHES",fontsize=25,fontweight="bold",color=INK,ha="left")
    fig.text(.04,.914,"50 roasted coffee beans  /  conical grinder hopper  /  five-second MuJoCo study",fontsize=11.5,color=MUTED,ha="left")
    fig.text(.04,.875,"BEAN  10 × 7 × 5.2 mm   ·   0.13 g   ·   two overlapping ellipsoids (shallow flat face + domed face)      THROAT  Ø 18.0 mm      GATE  closed → open at 1.00 s",fontsize=9.3,color=INK)
    fig.text(.04,.042,"MODEL  cone 90 → 18 mm  ·  bean friction μ 0.48  ·  wall μ 0.28  ·  tray μ 0.68  ·  tray 112 × 98 mm",fontsize=8.4,color=MUTED)
    fig.text(.04,.020,"SOLVER  Newton / elliptic cone  ·  implicitfast  ·  Δt 1 ms  ·  80 iterations  ·  multi-CCD  ·  gravity 9.81 m s⁻²",fontsize=8.4,color=MUTED)
    fig.text(.96,.035,f"{count_at[5.0]} / 50",fontsize=20,fontweight="bold",color=HOPPER,ha="right")
    fig.text(.96,.018,"COLLECTED AT 5 s",fontsize=7.5,color=MUTED,ha="right")
    fig.savefig(OUT,dpi=180,facecolor=BG,bbox_inches="tight",pad_inches=.18)
    print(f"Figure saved to {OUT}")
    print(f"MJCF saved to {XML_PATH}")


if __name__ == "__main__":
    make_figure(*simulate())