using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using LibMMD.Model;
using LibMMD.Unity3D.Animation;
using LibMMD.Unity3D.Physics;
using LibMMD.Util;
using UnityEngine;

namespace LibMMD.Unity3D
{
    internal sealed class MmdGameObjectPhysicsBuilder
    {
        private readonly MmdGameObject _owner;
        private readonly MmdModel _model;
        private readonly GameObject[] _bones;
        private readonly GameObject _boneRootGameObject;
        private readonly List<List<int>> _partIndexes;

        internal MmdGameObjectPhysicsBuilder(
            MmdGameObject owner,
            MmdModel model,
            GameObject[] bones,
            GameObject boneRootGameObject,
            List<List<int>> partIndexes)
        {
            _owner = owner ?? throw new ArgumentNullException(nameof(owner));
            _model = model ?? throw new ArgumentNullException(nameof(model));
            _bones = bones ?? throw new ArgumentNullException(nameof(bones));
            _boneRootGameObject = boneRootGameObject ?? throw new ArgumentNullException(nameof(boneRootGameObject));
            _partIndexes = partIndexes;
        }

        internal bool[] BuildUnityPhysicsComponents()
        {
            var rigids = CreateRigids();
            AssignRigidbodyToBone(_bones, rigids);
            SetRigidsSettings(_bones, rigids);
            var joints = SetupConfigurableJoint(rigids);
            GlobalizeRigidbody(joints);
            // Must run after GlobalizeRigidbody - it needs the final hierarchy to tell which
            // bones ended up detached from _boneRootGameObject.
            var bonePhysicsControlFlags = BuildBoneKinematicFlags();

            var ignoreGroups = SettingIgnoreRigidGroups();
            var groupTarget = GetRigidbodyGroupTargets();

            IgnoreCollisions(rigids, groupTarget, ignoreGroups);
            return bonePhysicsControlFlags;
        }

        internal IEnumerator BuildUnityPhysicsComponentsIncremental(
            MmdPhysicsBuildContext context, Action<bool[]> completed)
        {
            var rigids = new GameObject[_model.Rigidbodies.Length];
            for (var i = 0; i < rigids.Length; i++)
            {
                rigids[i] = ConvertRigidbody(_model.Rigidbodies[i]);
                var collider = rigids[i].GetComponent<Collider>();
                collider.material = CreatePhysicMaterial(_model.Rigidbodies, (uint)i);
                if (_model.Rigidbodies[i].CollisionMask == 0) collider.enabled = false;
                if (context.ShouldYield()) yield return null;
            }

            var physicsRootTransform = new GameObject("Physics").transform;
            physicsRootTransform.parent = _owner.transform;
            for (var i = 0; i < rigids.Length; i++)
            {
                var relBoneIndex = GetRelBoneIndexFromNearbyRigidbody(i);
                rigids[i].transform.parent = relBoneIndex < _bones.Length
                    ? _bones[relBoneIndex].transform : physicsRootTransform;
                if (context.ShouldYield()) yield return null;
            }

            var boneCount = _model.Bones.Length;
            // Physics continues stepping while this coroutine yields. Keep every body frozen
            // until the complete joint graph and collision filters are ready, otherwise loose
            // bodies fall for several frames before their constraints are attached.
            var intendedKinematicStates = new Dictionary<Rigidbody, bool>();
            for (var i = 0; i < _model.Rigidbodies.Length; i++)
            {
                var body = _model.Rigidbodies[i];
                var target = body.AssociatedBoneIndex < boneCount
                    ? _bones[body.AssociatedBoneIndex] : rigids[i];
                var existingBody = target.GetComponent<Rigidbody>();
                var intendedState = false;
                var hasIntendedState = existingBody != null &&
                    intendedKinematicStates.TryGetValue(existingBody, out intendedState);
                UnityRigidbodySetting(body, target);
                var rigidbody = target.GetComponent<Rigidbody>();
                if (!hasIntendedState) intendedState = rigidbody.isKinematic;
                intendedKinematicStates[rigidbody] = intendedState;
                rigidbody.isKinematic = true;
                if (context.ShouldYield()) yield return null;
            }

            var joints = new List<GameObject>();
            foreach (var joint in _model.Constraints)
            {
                var jointObject = CreateIncrementalConfigurableJoint(rigids, joint);
                if (jointObject != null) joints.Add(jointObject);
                if (context.ShouldYield()) yield return null;
            }

            foreach (var jointObject in joints)
            {
                var body = jointObject.GetComponent<Rigidbody>();
                if (body != null && intendedKinematicStates.TryGetValue(body, out var isKinematic) &&
                    !isKinematic)
                    jointObject.transform.parent = physicsRootTransform;
                if (context.ShouldYield()) yield return null;
            }

            var ignoreGroups = SettingIgnoreRigidGroups();
            var groupTargets = GetRigidbodyGroupTargets();
            for (var i = 0; i < rigids.Length; i++)
            {
                for (var shift = 0; shift < 16; shift++)
                {
                    if ((groupTargets[i] & (1 << shift)) != 0) continue;
                    foreach (var ignoreIndex in ignoreGroups[shift])
                    {
                        if (i == ignoreIndex) continue;
                        UnityEngine.Physics.IgnoreCollision(rigids[i].GetComponent<Collider>(),
                            rigids[ignoreIndex].GetComponent<Collider>(), true);
                    }
                }
                if (context.ShouldYield()) yield return null;
            }

            // Commit the entire graph atomically so the first simulated frame sees the same
            // completed setup as the former synchronous loader.
            UnityEngine.Physics.SyncTransforms();
            foreach (var pair in intendedKinematicStates)
            {
                var body = pair.Key;
                if (body == null) continue;
                body.isKinematic = pair.Value;
                if (pair.Value) continue;
                body.linearVelocity = Vector3.zero;
                body.angularVelocity = Vector3.zero;
                body.WakeUp();
            }
            UnityEngine.Physics.SyncTransforms();
            var flags = BuildBoneKinematicFlags();
            completed(flags);
        }

        private GameObject[] CreateRigids()
        {
            var result = _model.Rigidbodies.Select(ConvertRigidbody).ToArray();
            for (uint i = 0, iMax = (uint) result.Length; i < iMax; ++i)
            {
                var collider = result[i].GetComponent<Collider>();
                collider.material = CreatePhysicMaterial(_model.Rigidbodies, i);
                // A zero MMD collision mask ignores every group. Keeping such a collider in
                // PhysX broadphase adds pair generation work without ever producing contact.
                if (_model.Rigidbodies[i].CollisionMask == 0) collider.enabled = false;
            }
            return result;
        }

        private static GameObject ConvertRigidbody(MmdRigidBody mmdRigidBody)
        {
            var ret = new GameObject("r" + mmdRigidBody.Name);

            ret.transform.position = mmdRigidBody.Position;
            ret.transform.rotation = Quaternion.Euler(mmdRigidBody.Rotation * Mathf.Rad2Deg);

            switch (mmdRigidBody.Shape)
            {
                case MmdRigidBody.RigidBodyShape.RigidShapeSphere:
                    EntrySphereCollider(mmdRigidBody, ret);
                    break;
                case MmdRigidBody.RigidBodyShape.RigidShapeBox:
                    EntryBoxCollider(mmdRigidBody, ret);
                    break;
                case MmdRigidBody.RigidBodyShape.RigidShapeCapsule:
                    EntryCapsuleCollider(mmdRigidBody, ret);
                    break;
                default:
                    throw new ArgumentOutOfRangeException();
            }

            return ret;
        }

        private static void EntrySphereCollider(MmdRigidBody mmdRigidBody, GameObject obj)
        {
            var unityCollider = obj.AddComponent<SphereCollider>();
            unityCollider.radius = mmdRigidBody.Dimemsions.x;
        }

        private static void EntryBoxCollider(MmdRigidBody mmdRigidBody, GameObject obj)
        {
            var unityCollider = obj.AddComponent<BoxCollider>();
            unityCollider.size = mmdRigidBody.Dimemsions * 2.0f;
        }

        private static void EntryCapsuleCollider(MmdRigidBody mmdRigidBody, GameObject obj)
        {
            var unityCollider = obj.AddComponent<CapsuleCollider>();
            unityCollider.radius = mmdRigidBody.Dimemsions.x;
            unityCollider.height = mmdRigidBody.Dimemsions.y + mmdRigidBody.Dimemsions.x * 2.0f;
        }

        private PhysicsMaterial CreatePhysicMaterial(MmdRigidBody[] rigidbodys, uint index)
        {
            var mmdRigidBody = rigidbodys[index];

            return new PhysicsMaterial(_model.Name + "_r" + mmdRigidBody.Name)
            {
                bounciness = mmdRigidBody.Restitution,
                staticFriction = mmdRigidBody.Friction,
                dynamicFriction = mmdRigidBody.Friction
            };
        }

        private void AssignRigidbodyToBone(GameObject[] bones, GameObject[] rigids)
        {
            var physicsRootTransform = new GameObject("Physics").transform;
            physicsRootTransform.parent = _owner.transform;

            for (int i = 0, iMax = rigids.Length; i < iMax; ++i)
            {
                var relBoneIndex = GetRelBoneIndexFromNearbyRigidbody(i);
                rigids[i].transform.parent =
                    relBoneIndex < bones.Length ? bones[relBoneIndex].transform : physicsRootTransform;
            }
        }


        private int GetRelBoneIndexFromNearbyRigidbody(int rigidbodyIndex)
        {
            var boneCount = _model.Bones.Length;
            var result = _model.Rigidbodies[rigidbodyIndex].AssociatedBoneIndex;
            if (result < boneCount)
            {
                return result;
            }
            var jointAList = _model.Constraints.Where(x => x.AssociatedRigidBodyIndex[1] == rigidbodyIndex)
                .Where(x => x.AssociatedRigidBodyIndex[0] < boneCount)
                .Select(x => x.AssociatedRigidBodyIndex[0]);
            foreach (var jointA in jointAList)
            {
                result = GetRelBoneIndexFromNearbyRigidbody(jointA);
                if (result < boneCount)
                {
                    return result;
                }
            }

            var jointBList = _model.Constraints.Where(x => x.AssociatedRigidBodyIndex[0] == rigidbodyIndex)
                .Where(x => x.AssociatedRigidBodyIndex[1] < boneCount)
                .Select(x => x.AssociatedRigidBodyIndex[1]);
            foreach (var jointB in jointBList)
            {
                result = GetRelBoneIndexFromNearbyRigidbody(jointB);
                if (result < boneCount)
                {
                    return result;
                }
            }

            result = int.MaxValue;
            return result;
        }

        private void SetRigidsSettings(GameObject[] bones, GameObject[] rigid)
        {
            var boneCount = _model.Bones.Length;
            for (int i = 0, iMax = _model.Rigidbodies.Length; i < iMax; ++i)
            {
                var mmdRigidBody = _model.Rigidbodies[i];
                var target = mmdRigidBody.AssociatedBoneIndex < boneCount
                    ? bones[mmdRigidBody.AssociatedBoneIndex]
                    : rigid[i];
                UnityRigidbodySetting(mmdRigidBody, target);
            }
        }

        // True for bones the shared Animator/PlayableGraph pose driver (MmdAnimationDriver)
        // must never write to via TransformStreamHandle - that handle only resolves for
        // transforms still parented under the Animator's own GameObject
        // (_boneRootGameObject), and GlobalizeRigidbody detaches non-kinematic
        // Rigidbody/ConfigurableJoint bones (and, transitively, any kinematic bone still
        // parented under one of them, e.g. a plain tip bone at the end of a physics chain)
        // out of that hierarchy so PhysX can move them freely.
        private bool[] BuildBoneKinematicFlags()
        {
            var flags = new bool[_bones.Length];
            for (int i = 0, iMax = _bones.Length; i < iMax; i++)
            {
                var bone = _bones[i];
                if (!bone.transform.IsChildOf(_boneRootGameObject.transform))
                {
                    flags[i] = true;
                    continue;
                }
                var boneRigidbody = bone.GetComponent<Rigidbody>();
                if (boneRigidbody == null)
                {
                    continue;
                }
                if (!boneRigidbody.isKinematic)
                {
                    flags[i] = true;
                }
            }
            return flags;
        }

        private static void UnityRigidbodySetting(MmdRigidBody mmdRigidBody, GameObject target)
        {
            var unityRigidBody = target.GetComponent<Rigidbody>();
            if (null != unityRigidBody)
            {
                unityRigidBody.mass = Mathf.Clamp(unityRigidBody.mass + mmdRigidBody.Mass,
                    0.01f, 100f);
                unityRigidBody.linearDamping = (unityRigidBody.linearDamping + mmdRigidBody.TranslateDamp) * 0.5f;
                unityRigidBody.angularDamping = (unityRigidBody.angularDamping + mmdRigidBody.RotateDamp) * 0.5f;
            }
            else
            {
                unityRigidBody = target.AddComponent<Rigidbody>();
                unityRigidBody.isKinematic = MmdRigidBody.RigidBodyType.RigidTypeKinematic == mmdRigidBody.Type;
                unityRigidBody.mass = Mathf.Clamp(mmdRigidBody.Mass, 0.01f, 100f);
                unityRigidBody.linearDamping = mmdRigidBody.TranslateDamp;
                unityRigidBody.angularDamping = mmdRigidBody.RotateDamp;
            }

            // Physics chains need more than PhysX's low per-body defaults. These values are
            // deliberately local to imported MMD bodies and do not alter project-wide physics.
            unityRigidBody.solverIterations = Mathf.Max(unityRigidBody.solverIterations, 6);
            unityRigidBody.solverVelocityIterations = Mathf.Max(unityRigidBody.solverVelocityIterations, 1);
            unityRigidBody.maxLinearVelocity = Mathf.Min(unityRigidBody.maxLinearVelocity, 20f);
            unityRigidBody.maxAngularVelocity = Mathf.Min(unityRigidBody.maxAngularVelocity, 20.0f);
            unityRigidBody.maxDepenetrationVelocity = Mathf.Min(
                unityRigidBody.maxDepenetrationVelocity, 10f);
            unityRigidBody.collisionDetectionMode = CollisionDetectionMode.Discrete;
            unityRigidBody.interpolation = unityRigidBody.isKinematic
                ? RigidbodyInterpolation.None : RigidbodyInterpolation.Interpolate;
            if (!unityRigidBody.isKinematic)
                unityRigidBody.sleepThreshold = Mathf.Max(unityRigidBody.sleepThreshold, 0.02f);
        }

        private GameObject[] SetupConfigurableJoint(GameObject[] rigids)
        {
            var resultList = new List<GameObject>();
            foreach (var joint in _model.Constraints)
            {
                var transformA = rigids[joint.AssociatedRigidBodyIndex[0]].transform;
                var rigidbodyA = transformA.GetComponent<Rigidbody>();
                if (null == rigidbodyA)
                {
                    rigidbodyA = transformA.parent.GetComponent<Rigidbody>();
                }
                var transformB = rigids[joint.AssociatedRigidBodyIndex[1]].transform;
                var rigidbodyB = transformB.GetComponent<Rigidbody>();
                if (null == rigidbodyB)
                {
                    rigidbodyB = transformB.parent.GetComponent<Rigidbody>();
                }
                if (rigidbodyA == rigidbodyB) continue;
                var configJoint = rigidbodyB.gameObject.AddComponent<ConfigurableJoint>();
                configJoint.connectedBody = rigidbodyA;
                configJoint.enableCollision = false;
                configJoint.projectionMode = JointProjectionMode.PositionAndRotation;
                configJoint.projectionDistance = 0.01f;
                configJoint.projectionAngle = 5.0f;
                configJoint.enablePreprocessing = false;
                configJoint.massScale = 1f;
                configJoint.connectedMassScale = 1f;
                SetAttributeConfigurableJoint(joint, configJoint);

                resultList.Add(configJoint.gameObject);
            }
            return resultList.ToArray();
        }

        // Used only by the experimental incremental path; the production Unity backend keeps
        // using SetupConfigurableJoint above so its behavior matches ca7aac6 exactly.
        private GameObject CreateIncrementalConfigurableJoint(GameObject[] rigids, Constraint joint)
        {
            var transformA = rigids[joint.AssociatedRigidBodyIndex[0]].transform;
            var rigidbodyA = transformA.GetComponent<Rigidbody>() ?? transformA.parent.GetComponent<Rigidbody>();
            var transformB = rigids[joint.AssociatedRigidBodyIndex[1]].transform;
            var rigidbodyB = transformB.GetComponent<Rigidbody>() ?? transformB.parent.GetComponent<Rigidbody>();
            if (rigidbodyA == rigidbodyB) return null;
            var configJoint = rigidbodyB.gameObject.AddComponent<ConfigurableJoint>();
            configJoint.connectedBody = rigidbodyA;
            configJoint.enableCollision = false;
            configJoint.projectionMode = JointProjectionMode.PositionAndRotation;
            configJoint.projectionDistance = 0.01f;
            configJoint.projectionAngle = 5.0f;
            configJoint.enablePreprocessing = false;
            configJoint.massScale = 1f;
            configJoint.connectedMassScale = 1f;
            SetAttributeConfigurableJoint(joint, configJoint);
            return configJoint.gameObject;
        }

        private void SetAttributeConfigurableJoint(Constraint joint, ConfigurableJoint conf)
        {
            SetMotionAngularLock(joint, conf);
            SetDrive(joint, conf);
        }

        private static void SetMotionAngularLock(Constraint joint, ConfigurableJoint conf)
        {
            SoftJointLimit jlim;

            if (Math.Abs(joint.PositionLowLimit.x) < Tools.MmdMathConstEps &&
                Math.Abs(joint.PositionHiLimit.x) < Tools.MmdMathConstEps)
            {
                conf.xMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.xMotion = ConfigurableJointMotion.Limited;
            }

            if (Math.Abs(joint.PositionLowLimit.y) < Tools.MmdMathConstEps &&
                Math.Abs(joint.PositionHiLimit.y) < Tools.MmdMathConstEps)
            {
                conf.yMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.yMotion = ConfigurableJointMotion.Limited;
            }

            if (Math.Abs(joint.PositionLowLimit.z) < Tools.MmdMathConstEps &&
                Math.Abs(joint.PositionHiLimit.z) < Tools.MmdMathConstEps)
            {
                conf.zMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.zMotion = ConfigurableJointMotion.Limited;
            }

            if (Math.Abs(joint.RotationLowLimit.x) < Tools.MmdMathConstEps &&
                Math.Abs(joint.RotationHiLimit.x) < Tools.MmdMathConstEps)
            {
                conf.angularXMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.angularXMotion = ConfigurableJointMotion.Limited;
                var hlim = Mathf.Max(-joint.RotationLowLimit.x, -joint.RotationHiLimit.x);
                var llim = Mathf.Min(-joint.RotationLowLimit.x, -joint.RotationHiLimit.x);
                var jhlim = new SoftJointLimit {limit = Mathf.Clamp(hlim * Mathf.Rad2Deg, -180.0f, 180.0f)};
                conf.highAngularXLimit = jhlim;

                var jllim = new SoftJointLimit {limit = Mathf.Clamp(llim * Mathf.Rad2Deg, -180.0f, 180.0f)};
                conf.lowAngularXLimit = jllim;
            }

            if (Math.Abs(joint.RotationLowLimit.y) < Tools.MmdMathConstEps &&
                Math.Abs(joint.RotationHiLimit.y) < Tools.MmdMathConstEps)
            {
                conf.angularYMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.angularYMotion = ConfigurableJointMotion.Limited;
                conf.angularYMotion = ConfigurableJointMotion.Limited;
                var lim = Mathf.Min(Mathf.Abs(joint.RotationLowLimit.y), Mathf.Abs(joint.RotationHiLimit.y));
                jlim = new SoftJointLimit {limit = lim * Mathf.Clamp(Mathf.Rad2Deg, 0.0f, 180.0f)};
                conf.angularYLimit = jlim;
            }

            if (Math.Abs(joint.RotationLowLimit.z) < Tools.MmdMathConstEps &&
                Math.Abs(joint.RotationHiLimit.z) < Tools.MmdMathConstEps)
            {
                conf.angularZMotion = ConfigurableJointMotion.Locked;
            }
            else
            {
                conf.angularZMotion = ConfigurableJointMotion.Limited;
                var lim = Mathf.Min(Mathf.Abs(-joint.RotationLowLimit.z), Mathf.Abs(-joint.RotationHiLimit.z));
                jlim = new SoftJointLimit {limit = Mathf.Clamp(lim * Mathf.Rad2Deg, 0.0f, 180.0f)};
                conf.angularZLimit = jlim;
            }
        }

        private void SetDrive(Constraint joint, ConfigurableJoint conf)
        {
            // Position/Rotation are the joint's world-space anchor, not spring constants.
            // Feeding them into positionSpring allowed negative and location-dependent drive
            // strengths, which can inject energy and make a chain explode.
            conf.xDrive = CreateDrive(joint.SpringTranslate.x);
            conf.yDrive = CreateDrive(joint.SpringTranslate.y);
            conf.zDrive = CreateDrive(joint.SpringTranslate.z);
            conf.angularXDrive = CreateDrive(joint.SpringRotate.x);
            conf.angularYZDrive = CreateDrive((joint.SpringRotate.y + joint.SpringRotate.z) * 0.5f);
        }

        private static JointDrive CreateDrive(float spring)
        {
            spring = Mathf.Clamp(spring, 0f, 1000f);
            if (spring <= Tools.MmdMathConstEps) return default;

            return new JointDrive
            {
                positionSpring = spring,
                // A small amount of damping prevents a stiff MMD spring from continually
                // overshooting without making hair and clothing look unnaturally heavy.
                positionDamper = 2.0f * Mathf.Sqrt(spring),
                maximumForce = 10000f
            };
        }

        private void GlobalizeRigidbody(GameObject[] joints)
        {
            var physicsRootTransform = _owner.transform.Find("Physics");

            if (null == joints || 0 >= joints.Length) return;

            foreach (ConfigurableJoint joint in joints.Where(x => !x.GetComponent<Rigidbody>().isKinematic)
                .Select(x => x.GetComponent<ConfigurableJoint>()))
            {
                joint.transform.parent = physicsRootTransform;
            }
        }

        private List<int>[] SettingIgnoreRigidGroups()
        {
            const int maxGroup = 16;
            var result = new List<int>[maxGroup];
            for (int i = 0, iMax = maxGroup; i < iMax; ++i)
            {
                result[i] = new List<int>();
            }
            for (int i = 0, iMax = _model.Rigidbodies.Length; i < iMax; ++i)
            {
                result[_model.Rigidbodies[i].CollisionGroup].Add(i);
            }
            return result;
        }

        private int[] GetRigidbodyGroupTargets()
        {
            return _model.Rigidbodies.Select(x => (int) x.CollisionMask).ToArray();
        }

        private static void IgnoreCollisions(IList<GameObject> rigids, IList<int> groupTarget, List<int>[] ignoreList)
        {
            for (var i = 0; i < rigids.Count; i++)
            {
                for (var shift = 0; shift < 16; shift++)
                {
                    if ((groupTarget[i] & (1 << shift)) != 0) continue;
                    for (var j = 0; j < ignoreList[shift].Count; j++)
                    {
                        var ignoreIndex = ignoreList[shift][j];
                        if (i == ignoreIndex) continue;
                        UnityEngine.Physics.IgnoreCollision(rigids[i].GetComponent<Collider>(),
                            rigids[ignoreIndex].GetComponent<Collider>(), true);
                    }
                }
            }
        }
    }
}