Cosplay/nhf/touhou/houjuu_nue/joints.py

import math
from dataclasses import dataclass, field
from typing import Optional, Tuple
import cadquery as Cq
from nhf import Material, Role
from nhf.build import Model, target, assembly
from nhf.parts.box import MountingBox
from nhf.parts.springs import TorsionSpring
from nhf.parts.fasteners import FlatHeadBolt, HexNut, ThreaddedKnob, Washer
from nhf.parts.joints import TorsionJoint, HirthJoint
from nhf.parts.box import Hole, MountingBox, box_with_centre_holes
from nhf.touhou.houjuu_nue.electronics import (
    Winch, Flexor, LinearActuator, LINEAR_ACTUATOR_21,
)
import nhf.geometry
import nhf.utils

TOL = 1e-6

# Parts used

# uxcell 2 Pcs Star Knobs Grips M12 x 30mm Male Thread Steel Zinc Stud Replacement PP
HS_JOINT_KNOB = ThreaddedKnob(
    mass=77.3,
    diam_thread=12.0,
    height_thread=30.0,
    diam_knob=50.0,
    diam_neck=25.0,
    height_neck=12.5,
    height_knob=15.0,
)
# Tom's world 8Pcs M12-1.75 Hex Nut Assortment Set Stainless Steel 304(18-8)
# Metric Hexagon Nut for Bolts, Bright Finish, Full Thread (M12)
HS_JOINT_HEX_NUT = HexNut(
    mass=14.9,
    diam_thread=12.0,
    pitch=1.75,
    thickness=9.7,
    width=18.9,
)
SHOULDER_AXIS_BOLT = FlatHeadBolt(
    # FIXME: measure
    mass=0.0,
    diam_head=10.0,
    height_head=3.0,
    diam_thread=6.0,
    height_thread=20.0,
)
# Hoypeyfiy 10 Pieces Torsion Spring Woodworking DIY 90 Degrees Torsional
# Springs Repair Maintenance Spring
SHOULDER_TORSION_SPRING = TorsionSpring(
    mass=2.2,
    # inner diameter = 9
    radius=9/2 + 1.2,
    thickness=1.3,
    height=7.5,
)
# KALIONE 10 Pieces Torsion Spring, Stainless Steel Small Torsion Springs, Tiny
# Torsional Spring, 90° Deflection Compression Spring Kit for Repair Tools
# Woodworking DIY, 50mm
ELBOW_TORSION_SPRING = TorsionSpring(
    mass=1.7,
    radius=9 / 2,
    thickness=1.3,
    height=6.5,
    tail_length=45.0,
    right_handed=False,
)

ELBOW_AXLE_BOLT = FlatHeadBolt(
    mass=0.0,
    diam_head=6.87,
    height_head=3.06,
    diam_thread=4.0,
    height_thread=15.0,
)
ELBOW_AXLE_WASHER = Washer(
    mass=0.0,
    diam_outer=8.96,
    diam_thread=4.0,
    thickness=1.02,
    material_name="Nylon"
)
ELBOW_AXLE_HEX_NUT = HexNut(
    mass=0.0,
    diam_thread=4.0,
    pitch=0.7,
    thickness=3.6, # or 2.64 for metal
    width=6.89,
)

@dataclass
class RootJoint(Model):
    """
    The Houjuu-Scarlett Mechanism
    """
    knob: ThreaddedKnob = HS_JOINT_KNOB
    hex_nut: HexNut = HS_JOINT_HEX_NUT
    hirth_joint: HirthJoint = field(default_factory=lambda: HirthJoint(
        radius=25.0,
        radius_inner=15.0,
        tooth_height=5.60,
        base_height=4.0,
        n_tooth=24,
    ))
    parent_width: float = 85
    parent_thickness: float = 10
    parent_corner_fillet: float = 5
    parent_corner_cbore_diam: float = 12
    parent_corner_cbore_depth: float = 2
    parent_corner_inset: float = 12
    parent_mount_thickness: float = 25.4 / 16

    parent_substrate_thickness: float = 25.4 / 16
    parent_substrate_cull_corners: Tuple[bool, bool, bool, bool] = (False, False, True, False)
    parent_substrate_cull_edges: Tuple[bool, bool, bool, bool] = (False, False, True, False)

    child_corner_dx: float = 17.0
    child_corner_dz: float = 24.0

    axis_diam: float = 12.0
    axis_cbore_diam: float = 20
    axis_cbore_depth: float = 3
    corner_hole_diam: float = 6.0

    child_height: float = 60.0
    child_width: float = 50.0
    child_mount_thickness: float = 25.4 / 4

    def __post_init__(self):
        assert self.child_extra_thickness > 0.0
        assert self.parent_thickness >= self.hex_nut.thickness
        # twice the buffer allocated for substratum
        assert self.parent_width >= 4 * self.parent_corner_inset

    def corner_pos(self) -> list[tuple[int, int]]:
        """
        Generates a set of points corresponding to the connectorss
        """
        dx = self.parent_width / 2 - self.parent_corner_inset
        return [
            (dx, dx),
            (-dx, dx),
            (-dx, -dx),
            (dx, -dx),
        ]

    @property
    def child_extra_thickness(self) -> float:
        """
        Extra thickness allocated to child for padding
        """
        return self.knob.height_thread - self.hirth_joint.joint_height - self.child_mount_thickness - self.parent_thickness

    @property
    def base_to_surface_thickness(self) -> float:
        return self.hirth_joint.joint_height + self.child_extra_thickness

    @property
    def substrate_inset(self) -> float:
        return self.parent_corner_inset * 2

    def bridge_pair_horizontal(self, centre_dx: float) -> MountingBox:
        hole_dx = centre_dx / 2 - self.parent_width / 2 + self.parent_corner_inset
        hole_dy = self.parent_width / 2 - self.parent_corner_inset
        holes = [
            Hole(x=hole_dx, y=hole_dy),
            Hole(x=-hole_dx, y=hole_dy),
            Hole(x=-hole_dx, y=-hole_dy),
            Hole(x=hole_dx, y=-hole_dy),
        ]
        return MountingBox(
            length=centre_dx - self.parent_width + self.substrate_inset * 2,
            width=self.parent_width,
            thickness=self.parent_substrate_thickness,
            hole_diam=self.corner_hole_diam,
            holes=holes,
            centred=(True, True),
            generate_reverse_tags=True,
        )
    def bridge_pair_vertical(self, centre_dy: float) -> MountingBox:
        hole_dy = centre_dy / 2 - self.parent_width / 2 + self.parent_corner_inset
        holes = [
            Hole(x=0, y=hole_dy),
            Hole(x=0, y=-hole_dy),
        ]
        return MountingBox(
            length=self.substrate_inset,
            width=centre_dy - self.parent_width + self.substrate_inset * 2,
            thickness=self.parent_substrate_thickness,
            hole_diam=self.corner_hole_diam,
            holes=holes,
            centred=(True, True),
            generate_reverse_tags=True,
        )


    @target(name="parent")
    def parent(self):
        """
        Parent part of the Houjuu-Scarlett joint, which is composed of a Hirth
        coupling, a cylindrical base, and a mounting base.
        """
        hirth = self.hirth_joint.generate()
        conn = self.corner_pos()
        result = (
            Cq.Workplane('XY')
            .box(
                length=self.parent_width,
                width=self.parent_width,
                height=self.parent_thickness,
                centered=(True, True, False))
            .edges("|Z")
            .fillet(self.parent_corner_fillet)
            .faces(">Z")
            .workplane()
            .pushPoints(conn)
            .cboreHole(
                diameter=self.corner_hole_diam,
                cboreDiameter=self.parent_corner_cbore_diam,
                cboreDepth=self.parent_corner_cbore_depth)
        )
        sso = self.parent_width / 2 - self.substrate_inset
        loc_corner = Cq.Location((sso, sso, 0))
        loc_edge= Cq.Location((sso, -sso, 0))
        cut_corner = Cq.Solid.makeBox(
            length=self.parent_width,
            width=self.parent_width,
            height=self.parent_substrate_thickness,
        )
        cut_edge = Cq.Solid.makeBox(
            length=self.parent_width,
            width=self.parent_width - self.substrate_inset * 2,
            height=self.parent_substrate_thickness,
        )
        step = 90
        for i, flag in enumerate(self.parent_substrate_cull_corners):
            if not flag:
                continue
            loc = Cq.Location((0,0,0),(0,0,1), i * step) * loc_corner
            result = result.cut(cut_corner.located(loc))
        for i, flag in enumerate(self.parent_substrate_cull_edges):
            if not flag:
                continue
            loc = Cq.Location((0,0,0),(0,0,1), i * step) * loc_edge
            result = result.cut(cut_edge.located(loc))

        result = result.translate((0, 0, -self.parent_thickness))
        # Creates a plane parallel to the holes but shifted to the base
        plane = result.faces(">Z").workplane(offset=-self.parent_thickness)

        for i, (px, py) in enumerate(conn):
            plane.moveTo(px, py).tagPoint(f"h{i}")
        result = (
            result
            .faces(">Z")
            .workplane()
            .union(hirth, tol=0.1)
            .clean()
        )
        result = (
            result.faces("<Z")
            .workplane()
            .hole(diameter=self.axis_diam)
            .cut(self.hex_nut.generate().translate((0, 0, -self.parent_thickness)))
        )
        result.copyWorkplane(Cq.Workplane('XY', origin=(0,0,-self.parent_thickness))).tagPlane("base", direction="-Z")
        return result

    @target(name="child")
    def child(self) -> Cq.Workplane:
        hirth = self.hirth_joint.generate(is_mated=True)
        dy = self.child_corner_dx
        dx = self.child_corner_dz
        conn = [
            (-dx, -dy),
            (dx, -dy),
            (dx, dy),
            (-dx, dy),
        ]
        result = (
            Cq.Workplane('XY')
            .box(
                length=self.child_height,
                width=self.child_width,
                height=self.child_extra_thickness + self.hirth_joint.base_height,
                centered=(True, True, False))
            .translate((0, 0, -self.child_extra_thickness))
            #.edges("|Z")
            #.fillet(self.hs_joint_corner_fillet)
            .faces(">Z")
            .workplane()
            .pushPoints(conn)
            .hole(self.corner_hole_diam)
        )
        # Creates a plane parallel to the holes but shifted to the base
        plane = result.copyWorkplane(Cq.Workplane('XY', origin=(0, 0, -self.child_extra_thickness)))

        for i, (px, py) in enumerate(conn):
            plane.moveTo(px, py).tagPlane(f"conn{i}", direction="-Z")
        result = (
            result
            .faces(">Z")
            .workplane()
            .union(hirth, tol=0.1)
            .clean()
        )
        result = (
            result.faces("<Z")
            .workplane()
            .hole(self.axis_diam)
        )
        return result

    def assembly(self,
                 offset: int = 0,
                 fastener_pos: float = 0,
                 ignore_fasteners: bool = False) -> Cq.Assembly:
        """
        Specify knob position to determine the position of the knob from fully
        inserted (0) or fully uninserted (1)
        """
        knob_h = self.hex_nut.thickness
        result = (
            Cq.Assembly()
            .addS(self.parent(), name="parent",
                  material=Material.PLASTIC_PLA,
                  role=Role.PARENT)
            .constrain("parent", "Fixed")
            .addS(self.child(), name="child",
                  material=Material.PLASTIC_PLA,
                  role=Role.CHILD)
        )
        if not ignore_fasteners:
            (
                result
                .addS(self.hex_nut.assembly(), name="hex_nut")
                .addS(self.knob.assembly(), name="knob",
                      loc=Cq.Location((0, 0, knob_h * fastener_pos - self.parent_thickness)))
                .constrain("knob/thread", "Fixed")
                .constrain("hex_nut?bot", "parent?base", "Plane", param=0)
                .constrain("hex_nut?dirX", "parent@faces@>X", "Axis", param=0)
            )
        self.hirth_joint.add_constraints(
            result,
            "parent",
            "child",
            offset=offset
        )
        return result.solve()

@dataclass
class ShoulderJoint(Model):

    bolt: FlatHeadBolt = SHOULDER_AXIS_BOLT

    height: float = 70.0
    torsion_joint: TorsionJoint = field(default_factory=lambda: TorsionJoint(
        radius_track=18,
        radius_rider=18,
        groove_depth=4.8,
        groove_radius_outer=16,
        groove_radius_inner=13,
        track_disk_height=5.0,
        rider_disk_height=5.0,
        radius_axle=3.0,
        spring=SHOULDER_TORSION_SPRING,
        rider_slot_begin=0,
        rider_n_slots=1,
        rider_slot_span=0,
    ))

    # On the parent side, drill vertical holes

    parent_conn_hole_diam: float = 4.0
    # Position of the holes relative centre line
    parent_conn_hole_pos: list[Tuple[float, float]] = field(default_factory=lambda: [
        (15, 8),
        (15, -8),
    ])

    # Distance from centre of lips to the axis
    parent_lip_ext: float = 40.0

    parent_lip_length: float = 25.0
    parent_lip_width: float = 30.0
    parent_lip_thickness: float = 5.0

    # The parent side has arms which connect to the lips
    parent_arm_width: float = 25.0
    parent_arm_height: float = 12.0
    # remove a bit of material from the base so it does not interfere with gluing
    parent_arm_base_shift: float = 1.0

    # Generates a child guard which covers up the internals. The lip length is
    # relative to the +X surface of the guard.
    child_guard_ext: float = 20.0
    child_guard_width: float = 25.0
    # guard length measured from axle
    child_lip_ext: float = 50.0
    child_lip_width: float = 20.0
    child_lip_height: float = 40.0
    child_lip_thickness: float = 5.0
    child_conn_hole_diam: float = 4.0
    # Measured from centre of axle
    child_conn_hole_pos: list[float] = field(default_factory=lambda: [-15, -5, 5, 15])
    child_core_thickness: float = 3.0

    # Rotates the torsion joint to avoid collisions or for some other purpose
    axis_rotate_bot: float = 90
    axis_rotate_top: float = 0

    directrix_id: int = 0
    angle_neutral: float = -15.0
    angle_max_deflection: float = 65.0

    spool_radius_diff: float = 2.0
    # All the heights here are mirrored for the bottom as well
    spool_cap_height: float = 3.0
    spool_core_height: float = 2.0

    spool_line_thickness: float = 1.2
    spool_groove_radius: float = 10.0

    flip: bool = False
    winch: Optional[Winch] = None # Initialized later
    actuator: LinearActuator = LINEAR_ACTUATOR_21

    def __post_init__(self):
        assert self.parent_lip_length * 2 < self.height
        assert self.child_guard_ext > self.torsion_joint.radius_rider
        assert self.spool_groove_radius < self.spool_inner_radius < self.spool_outer_radius
        assert self.child_lip_height < self.height
        assert self.draft_length <= self.actuator.stroke_length
        self.winch = Winch(
            actuator=self.actuator,
            linear_motion_span=self.draft_length,
        )

    @property
    def spool_outer_radius(self):
        return self.torsion_joint.radius_rider - self.child_core_thickness
    @property
    def spool_inner_radius(self):
        return self.spool_outer_radius - self.spool_radius_diff

    @property
    def radius(self):
        return self.torsion_joint.radius

    @property
    def draft_length(self):
        """
        Amount of wires that need to draft on the spool
        """
        return self.spool_inner_radius * math.radians(self.angle_max_deflection)

    @property
    def draft_height(self):
        """
        Position of the middle of the spool measured from the middle
        """
        return 0

    @property
    def parent_lip_gap(self):
        return self.height - self.parent_lip_length * 2

    def parent_lip_loc(self, left: bool=True) -> Cq.Location:
        """
        2d location of the arm surface on the parent side, relative to axle
        """
        dy = self.parent_arm_width / 2
        sign = 1 if left else -1
        loc_dir = Cq.Location((0,sign * dy,0), (0, 0, 1), sign * 90)
        return Cq.Location.from2d(self.parent_lip_ext, 0, 0) * loc_dir
    def child_lip_loc(self) -> Cq.Location:
        """
        2d location to middle of lip
        """
        return Cq.Location.from2d(self.child_lip_ext - self.child_guard_ext, 0, 180)

    @property
    def _max_contraction_angle(self) -> float:
        return 180 - self.angle_max_deflection + self.angle_neutral

    def _contraction_cut_angle(self) -> float:
        aspect = self.child_guard_width / self.parent_arm_width
        theta = math.radians(self._max_contraction_angle)
        #theta_p = math.atan(math.sin(theta) / (math.cos(theta) + aspect))
        theta_p = math.atan2(math.sin(theta), math.cos(theta) + aspect)
        angle = math.degrees(theta_p)
        assert 0 <= angle <= 90
        return angle

    def _contraction_cut_geometry(self, parent: bool = False, mirror: bool=False) -> Cq.Solid:
        """
        Generates a cylindrical sector which cuts away overlapping regions of the child and parent
        """
        angle = self._contraction_cut_angle()
        # outer radius of the cut, overestimated
        cut_radius = math.sqrt(self.child_guard_width ** 2 + self.parent_arm_width ** 2)
        span = 180
        result = (
            Cq.Solid.makeCylinder(
                height=self.height,
                radius=cut_radius,
                angleDegrees=span,
            ).cut(Cq.Solid.makeCylinder(
                height=self.height,
                radius=self.torsion_joint.radius,
            ))
        )
        if parent:
            angle = - span - angle
        else:
            angle = self._max_contraction_angle - angle
        result = result.located(Cq.Location((0,0,-self.height/2), (0,0,1), angle))
        if mirror:
            result = result.mirror('XZ')
        return result

    def parent(self, top: bool = False) -> Cq.Assembly:
        joint = self.torsion_joint
        # Thickness of the lip connecting this joint to the wing root
        assert self.parent_arm_width <= joint.radius_track * 2
        assert self.parent_lip_ext > joint.radius_track

        cut_arm = Cq.Solid.makeBox(
            self.parent_lip_ext + self.parent_lip_width / 2,
            self.parent_arm_width,
            self.parent_arm_base_shift,
        )
        arm = (
            Cq.Solid.makeBox(
                self.parent_lip_ext + self.parent_lip_width / 2,
                self.parent_arm_width,
                self.parent_arm_height)
            .cut(cut_arm)
            .located(Cq.Location((0, -self.parent_arm_width/2 , 0)))
            .cut(Cq.Solid.makeCylinder(joint.radius_track, self.parent_arm_height))
            .cut(self._contraction_cut_geometry(parent=True, mirror=top))
        )
        t = self.parent_arm_base_shift
        lip_args = dict(
            length=self.parent_lip_length - t,
            width=self.parent_lip_width,
            thickness=self.parent_lip_thickness,
            hole_diam=self.parent_conn_hole_diam,
            generate_side_tags=False,
        )
        lip1 = MountingBox(
            **lip_args,
            holes=[
                Hole(x=self.height / 2 - x - t, y=-y)
                for x, y in self.parent_conn_hole_pos
            ],
        )
        lip2 = MountingBox(
            **lip_args,
            holes=[
                Hole(x=self.height / 2 - x - t, y=y)
                for x, y in self.parent_conn_hole_pos
            ],
        )
        lip_dy = self.parent_arm_width / 2 - self.parent_lip_thickness
        # Flip so the lip's holes point to -X
        loc_shift = Cq.Location((self.parent_arm_base_shift, 0, 0))
        loc_axis = Cq.Location((0,0,0), (0, 1, 0), -90)
        loc_dir1 = Cq.Location((0,lip_dy,0), (0, 0, 1), -90)
        loc_dir2 = Cq.Location((0,-lip_dy,0), (0, 0, 1), 90)
        loc_pos = Cq.Location((self.parent_lip_ext, 0, 0))

        rot = -self.axis_rotate_top if top else self.axis_rotate_bot

        lip_p_tag, lip_n_tag = "lip_right", "lip_left"
        if not top:
            lip_p_tag, lip_n_tag = lip_n_tag, lip_p_tag
        result = (
            Cq.Assembly()
            .add(joint.track(), name="track",
                 loc=Cq.Location((0, 0, 0), (0, 0, 1), rot))
            .add(arm, name="arm")
            .add(lip1.generate(), name=lip_p_tag, loc=loc_pos * loc_dir1 * loc_axis * loc_shift)
            .add(lip2.generate(), name=lip_n_tag, loc=loc_pos * loc_dir2 * loc_axis * loc_shift)
        )
        return result

    @target(name="parent-bot")
    def parent_bot(self) -> Cq.Assembly:
        return self.parent(top=False)
    @target(name="parent-top")
    def parent_top(self) -> Cq.Assembly:
        return self.parent(top=True)

    @property
    def child_height(self) -> float:
        """
        Calculates the y distance between two joint surfaces on the child side
        of the shoulder joint.
        """
        joint = self.torsion_joint
        return self.height - 2 * joint.total_height + 2 * joint.rider_disk_height

    def _spool(self) -> Cq.Compound:
        """
        Generates the spool piece which holds the line in tension
        """
        t = self.spool_line_thickness
        spindle = Cq.Solid.makeCone(
            radius1=self.spool_inner_radius,
            radius2=self.spool_outer_radius,
            height=self.spool_core_height,
        )
        cap = Cq.Solid.makeCylinder(
            radius=self.spool_outer_radius,
            height=self.spool_cap_height
        ).moved(Cq.Location((0,0,self.spool_core_height)))
        cut_height = self.spool_cap_height + self.spool_core_height
        cut_hole = Cq.Solid.makeCylinder(
            radius=t / 2,
            height=cut_height,
        ).moved(Cq.Location((self.spool_groove_radius, 0, 0)))
        cut_slot = Cq.Solid.makeBox(
            length=self.spool_outer_radius - self.spool_groove_radius,
            width=t,
            height=self.spool_core_height,
        ).moved(Cq.Location((self.spool_groove_radius, -t/2, 0)))
        cut_centre_hole = Cq.Solid.makeCylinder(
            radius=self.torsion_joint.radius_axle,
            height=cut_height,
        )
        top = spindle.fuse(cap).cut(cut_hole, cut_centre_hole, cut_slot)
        return (
            top
            .fuse(top.located(Cq.Location((0,0,0), (1,0, 0), 180)))
        )

    @target(name="child")
    def child(self) -> Cq.Assembly:
        """
        Creates the top/bottom shoulder child joint
        """

        joint = self.torsion_joint

        # Half of the height of the bridging cylinder
        dh = self.height / 2 - joint.total_height
        core_start_angle = 30

        radius_core_inner = joint.radius_rider - self.child_core_thickness
        core_profile1 = (
            Cq.Sketch()
            .arc((0, 0), joint.radius_rider, core_start_angle, self.angle_max_deflection)
            .segment((0, 0))
            .close()
            .assemble()
            .circle(radius_core_inner, mode='s')
        )
        core_profile2 = (
            Cq.Sketch()
            .arc((0, 0), joint.radius_rider, -core_start_angle, -(90 - self.angle_neutral))
            .segment((0, 0))
            .close()
            .assemble()
            .circle(radius_core_inner, mode='s')
        )
        angle_line_span = -self.angle_neutral + self.angle_max_deflection + 90
        angle_line = 180 - angle_line_span
        # leave space for the line to rotate
        spool_cut = Cq.Solid.makeCylinder(
            radius=joint.radius_rider * 2,
            height=self.spool_core_height * 2,
            angleDegrees=angle_line_span,
        ).moved(Cq.Location((0,0,-self.spool_core_height), (0,0,1), angle_line))
        lip_extension = (
            Cq.Solid.makeBox(
                length=self.child_lip_ext - self.child_guard_ext,
                width=self.child_lip_width,
                height=self.child_lip_height,
            ).cut(Cq.Solid.makeBox(
                length=self.child_lip_ext - self.child_guard_ext,
                width=self.child_lip_width - self.child_lip_thickness * 2,
                height=self.child_lip_height,
            ).located(Cq.Location((0, self.child_lip_thickness, 0))))
            .cut(Cq.Solid.makeBox(
                length=self.child_lip_ext - self.child_guard_ext - self.child_lip_thickness,
                width=self.child_lip_width,
                height=self.child_lip_height - self.child_lip_thickness * 2,
            ).located(Cq.Location((0, 0, self.child_lip_thickness))))
            .located(Cq.Location((
                self.child_guard_ext,
                -self.child_lip_width / 2,
                -self.child_lip_height / 2,
            )))
        )
        core_guard = (
            Cq.Workplane('XY')
            .box(
                length=self.child_guard_ext,
                width=self.child_guard_width,
                height=self.height,
                centered=(False, True, True),
            )
            #.copyWorkplane(Cq.Workplane('XY'))
            #.box(
            #    length=self.child_lip_ext,
            #    width=self.child_guard_width,
            #    height=self.child_lip_height,
            #    combine=True,
            #    centered=(False, True, True),
            #)
            .copyWorkplane(Cq.Workplane('XY'))
            .cylinder(
                radius=self.radius,
                height=self.height,
                combine='cut',
                centered=True,
            )
            .copyWorkplane(Cq.Workplane('XY'))
            .box(
                length=self.child_lip_ext,
                width=self.child_lip_width - 2 * self.child_lip_thickness,
                height=self.height - self.torsion_joint.total_height * 2,
                combine='cut',
                centered=(False, True, True),
            )
            .union(lip_extension)
            .cut(self._contraction_cut_geometry(parent=False))
        )
        core = (
            Cq.Workplane('XY')
            .placeSketch(core_profile1)
            .toPending()
            .extrude(dh * 2)
            .copyWorkplane(Cq.Workplane('XY'))
            .placeSketch(core_profile2)
            .toPending()
            .extrude(dh * 2)
            .translate(Cq.Vector(0, 0, -dh))
            .cut(spool_cut)
            .union(core_guard, tol=TOL)
        )
        assert self.child_lip_width / 2 <= joint.radius_rider
        sign = 1 if self.flip else -1
        holes = [Hole(x=sign * x) for x in self.child_conn_hole_pos]
        lip_obj = MountingBox(
            length=self.child_lip_height,
            width=self.child_lip_width,
            thickness=self.child_lip_thickness,
            holes=holes,
            hole_diam=self.child_conn_hole_diam,
            centred=(True, True),
            generate_side_tags=False,
            generate_reverse_tags=False,
        )
        theta = self.torsion_joint.spring.angle_neutral - self.torsion_joint.rider_slot_span
        loc_rotate = Cq.Location((0, 0, 0), (1, 0, 0), 180)
        loc_axis_rotate_bot = Cq.Location((0, 0, 0), (0, 0, 1), self.axis_rotate_bot + self.angle_neutral)
        loc_axis_rotate_top = Cq.Location((0, 0, 0), (0, 0, 1), self.axis_rotate_top + self.angle_neutral)
        spool_dz = 0
        spool_angle = -self.angle_neutral
        loc_spool_flip = Cq.Location((0,0,0),(0,1,0),180) if self.flip else Cq.Location()
        result = (
            Cq.Assembly()
            .add(core, name="core", loc=Cq.Location())
            .add(joint.rider(rider_slot_begin=-90, reverse_directrix_label=True), name="rider_top",
                 loc=loc_axis_rotate_top * Cq.Location((0, 0, dh), (0, 0, 1), -90) * Cq.Location((0, 0, 0), (0, 0, 1), theta))
            .add(joint.rider(rider_slot_begin=180), name="rider_bot",
                 loc=loc_axis_rotate_bot * Cq.Location((0, 0, -dh), (0, 0, 1), -90) * loc_rotate)
            .add(lip_obj.generate(), name="lip",
                 loc=Cq.Location((self.child_lip_ext - self.child_lip_thickness,0,0), (0,1,0), 90))
            .add(self._spool(), name="spool",
                 loc=loc_spool_flip * Cq.Location((0, 0, -spool_dz), (0, 0, 1), spool_angle))
        )
        return result

    @assembly()
    def assembly(
            self,
            fastener_pos: float = 0.0,
            deflection: float = 0.0,
            ignore_fasteners: bool = False,
    ) -> Cq.Assembly:
        assert deflection <= self.angle_max_deflection
        directrix = self.directrix_id
        mat = Material.RESIN_TRANSPERENT
        mat_spring = Material.STEEL_SPRING
        bolt_z = self.height / 2 + self.bolt.height_thread * (fastener_pos - 1)
        result = (
            Cq.Assembly()
            .addS(self.child(), name="child",
                  role=Role.CHILD, material=mat)
            .constrain("child/core", "Fixed")
            .addS(self.torsion_joint.spring.assembly(deflection=-deflection), name="spring_top",
                  role=Role.DAMPING, material=mat_spring)
            .addS(self.parent_top(),
                  name="parent_top",
                  role=Role.PARENT, material=mat)
            .addS(self.torsion_joint.spring.assembly(deflection=deflection), name="spring_bot",
                  role=Role.DAMPING, material=mat_spring)
            .addS(self.parent_bot(),
                  name="parent_bot",
                  role=Role.PARENT, material=mat)
        )
        if not ignore_fasteners:
            (
                result
                # Fasteners
                .addS(self.bolt.assembly(), name="bolt_top",
                      loc=Cq.Location((0, 0, bolt_z)))
                .constrain("bolt_top?root", 'Fixed')
                .addS(self.bolt.assembly(), name="bolt_bot",
                      loc=Cq.Location((0, 0, -bolt_z), (1,0,0), 180))
                .constrain("bolt_bot?root", 'Fixed')
            )
        TorsionJoint.add_constraints(
            result,
            rider="child/rider_top",
            track="parent_top/track",
            spring="spring_top",
            directrix=directrix)
        TorsionJoint.add_constraints(
            result,
            rider="child/rider_bot",
            track="parent_bot/track",
            spring="spring_bot",
            directrix=directrix)
        return result.solve()


@dataclass
class Beam:
    """
    A I-shaped spine with two feet
    """

    foot_length: float = 40.0
    foot_width: float = 20.0
    foot_height: float = 5.0
    spine_thickness: float = 4.0
    spine_length: float = 10.0
    total_height: float = 50.0

    hole_diam: float = 6.0
    # distance between the centres of the two holes
    hole_dist: float = 24.0

    def __post_init__(self):
        assert self.spine_height > 0
        assert self.hole_diam + self.hole_dist < self.foot_length
        assert self.hole_dist - self.hole_diam >= self.spine_length

    @property
    def spine_height(self):
        return self.total_height - self.foot_height * 2

    def foot(self) -> Cq.Workplane:
        """
        just one foot
        """
        dx = self.hole_dist / 2
        result = (
            Cq.Workplane('XZ')
            .box(self.foot_length, self.foot_width, self.foot_height,
                 centered=(True, True, False))
            .faces(">Y")
            .workplane()
            .pushPoints([(dx, 0), (-dx, 0)])
            .hole(self.hole_diam)
        )
        plane = result.faces(">Y").workplane()
        plane.moveTo(dx, 0).tagPlane("conn1")
        plane.moveTo(-dx, 0).tagPlane("conn0")
        return result

    def generate(self, flip: bool = False) -> Cq.Assembly:
        beam = (
            Cq.Workplane('XZ')
            .box(self.spine_length, self.spine_thickness, self.spine_height)
        )
        h = self.spine_height / 2 + self.foot_height

        tag_p, tag_n = "top", "bot"
        if flip:
            tag_p, tag_n = tag_n, tag_p
        result = (
            Cq.Assembly()
            .add(beam, name="beam")
            .add(self.foot(), name=tag_p,
                 loc=Cq.Location((0, h, 0)))
            .add(self.foot(), name=tag_n,
                 loc=Cq.Location((0, -h, 0), (1, 0, 0), 180))
        )
        return result


@dataclass
class DiskJoint(Model):
    """
    Sandwiched disk joint for the wrist and elbow

    We embed a spring inside the joint, with one leg in the disk and one leg in
    the housing. This provides torsion resistance.
    """
    spring: TorsionSpring = ELBOW_TORSION_SPRING

    radius_housing: float = 22.0
    radius_disk: float = 20.0

    housing_thickness: float = 2.0
    disk_thickness: float = 6.0
    tongue_thickness: float = 12.0

    # Amount by which the wall carves in
    wall_inset: float = 2.0
    # Height of the spring hole; if you make it too short the spring can't enter
    spring_tail_hole_height: float = 2.0

    # Spring angle at 0 degrees of movement
    spring_angle_at_0: float = 90.0
    spring_slot_offset: float = 5.0

    # Angular span of movement
    movement_angle: float = 120.0
    # leave some gap for cushion
    movement_gap: float = 5.0
    # Angular span of tongue on disk
    tongue_span: float = 25.0
    tongue_length: float = 10.0

    generate_inner_wall: bool = False

    axle_bolt: FlatHeadBolt = ELBOW_AXLE_BOLT
    axle_washer: Washer = ELBOW_AXLE_WASHER
    axle_hex_nut: HexNut = ELBOW_AXLE_HEX_NUT

    def __post_init__(self):
        super().__init__(name="disk-joint")
        assert self.radius_housing > self.radius_disk > self.radius_axle
        assert self.spring.height < self.housing_thickness + self.disk_thickness

        assert self.housing_upper_carve_offset > 0
        assert self.spring_tail_hole_height > self.spring.thickness

        assert self.axle_bolt.diam_thread == self.axle_washer.diam_thread
        assert self.axle_bolt.diam_thread == self.axle_hex_nut.diam_thread
        assert self.axle_bolt.height_thread > self.total_thickness, "Bolt is not long enough"

    @property
    def neutral_movement_angle(self) -> Optional[float]:
        a = self.spring.angle_neutral - self.spring_angle_at_0
        if 0 <= a and a <= self.movement_angle:
            return a
        return None

    @property
    def radius_axle(self) -> float:
        return self.axle_bolt.diam_thread

    @property
    def total_thickness(self) -> float:
        return self.housing_thickness * 2 + self.disk_thickness
    @property
    def disk_bot_thickness(self) -> float:
        """
        Pads the bottom of the disk up to spring height
        """
        return max(0, self.disk_thickness + self.spring.thickness - self.spring.height)

    @property
    def opening_span(self) -> float:
        return self.movement_angle + self.tongue_span

    @property
    def housing_upper_carve_offset(self) -> float:
        """
        Distance between the spring track and the outside of the upper housing
        """
        return self.spring_tail_hole_height + (self.disk_thickness - self.disk_bot_thickness) - self.spring.height

    @property
    def housing_upper_dz(self) -> float:
        """
        Distance between the default upper housing location and the median line
        """
        return self.total_thickness / 2 - self.housing_thickness

    def _disk_cut(self) -> Cq.Workplane:
        rl = Cq.Location((0, 0, 0), (0, 0, 1), self.spring_slot_offset)
        r = (
            Cq.Solid.makeBox(
                length=self.spring.tail_length,
                width=self.spring.thickness,
                height=self.disk_thickness - self.disk_bot_thickness,
            )
            .moved(rl * Cq.Location((0, self.spring.radius_inner, self.disk_bot_thickness)))
            #.rotate((0, 0, 0), (0, 0, 1), self.spring_slot_offset)
        )
        return (
            Cq.Workplane()
            .union(r)
            .val()
        )

    @target(name="disk")
    def disk(self) -> Cq.Workplane:
        radius_tongue = self.radius_disk + self.tongue_length
        outer_tongue = (
            Cq.Solid.makeCylinder(
                height=self.tongue_thickness,
                radius=radius_tongue,
                angleDegrees=self.tongue_span,
            ).cut(Cq.Solid.makeCylinder(
                height=self.tongue_thickness,
                radius=self.radius_housing,
            ))
            .moved(Cq.Location((0,0,(self.disk_thickness - self.tongue_thickness) / 2)))
        )
        inner_tongue = (
            Cq.Solid.makeCylinder(
                height=self.disk_thickness,
                radius=self.radius_housing,
                angleDegrees=self.tongue_span,
            ).cut(Cq.Solid.makeCylinder(
                height=self.disk_thickness,
                radius=self.radius_disk,
            ))
        )
        result = (
            Cq.Workplane('XY')
            .cylinder(
                height=self.disk_thickness,
                radius=self.radius_disk,
                centered=(True, True, False)
            )
            .union(inner_tongue, tol=TOL)
            .union(outer_tongue, tol=TOL)
            .copyWorkplane(Cq.Workplane('XY'))
            .cylinder(
                height=self.disk_thickness,
                radius=self.spring.radius,
                centered=(True, True, False),
                combine='cut',
            )
            .cut(self._disk_cut())
        )
        plane = result.copyWorkplane(Cq.Workplane('XY'))
        theta = math.radians(self.spring_slot_offset)
        plane.tagPlane("dir", direction=(math.cos(theta), math.sin(theta), 0))
        plane.workplane(offset=self.disk_thickness).tagPlane("mate_top")
        plane.workplane(offset=self.disk_bot_thickness).tagPlane("mate_spring")
        result.copyWorkplane(Cq.Workplane('YX')).tagPlane("mate_bot")

        return result

    def wall(self) -> Cq.Compound:
        height = self.disk_thickness + self.wall_inset
        wall = Cq.Solid.makeCylinder(
            radius=self.radius_housing,
            height=height,
            angleDegrees=360 - self.opening_span - self.movement_gap*2,
        ).cut(Cq.Solid.makeCylinder(
            radius=self.radius_disk,
            height=height,
        )).rotate((0, 0, 0), (0, 0, 1), self.opening_span+self.movement_gap)
        return wall

    @target(name="housing-lower")
    def housing_lower(self) -> Cq.Workplane:
        result = (
            Cq.Workplane('XY')
            .cylinder(
                radius=self.radius_housing,
                height=self.housing_thickness,
                centered=(True, True, False),
            )
            .cut(Cq.Solid.makeCylinder(
                radius=self.radius_axle,
                height=self.housing_thickness,
            ))
        )
        result.faces(">Z").tag("mate")
        result.faces("<Z").tag("bot")
        result.faces(">Z").workplane().tagPlane("dirX", direction="+X")
        result = result.cut(
            self
            .wall()
            .located(Cq.Location((0, 0, self.housing_thickness - self.wall_inset)))
            #.rotate((0, 0, 0), (1, 0, 0), 180)
            #.located(Cq.Location((0, 0, self.disk_thickness + self.housing_thickness)))
        )
        return result

    @target(name="housing-upper")
    def housing_upper(self) -> Cq.Workplane:
        carve_angle = -(self.spring_angle_at_0 - self.spring_slot_offset)
        carve = (
            Cq.Solid.makeCylinder(
                radius=self.spring.radius,
                height=self.spring_tail_hole_height,
            ).fuse(Cq.Solid.makeBox(
                length=self.spring.tail_length,
                width=self.spring.thickness,
                height=self.spring_tail_hole_height,
            ).located(Cq.Location((0, -self.spring.radius, 0))))
        ).rotate((0, 0, 0), (0, 0, 1), carve_angle)
        result = (
            Cq.Workplane('XY')
            .cylinder(
                radius=self.radius_housing,
                height=self.housing_thickness,
                centered=(True, True, False),
            )
        )
        theta = math.radians(carve_angle)
        result.faces("<Z").tag("mate")
        result.faces(">Z").tag("top")
        p_xy = result.copyWorkplane(Cq.Workplane('XY'))
        p_xy.tagPlane("dirX", direction="+X")
        p_xy.tagPlane("dir", direction=(math.cos(theta), math.sin(theta), 0))
        result = result.faces(">Z").hole(self.radius_axle * 2)

        # tube which holds the spring interior
        if self.generate_inner_wall:
            tube = (
                Cq.Solid.makeCylinder(
                    radius=self.radius_spring_internal,
                    height=self.disk_thickness + self.housing_thickness,
                ).cut(Cq.Solid.makeCylinder(
                    radius=self.radius_axle,
                    height=self.disk_thickness + self.housing_thickness,
                ))
            )
            result = result.union(tube)
        wall = (
            self.wall()
            .located(Cq.Location((0, 0, -self.disk_thickness-self.wall_inset)))
        )
        result = (
            result
            .union(wall, tol=TOL)
            #.cut(carve)
            .cut(carve.located(Cq.Location((0, 0, -self.housing_upper_carve_offset))))
        )
        return result.clean()

    def add_constraints(self,
                        assembly: Cq.Assembly,
                        housing_lower: str,
                        housing_upper: str,
                        disk: str,
                        angle: float = 0.0,
                        fasteners: bool = True,
                        fastener_prefix: str = "fastener",
                        ) -> Cq.Assembly:
        assert 0 <= angle <= self.movement_angle
        deflection = angle - (self.spring.angle_neutral - self.spring_angle_at_0)
        spring_name = disk.replace("/", "__Z") + "_spring"
        (
            assembly
            .addS(
                self.spring.assembly(deflection=-deflection),
                name=spring_name,
                role=Role.DAMPING,
                material=Material.STEEL_SPRING)
            .constrain(f"{disk}?mate_bot", f"{housing_lower}?mate", "Plane")
            .constrain(f"{disk}?mate_top", f"{housing_upper}?mate", "Plane")
            .constrain(f"{housing_lower}?dirX", f"{housing_upper}?dirX", "Axis", param=0)
            .constrain(f"{housing_upper}?dir", f"{spring_name}?dir_top", "Axis", param=0)
            .constrain(f"{spring_name}?dir_bot", f"{disk}?dir", "Axis", param=0)
            .constrain(f"{disk}?mate_spring", f"{spring_name}?bot", "Plane")
            #.constrain(f"{housing_lower}?dirX", f"{housing_upper}?dir", "Axis", param=0)
            #.constrain(f"{housing_lower}?dirX", f"{disk}?dir", "Axis", param=angle)
            #.constrain(f"{housing_lower}?dirY", f"{disk}?dir", "Axis", param=angle - 90)
        )
        if fasteners:
            tag_bolt = f"{fastener_prefix}_bolt"
            tag_nut = f"{fastener_prefix}_nut"
            (
                assembly
                .add(self.axle_bolt.assembly(), name=tag_bolt)
                .add(self.axle_hex_nut.assembly(), name=tag_nut)
                .constrain(f"{housing_lower}?bot", f"{tag_nut}?bot", "Plane")
                .constrain(f"{housing_upper}?top", f"{tag_bolt}?root", "Plane", param=0)
            )
        return (
            assembly
        )


    def assembly(self, angle: Optional[float] = 0) -> Cq.Assembly:
        if angle is None:
            angle = self.neutral_movement_angle
            if angle is None:
                angle = 0
        else:
            assert 0 <= angle <= self.movement_angle
        result = (
            Cq.Assembly()
            .addS(self.disk(), name="disk", role=Role.CHILD)
            .addS(self.housing_lower(), name="housing_lower", role=Role.PARENT)
            .addS(self.housing_upper(), name="housing_upper", role=Role.CASING)
            .constrain("housing_lower", "Fixed")
        )
        result = self.add_constraints(
            result,
            housing_lower="housing_lower",
            housing_upper="housing_upper",
            disk="disk",
            angle=angle,
        )
        return result.solve()


@dataclass(kw_only=True)
class ElbowJoint(Model):
    """
    Creates the elbow and wrist joints.

    This consists of a disk joint, where each side of the joint has mounting
    holes for connection to the exoskeleton. Each side 2 mounting feet on the
    top and bottom, and each foot has 2 holes.

    On the parent side, additional bolts are needed to clamp the two sides of
    the housing together.
    """

    disk_joint: DiskJoint = field(default_factory=lambda: DiskJoint(
        movement_angle=60,
    ))

    # Distance between the child/parent arm to the centre
    child_arm_radius: float = 40.0
    parent_arm_radius: float = 40.0

    lip_thickness: float = 5.0
    lip_length: float = 40.0
    # Carve which allows light to go through
    lip_side_depression_width: float = 10.0
    hole_pos: list[float] = field(default_factory=lambda: [12.5])
    parent_arm_width: float = 10.0
    # Angle of the beginning of the parent arm
    parent_arm_angle: float = 180.0

    # Size of the mounting holes
    hole_diam: float = 4.0

    material: Material = Material.RESIN_TRANSPERENT

    # If set to true, the joint is flipped upside down.
    flip: bool = False
    angle_neutral: float = 30.0

    actuator: Optional[LinearActuator] = None
    flexor: Optional[Flexor] = None
    # Rotates the entire flexor
    flexor_offset_angle: float = 0
    # Rotates the surface of the mount relative to radially inwards
    flexor_mount_angle_parent: float = 0
    flexor_mount_angle_child: float = -90
    flexor_pos_smaller: bool = True
    flexor_child_arm_radius: Optional[float] = None
    flexor_line_length: float = 0.0
    flexor_line_slack: float = 0.0
    flexor_parent_angle_fix: Optional[float] = 180.0
    flexor_child_angle_fix: Optional[float] = None

    def __post_init__(self):
        assert self.child_arm_radius > self.disk_joint.radius_housing
        assert self.parent_arm_radius > self.disk_joint.radius_housing
        self.disk_joint.tongue_length = self.child_arm_radius - self.disk_joint.radius_disk - self.lip_thickness / 2
        if self.actuator:
            self.flexor = Flexor(
                actuator=self.actuator,
                motion_span=self.motion_span,
                pos_smaller=self.flexor_pos_smaller,
                arm_radius=self.flexor_child_arm_radius,
                line_length=self.flexor_line_length,
                line_slack=self.flexor_line_slack,
            )

    def hole_loc_tags(self):
        """
        An iterator which iterates through positions of the hole and tags
        """
        for i, x in enumerate(self.hole_pos):
            yield x, f"conn_top{i}"
            yield -x, f"conn_bot{i}"

    @property
    def total_thickness(self) -> float:
        candidate1 = self.disk_joint.axle_bolt.height_thread
        candidate2 = self.disk_joint.total_thickness + self.disk_joint.axle_hex_nut.thickness
        head_thickness = self.disk_joint.axle_bolt.height_head
        return head_thickness + max(candidate1, candidate2)

    @property
    def motion_span(self) -> float:
        return self.disk_joint.movement_angle

    def parent_arm_loc(self) -> Cq.Location:
        """
        2d Location of the centre of the arm surface on the parent side, assuming
        axle is at position 0, and parent direction is -X
        """
        return Cq.Location.from2d(-self.parent_arm_radius, 0, 0)
    def child_arm_loc(self, angle: float = 0.0) -> Cq.Location:
        """
        2d Location of the centre of the arm surface on the child side, assuming
        axle is at position 0, and parent direction is -X
        """
        result = Cq.Location.rot2d(self.angle_neutral + angle) * Cq.Location.from2d(self.child_arm_radius, 0, 180)
        return result.flip_y() if self.flip else result
    def actuator_mount(self) -> Cq.Workplane:
        holes = [
            Hole(x=0, y=0, tag="mount"),
        ]
        mbox = MountingBox(
            length=self.disk_joint.total_thickness,
            width=self.disk_joint.total_thickness,
            thickness=self.lip_thickness,
            holes=holes,
            hole_diam=self.hole_diam,
            centred=(True, True),
            generate_side_tags=False,
        )
        return mbox.generate()
    def actuator_mount_loc(
            self,
            child: bool = False,
            # If set to true, use the local coordinate system
            unflip: bool = False,
    ) -> Cq.Location:
        # Moves the hole so the axle of the mount is perpendicular to it
        loc_mount = Cq.Location.from2d(self.flexor.mount_height, 0) * Cq.Location.rot2d(180)
        mount_angle = self.flexor_mount_angle_child if child else self.flexor_mount_angle_parent
        loc_mount_orient = Cq.Location.rot2d(mount_angle)
        # Moves the hole to be some distance apart from 0
        mount_r, mount_loc_angle, mount_parent_r = self.flexor.open_pos()
        loc_span = Cq.Location.from2d(mount_r if child else mount_parent_r, 0)
        if self.flexor_parent_angle_fix is not None:
            alpha = (-mount_loc_angle if child else 0) + self.flexor_parent_angle_fix - self.flexor_offset_angle
        elif self.flexor_child_angle_fix is not None:
            alpha = self.flexor_child_angle_fix + (0 if child else mount_loc_angle)
        else:
            raise ValueError("One of flexor_{parent,child}_angle_fix must be set")
        loc_rot = Cq.Location.rot2d(alpha)
        loc = loc_rot * loc_span * loc_mount_orient * loc_mount
        return loc.flip_y() if self.flip and not unflip else loc

    def lip(self) -> Cq.Workplane:
        sign = -1 if self.flip else 1
        holes = [
            h
            for i, x in enumerate(self.hole_pos)
            for h in [
                    Hole(x=sign * x, tag=f"conn_top{i}"),
                    Hole(x=-sign * x, tag=f"conn_bot{i}")
            ]
        ]
        def post(sketch: Cq.Sketch) -> Cq.Sketch:
            y_outer = self.disk_joint.total_thickness / 2
            y_inner = self.disk_joint.tongue_thickness / 2
            if y_outer < y_inner:
                return sketch
            y = (y_outer + y_inner) / 2
            width = self.lip_side_depression_width
            height = y_outer - y_inner
            return (
                sketch
                .push([(0, y), (0, -y)])
                .rect(width, height, mode='s')
            )
        mbox = MountingBox(
            length=self.lip_length,
            width=self.disk_joint.total_thickness,
            thickness=self.lip_thickness,
            holes=holes,
            hole_diam=self.hole_diam,
            centred=(True, True),
            generate_side_tags=False,
            generate_reverse_tags=True,
            profile_callback=post,
        )
        return mbox.generate()

    def child_joint(self, generate_mount: bool=False, generate_tags=True) -> Cq.Assembly:
        angle = -self.disk_joint.tongue_span / 2
        dz = self.disk_joint.disk_thickness / 2
        # We need to ensure the disk is on the "other" side so
        flip_x = Cq.Location((0, 0, 0), (1, 0, 0), 180)
        flip_z = Cq.Location((0, 0, 0), (0, 0, 1), 180)
        lip_dz = self.lip_thickness
        loc_lip = (
            Cq.Location((0, 0, 0), (0, 1, 0), 180) *
            Cq.Location((-lip_dz, 0, 0), (1, 0, 0), 90) *
            Cq.Location((0, 0, 0), (0, 1, 0), 90)
        )
        loc_rot_neutral = Cq.Location.rot2d(self.angle_neutral)
        loc_disk = flip_x * flip_z * Cq.Location((-self.child_arm_radius, 0, 0))
        loc_cut_rel = Cq.Location((0, self.disk_joint.spring.radius_inner, -self.disk_joint.disk_bot_thickness))
        loc_disk_orient = Cq.Location((0, 0, -dz), (0,0,1), angle)
        disk_cut = self.disk_joint._disk_cut().moved(
            #Cq.Location(0,0,0))
            loc_lip.inverse * loc_disk * loc_disk_orient)
        #lip_extra = Cq.Solid.makeBox(
        #    length=self.child_lip_extra_length,
        #    width=self.total_thickness,
        #    height=self.lip_thickness,
        #).located(Cq.Location((
        #    self.lip_length / 2,
        #    -self.total_thickness / 2,
        #    0,
        #)))
        result = (
            Cq.Assembly()
            .add(self.disk_joint.disk(), name="disk",
                 loc=loc_rot_neutral * loc_disk_orient)
            .add(self.lip().cut(disk_cut), name="lip",
                 loc=loc_rot_neutral * loc_disk.inverse * loc_lip)
            #.add(lip_extra, name="lip_extra",
            #     loc=loc_rot_neutral * loc_disk.inverse * loc_lip)
        )
        # Orientes the hole surface so it faces +X
        loc_thickness = Cq.Location((-self.lip_thickness, 0, 0), (0, 1, 0), 90)
        if self.flexor and generate_tags:
            loc_mount = self.actuator_mount_loc(child=True, unflip=True)
            result.add(
                Cq.Edge.makeLine((-1,0,0), (1,0,0)),
                name="act",
                loc=loc_mount)
            if generate_mount:
                # Orientes the hole surface so it faces +X
                loc_thickness = Cq.Location((-self.lip_thickness, 0, 0), (0, 1, 0), 90)
                result.add(
                    self.actuator_mount(),
                    name="act_mount",
                    loc=loc_mount * loc_thickness,
                )
        return result

    @target(name="child")
    def target_child(self) -> Cq.Assembly:
        return self.child_joint(generate_tags=False)

    @target(name="parent-lower")
    def parent_joint_lower(self) -> Cq.Workplane:
        return self.disk_joint.housing_lower()

    def parent_joint_upper(self, generate_mount: bool=False, generate_tags=True):
        axial_offset = Cq.Location((self.parent_arm_radius, 0, 0))
        housing_dz = self.disk_joint.housing_upper_dz
        conn_h = self.disk_joint.tongue_thickness
        conn_w = self.parent_arm_width
        connector = (
            Cq.Solid.makeBox(
                length=self.parent_arm_radius,
                width=conn_w,
                height=conn_h,
            ).located(Cq.Location((0, -conn_w/2, 0)))
            #Cq.Solid.makeCylinder(
            #    height=conn_h,
            #    radius=self.parent_arm_radius - self.lip_thickness / 2,
            #    angleDegrees=self.parent_arm_span)
            .cut(Cq.Solid.makeCylinder(
                height=conn_h,
                radius=self.disk_joint.radius_housing,
            ))
            .located(Cq.Location((0, 0, -conn_h / 2)))
            .rotate((0,0,0), (0,0,1), 180)
            #.rotate((0,0,0), (0,0,1), 180-self.parent_arm_span / 2)
        )
        housing = self.disk_joint.housing_upper()
        housing_loc = Cq.Location(
            (0, 0, housing_dz),
            (0, 0, 1),
            -self.disk_joint.tongue_span / 2 + self.angle_neutral
        )
        lip_dz = self.lip_thickness
        result = (
            Cq.Assembly()
            # rotate so 0 degree is at +X
            .add(housing, name="housing", loc=housing_loc)
            .add(self.lip(), name="lip", loc=
                 axial_offset.inverse *
                 Cq.Location((0, 0, 0), (0, 1, 0), 180) *
                 Cq.Location((-lip_dz, 0, 0), (1, 0, 0), 90) *
                 Cq.Location((0, 0, 0), (0, 1, 0), 90))
            .add(connector, name="connector")
            #.constrain("housing", "Fixed")
            #.constrain("connector", "Fixed")
            #.solve()
        )
        if self.flexor and generate_tags:
            loc_mount = self.actuator_mount_loc(child=False, unflip=True)
            result.add(
                Cq.Edge.makeLine((-1,0,0), (1,0,0)),
                name="act",
                loc=loc_mount)
            if generate_mount:
                # Orientes the hole surface so it faces +X
                loc_thickness = Cq.Location((-self.lip_thickness, 0, 0), (0, 1, 0), 90)
                result.add(
                    self.actuator_mount(),
                    name="act_mount",
                    loc=loc_mount * loc_thickness
                )
        return result

    @target(name="parent-upper")
    def target_parent_upper(self) -> Cq.Assembly:
        return self.parent_joint_upper(generate_tags=False)

    @assembly()
    def assembly(self,
                 angle: float = 0,
                 generate_mount: bool = False,
                 ignore_actuators: bool = False) -> Cq.Assembly:
        assert 0 <= angle <= self.motion_span
        result = (
            Cq.Assembly()
            .addS(self.child_joint(generate_mount=generate_mount), name="child",
                  role=Role.CHILD, material=self.material)
            .addS(self.parent_joint_lower(), name="parent_lower",
                  role=Role.CASING, material=self.material)
            .addS(self.parent_joint_upper(generate_mount=generate_mount), name="parent_upper",
                  role=Role.PARENT, material=self.material)
            #.constrain("child/disk?mate_bot", "Fixed")
        )
        result = self.disk_joint.add_constraints(
            result,
            housing_lower="parent_lower",
            housing_upper="parent_upper/housing",
            disk="child/disk",
            angle=angle,
        )
        if not ignore_actuators and self.flexor:
            target_length = self.flexor.target_length_at_angle(
                angle=angle,
            )
            self.flexor.add_to(
                result,
                target_length=target_length,
                tag_hole_back="parent_upper/act",
                tag_hole_front="child/act?mount",
                tag_dir="parent_lower?mate",
            )
        return result.solve()

if __name__ == '__main__':
    p = ShoulderJoint()
    p.build_all()
    p = DiskJoint()
    p.build_all()