# Mesure la pose Idle (image 0) : mannequin, Bridget, Michael.
# But : voir si la main passe derriere le buste, et de quel angle le bras est roule.

import unreal

PAIRS = (
    ("source", "/Game/Quaternius/UAL/Idle_Loop.Idle_Loop", "/Game/Quaternius/UAL/UAL1_Standard.UAL1_Standard",
     ("pelvis", "spine_03", "clavicle_l", "upperarm_l", "lowerarm_l", "hand_l",
      "clavicle_r", "upperarm_r", "lowerarm_r", "hand_r")),
    ("bridget", "/Game/Quaternius/UAL/B_Idle_Loop.B_Idle_Loop", "/Game/Heavy/Daz/Bridget/bridget_gp.bridget_gp",
     ("pelvis", "chestUpper", "lCollar", "lShldrBend", "lForearmBend", "lHand",
      "rCollar", "rShldrBend", "rForearmBend", "rHand")),
    ("michael", "/Game/Quaternius/UAL/M_Idle_Loop.M_Idle_Loop", "/Game/Heavy/Daz/Michael/michael.michael",
     ("pelvis", "chestUpper", "lCollar", "lShldrBend", "lForearmBend", "lHand",
      "rCollar", "rShldrBend", "rForearmBend", "rHand")),
)

FINGERS = (
    "lThumb1", "lIndex1", "lMid1", "lRing1", "lPinky1",
    "thumb_01_l", "index_01_l", "middle_01_l", "pinky_01_l",
    "hand_l", "lHand",
)


def dot(a, b):
    return a.x * b.x + a.y * b.y + a.z * b.z


def sub(a, b):
    return unreal.MathLibrary.subtract_vector_vector(a, b)


def norm(v):
    return unreal.MathLibrary.normal(v)


def cross(a, b):
    return unreal.MathLibrary.cross_vector_vector(a, b)


def fmt(v):
    return "(%.1f %.1f %.1f)" % (v.x, v.y, v.z)


def bone_names(mesh):
    pose = mesh.skeleton.get_reference_pose()
    names = unreal.AnimPoseExtensions.get_bone_names(pose)
    return [str(n) for n in names]


def pose_at_zero(sequence):
    for call in (
        lambda: unreal.AnimPoseExtensions.get_anim_pose_at_time(sequence, 0.0),
        lambda: unreal.AnimPoseExtensions.get_anim_pose_at_frame(sequence, 0),
    ):
        try:
            return call()
        except Exception as exc:
            unreal.log_warning("POSE api %s" % exc)
    raise RuntimeError("pas de lecture de pose")


def bone_tm(pose, name):
    return unreal.AnimPoseExtensions.get_bone_pose(pose, name, unreal.AnimPoseSpaces.WORLD)


def main():
    unreal.log_warning("POSE debut")
    for label, anim_path, mesh_path, bones in PAIRS:
        anim = unreal.load_asset(anim_path)
        mesh = unreal.load_asset(mesh_path)
        if not anim or not mesh:
            unreal.log_error("POSE absent %s" % label)
            continue
        names = bone_names(mesh)
        pose = pose_at_zero(anim)
        got = {}
        for bone in bones:
            if bone not in names:
                unreal.log_warning("POSE %s os absent %s" % (label, bone))
                continue
            got[bone] = bone_tm(pose, bone).translation
        pelvis = got.get("pelvis")
        chest_name = "spine_03" if "spine_03" in got else "chestUpper"
        chest = got.get(chest_name)
        if not pelvis or not chest:
            unreal.log_error("POSE %s sans buste" % label)
            continue
        up = norm(sub(chest, pelvis))
        # Avant approximatif : produit du haut du buste et de l'axe gauche-droite des epaules.
        left = got.get("clavicle_l") or got.get("upperarm_l") or got.get("lCollar") or got.get("lShldrBend")
        right = got.get("clavicle_r") or got.get("upperarm_r") or got.get("rCollar") or got.get("rShldrBend")
        if not left or not right:
            unreal.log_error("POSE %s sans epaules" % label)
            continue
        side = norm(sub(left, right))
        forward = norm(cross(side, up))
        unreal.log_warning("POSE %s avant %s" % (label, fmt(forward)))
        for side_name, hand_name, elbow_name in (
            ("gauche", "hand_l" if "hand_l" in got else "lHand", "lowerarm_l" if "lowerarm_l" in got else "lForearmBend"),
            ("droite", "hand_r" if "hand_r" in got else "rHand", "lowerarm_r" if "lowerarm_r" in got else "rForearmBend"),
        ):
            hand = got.get(hand_name)
            elbow = got.get(elbow_name)
            if not hand:
                continue
            rel = sub(hand, chest)
            ahead = dot(rel, forward)
            unreal.log_warning(
                "POSE %s main %s %s devant %.1f pos %s" % (label, side_name, hand_name, ahead, fmt(hand)))
            if elbow:
                unreal.log_warning("POSE %s coude %s %s" % (label, side_name, fmt(elbow)))
        present = [n for n in FINGERS if n in names]
        unreal.log_warning("POSE %s doigts %s" % (label, " ".join(present)))
    unreal.log_warning("POSE fin")
    unreal.SystemLibrary.quit_editor()


main()