using System.Reflection;
using pdinklag.CPW.Common;
using RootMotion.Dynamics;
using UnityEngine;

[RequireComponent(typeof(BodyDescription))]
public class PuppetMasterTest : MonoBehaviour
{
    private static readonly float MAX_DEPENETRATION_VELOCITY = 0.5f;
    private static readonly int RAGDOLL_LAYER = 0;

    public float Weight = 100.0f;
    private void Start()
    {
        var pm = Util.SetupPuppetMaster(GetComponent<BodyDescription>(), Weight);

        // group in new game objects
        var root = new GameObject(gameObject.name + " (Root)").transform;
        transform.SetParent(root, true);
        pm.transform.SetParent(root, true);
    }

    /// <summary>
    /// Alternative to the BipedRagdollCreator; creates a ragdoll based on a BodyDescription.
    /// </summary>
    public static class Util
    {
        private delegate void BipedRagdollCreatorAPI(BipedRagdollReferences r, BipedRagdollCreator.Options o);
        private static readonly BipedRagdollCreatorAPI BipedRagdollCreator__CreateJoints;
        private static readonly BipedRagdollCreatorAPI BipedRagdollCreator__MassDistribution;

        static Util()
        {
            BipedRagdollCreator__CreateJoints = (BipedRagdollCreatorAPI)typeof(BipedRagdollCreator)
                .GetMethod("CreateJoints", BindingFlags.Static | BindingFlags.NonPublic)
                .CreateDelegate(typeof(BipedRagdollCreatorAPI));
            BipedRagdollCreator__MassDistribution = (BipedRagdollCreatorAPI)typeof(BipedRagdollCreator)
                .GetMethod("MassDistribution", BindingFlags.Static | BindingFlags.NonPublic)
                .CreateDelegate(typeof(BipedRagdollCreatorAPI));
        }

        private static void SetJointAxes(Transform part, Vector3 mainAxis, Vector3 secondaryAxis)
        {
            var joint = part.GetComponent<ConfigurableJoint>();
            if (joint)
            {
                joint.axis = mainAxis;
                joint.secondaryAxis = secondaryAxis;
            }
            else
            {
                Debug.LogWarning(part + " has no ConfigurableJoint component");
            }
        }

        private static void SetFootJointLimits(ConfigurableJoint foot)
        {
            var angularXLow = foot.lowAngularXLimit;
            var angularXHigh = foot.highAngularXLimit;

            angularXLow.limit = -11.25f;
            angularXHigh.limit = 80.0f;

            foot.lowAngularXLimit = angularXLow;
            foot.highAngularXLimit = angularXHigh;

            var angularY = foot.angularYLimit;
            angularY.limit = 11.25f;
            foot.angularYLimit = angularY;
        }

        private static void SetLowerLegJointLimits(ConfigurableJoint lowerLeg)
        {
            var angularXLow = lowerLeg.lowAngularXLimit;
            angularXLow.limit = 0.0f;
            lowerLeg.lowAngularXLimit = angularXLow;
        }

        private static void SetUpperLegJointLimits(ConfigurableJoint upperLeg)
        {
            var angularXLow = upperLeg.lowAngularXLimit;
            angularXLow.limit = -75.0f;
            upperLeg.lowAngularXLimit = angularXLow;

            var angularY = upperLeg.angularYLimit;
            angularY.limit = 45.0f;
            upperLeg.angularYLimit = angularY;
        }

        private static void CreateJoints(BodyDescription desc, BipedRagdollReferences r, BipedRagdollCreator.Options o)
        {
            // let PuppetMaster set them up
            BipedRagdollCreator__CreateJoints(r, o);

            // set joint limits
            SetFootJointLimits(r.leftFoot.GetComponent<ConfigurableJoint>());
            SetFootJointLimits(r.rightFoot.GetComponent<ConfigurableJoint>());
            SetLowerLegJointLimits(r.leftLowerLeg.GetComponent<ConfigurableJoint>());
            SetLowerLegJointLimits(r.rightLowerLeg.GetComponent<ConfigurableJoint>());
            SetUpperLegJointLimits(r.leftUpperLeg.GetComponent<ConfigurableJoint>());
            SetUpperLegJointLimits(r.rightUpperLeg.GetComponent<ConfigurableJoint>());

            // fix joint axes
            SetJointAxes(r.head, desc.HeadOrientation.Right(), desc.HeadOrientation.Forward());
            SetJointAxes(r.spine, desc.SpineOrientation.Right(), desc.SpineOrientation.Forward());

            SetJointAxes(r.leftUpperArm, desc.LeftUpperArmOrientation.Down(), desc.LeftUpperArmOrientation.Back());
            SetJointAxes(r.leftLowerArm, desc.LeftLowerArmOrientation.Down(), desc.LeftLowerArmOrientation.Back());
            SetJointAxes(r.leftHand, desc.LeftHandOrientation.Down(), desc.LeftHandOrientation.Back());
            SetJointAxes(r.rightUpperArm, desc.RightUpperArmOrientation.Up(), desc.RightUpperArmOrientation.Back());
            SetJointAxes(r.rightLowerArm, desc.RightLowerArmOrientation.Up(), desc.RightLowerArmOrientation.Back());
            SetJointAxes(r.rightHand, desc.RightHandOrientation.Up(), desc.RightHandOrientation.Back());

            SetJointAxes(r.leftUpperLeg, desc.LeftUpperLegOrientation.Left(), desc.LeftUpperLegOrientation.Forward());
            SetJointAxes(r.leftLowerLeg, desc.LeftLowerLegOrientation.Left(), desc.LeftLowerLegOrientation.Forward());
            SetJointAxes(r.leftFoot, desc.LeftFootOrientation.Left(), desc.LeftFootOrientation.Forward());
            SetJointAxes(r.rightUpperLeg, desc.RightUpperLegOrientation.Left(), desc.RightUpperLegOrientation.Forward());
            SetJointAxes(r.rightLowerLeg, desc.RightLowerLegOrientation.Left(), desc.RightLowerLegOrientation.Forward());
            SetJointAxes(r.rightFoot, desc.RightFootOrientation.Left(), desc.RightFootOrientation.Forward());
        }

        private static void MassDistribution(BipedRagdollReferences r, BipedRagdollCreator.Options o)
        {
            // let PuppetMaster do it!
            BipedRagdollCreator__MassDistribution(r, o);
        }

        private static Transform CreateRagdollPart(string name, Transform parent, Transform pos)
        {
            var go = new GameObject(name);
            go.layer = RAGDOLL_LAYER;

            var xform = go.transform;
            xform.SetParent(parent);
            xform.SetPositionAndRotation(pos.transform.position, pos.transform.rotation);
            xform.localScale = pos.lossyScale;

            var r = go.AddComponent<Rigidbody>();
            r.maxDepenetrationVelocity = MAX_DEPENETRATION_VELOCITY;
            return xform;
        }

        private static void LinkBodyPart(Transform ragdollPart, BodyPartVolumetric bodyPart)
        {
            // create and position collider object
            var colliderGO = new GameObject(ragdollPart.name + " (Collider)");
            colliderGO.layer = RAGDOLL_LAYER;

            var colliderXform = colliderGO.transform;
            colliderXform.SetParent(ragdollPart);
            colliderXform.SetPositionAndRotation(bodyPart.transform.position, bodyPart.transform.rotation);
            var scale = ragdollPart.lossyScale;
            colliderXform.localScale = new(1.0f / scale.x, 1.0f / scale.y, 1.0f / scale.z); // invert any scale from the rig

            bodyPart.CreateCollider(colliderGO);
        }

        private static Muscle CreateMuscle(Transform ragdollPart, Transform target, Muscle.Group group)
        {
            Muscle m = new Muscle();
            m.target = target;
            m.joint = ragdollPart.GetComponent<ConfigurableJoint>();
            m.name = ragdollPart.name;
            m.props = new Muscle.Props(1f, 1f, 1f, 1f, group);
            return m;
        }

        public static PuppetMaster SetupPuppetMaster(BodyDescription desc, float ragdollWeight)
        {
            // get animator
            var anim = desc.GetComponent<Animator>();

            // create ragdoll object
            var target = desc.transform;
            var ragdollGO = new GameObject(desc.name + " (Puppet)");
            var ragdoll = ragdollGO.transform;
            ragdoll.SetParent(target.parent);
            ragdoll.SetPositionAndRotation(target.position, target.rotation);

            // options
            BipedRagdollCreator.Options o = BipedRagdollCreator.Options.Default;
            o.handColliders = RagdollCreator.ColliderType.Box;
            o.chest = false;
            o.spine = true;
            o.colliderLengthOverlap = 0;
            o.weight = ragdollWeight;

            // setup body part objects and rigidbodies
            BipedRagdollReferences r;
            r.root = ragdoll;
            r.hips = CreateRagdollPart(desc.LowerTorso.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.Hips));
            r.spine = CreateRagdollPart(desc.UpperTorso.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.Spine));
            r.chest = null;
            r.head = CreateRagdollPart(desc.Head.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.Head));
            r.leftUpperArm = CreateRagdollPart(desc.LeftUpperArm.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftUpperArm));
            r.leftLowerArm = CreateRagdollPart(desc.LeftLowerArm.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftLowerArm));
            r.leftHand = CreateRagdollPart(desc.LeftHand.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftHand));
            r.rightUpperArm = CreateRagdollPart(desc.RightUpperArm.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightUpperArm));
            r.rightLowerArm = CreateRagdollPart(desc.RightLowerArm.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightLowerArm));
            r.rightHand = CreateRagdollPart(desc.RightHand.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightHand));
            r.leftUpperLeg = CreateRagdollPart(desc.LeftUpperLeg.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftUpperLeg));
            r.leftLowerLeg = CreateRagdollPart(desc.LeftLowerLeg.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftLowerLeg));
            r.leftFoot = CreateRagdollPart(desc.LeftFoot.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.LeftFoot));
            r.rightUpperLeg = CreateRagdollPart(desc.RightUpperLeg.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightUpperLeg));
            r.rightLowerLeg = CreateRagdollPart(desc.RightLowerLeg.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightLowerLeg));
            r.rightFoot = CreateRagdollPart(desc.RightFoot.name, ragdoll, anim.GetBoneTransform(HumanBodyBones.RightFoot));

            // do mass distribution
            MassDistribution(r, o);

            // setup joints
            CreateJoints(desc, r, o);

            // create colliders
            // - this rotates the body part objects, so joints needed to be created first
            LinkBodyPart(r.hips, desc.LowerTorso);
            LinkBodyPart(r.spine, desc.UpperTorso);
            LinkBodyPart(r.head, desc.Head);
            LinkBodyPart(r.leftUpperArm, desc.LeftUpperArm);
            LinkBodyPart(r.leftLowerArm, desc.LeftLowerArm);
            LinkBodyPart(r.leftHand, desc.LeftHand);
            LinkBodyPart(r.rightUpperArm, desc.RightUpperArm);
            LinkBodyPart(r.rightLowerArm, desc.RightLowerArm);
            LinkBodyPart(r.rightHand, desc.RightHand);
            LinkBodyPart(r.leftUpperLeg, desc.LeftUpperLeg);
            LinkBodyPart(r.leftLowerLeg, desc.LeftLowerLeg);
            LinkBodyPart(r.leftFoot, desc.LeftFoot);
            LinkBodyPart(r.rightUpperLeg, desc.RightUpperLeg);
            LinkBodyPart(r.rightLowerLeg, desc.RightLowerLeg);
            LinkBodyPart(r.rightFoot, desc.RightFoot);

            // setup puppet master
            var pm = ragdollGO.AddComponent<PuppetMaster>();
            pm.solverIterationCount = Physics.defaultSolverIterations;
            pm.targetRoot = target;
            pm.muscles = new Muscle[] {
                CreateMuscle(r.hips, anim.GetBoneTransform(HumanBodyBones.Hips), Muscle.Group.Hips),
                CreateMuscle(r.spine, anim.GetBoneTransform(HumanBodyBones.Spine), Muscle.Group.Spine),
                CreateMuscle(r.head, anim.GetBoneTransform(HumanBodyBones.Head), Muscle.Group.Head),
                CreateMuscle(r.leftUpperArm, anim.GetBoneTransform(HumanBodyBones.LeftUpperArm), Muscle.Group.Arm),
                CreateMuscle(r.leftLowerArm, anim.GetBoneTransform(HumanBodyBones.LeftLowerArm), Muscle.Group.Arm),
                CreateMuscle(r.leftHand, anim.GetBoneTransform(HumanBodyBones.LeftHand), Muscle.Group.Hand),
                CreateMuscle(r.rightUpperArm, anim.GetBoneTransform(HumanBodyBones.RightUpperArm), Muscle.Group.Arm),
                CreateMuscle(r.rightLowerArm, anim.GetBoneTransform(HumanBodyBones.RightLowerArm), Muscle.Group.Arm),
                CreateMuscle(r.rightHand, anim.GetBoneTransform(HumanBodyBones.RightHand), Muscle.Group.Hand),
                CreateMuscle(r.leftUpperLeg, anim.GetBoneTransform(HumanBodyBones.LeftUpperLeg), Muscle.Group.Leg),
                CreateMuscle(r.leftLowerLeg, anim.GetBoneTransform(HumanBodyBones.LeftLowerLeg), Muscle.Group.Leg),
                CreateMuscle(r.leftFoot, anim.GetBoneTransform(HumanBodyBones.LeftFoot), Muscle.Group.Foot),
                CreateMuscle(r.rightUpperLeg, anim.GetBoneTransform(HumanBodyBones.RightUpperLeg), Muscle.Group.Leg),
                CreateMuscle(r.rightLowerLeg, anim.GetBoneTransform(HumanBodyBones.RightLowerLeg), Muscle.Group.Leg),
                CreateMuscle(r.rightFoot, anim.GetBoneTransform(HumanBodyBones.RightFoot), Muscle.Group.Foot),
            };
            return pm;
        }
    }
}
