using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Xml.Linq;
using UnityEngine;

namespace Unity.MLAgents
{
    public class MarathonSpawner : MonoBehaviour
    {
        [Tooltip("The MuJoCo xml file to parse")]
        /**< \brief The MuJoCo xml file to parse*/
        public TextAsset Xml;

        public Material Material;
        public PhysicMaterial PhysicMaterial;

        [Tooltip("When True, UnityEngine.Time.fixedDeltaTime is set by <option timestep=xxx>")]
        /**< \brief When True, UnityEngine.Time.fixedDeltaTime is set by <option timestep=xxx>*/
        public bool UseXmlTimestep = true;

        [Tooltip("During XML parsing, debug messages are sent to the console")]
        /**< \brief During XML parsing, debug messages are sent to the console*/
        public bool DebugOutput;

        [Tooltip("Used for 2D MuJoCo objects (hopper, walker, etc)")]
        /**< \brief Used for 2D MuJoCo objects (hopper, walker, etc)*/
        public bool Force2D;

        [Tooltip("The random noise applied at the start of each episode to improve training variance")]
        /**< \brief The random noise applied at the start of each episode to improve training variance"*/
        public float OnGenerateApplyRandom = 0.005f;

        [Tooltip("The default density (Mujoco's default value is 1000)")]
        /**< \brief The default density (Mujoco's default value is 1000)"*/
        public float DefaultDensity = 1000f;

        [Tooltip("Use to scale the power of the motors (default is 1)")]
        /**< \brief Use to scale the power of the motors (default is 1)"*/
        public float MotorScale = 1f;


        XElement _root;
        Stack<XElement> _childClassStack;
        Dictionary<string, XElement> _jointXDocs;

        bool _hasParsed;
        bool _useWorldSpace = false;
        Quaternion _orginalTransformRotation;
        Vector3 _orginalTransformPosition;

        public void SpawnFromXml()
        {
            LoadXml(Xml.text);
            Parse();
        }

        void LoadXml(string str)
        {
            _root = XElement.Parse(str);
        }

        void DebugPrint(string str)
        {
            if (DebugOutput)
                print(str);
        }

        void Parse()
        {
            XElement element = _root;
            var name = element.Name.LocalName;
            DebugPrint($"- Begin");

            _jointXDocs = new Dictionary<string, XElement>();
            ParseCompilerOptions(_root);

            _childClassStack = new Stack<XElement>();

            foreach (var attribute in element.Attributes())
            {
                switch (attribute.Name.LocalName)
                {
                    case "model":
                        gameObject.name = attribute.Value;
                        break;
                    default:
                        throw new NotImplementedException();
                }
            }

            // when using world space, geoms will be created in global space
            // so setting the parent object to 0,0,0 allows us to fix that 
            _orginalTransformRotation = this.gameObject.transform.rotation;
            _orginalTransformPosition = this.gameObject.transform.position;
            this.gameObject.transform.rotation = new Quaternion();
            this.gameObject.transform.position = new Vector3();

            var joints = ParseBody(element.Element("worldbody"), this.gameObject);
            var mujocoJoints = ParseGears(element.Element("actuator"), joints);
            var mujocoSensors = ParseSensors(element.Element("sensor"), GetComponentsInChildren<Collider>());

            if (Material != null)
                foreach (var item in GetComponentsInChildren<Renderer>())
                {
                    item.material = Material;
                }

            if (PhysicMaterial != null)
                foreach (var item in GetComponentsInChildren<Collider>())
                {
                    item.material = PhysicMaterial;
                }

            if (Force2D)
            {
                foreach (var item in GetComponentsInChildren<Rigidbody>())
                    item.constraints = RigidbodyConstraints.FreezePositionZ;
            }

            if (this.gameObject.layer != 0)
            {
                foreach (var item in GetComponentsInChildren<Collider>())
                    item.gameObject.layer = this.gameObject.layer;
            }

            // restore positions and orientation
            gameObject.transform.rotation = _orginalTransformRotation;
            gameObject.transform.position = _orginalTransformPosition;

            GetComponent<MarathonAgent>().SetMarathonJoints(mujocoJoints);
            GetComponent<MarathonAgent>().SetMarathonSensors(mujocoSensors);
        }

        public void ApplyRandom()
        {
            if (OnGenerateApplyRandom != 0f)
            {
                float velocityScaler = 5000f;
                foreach (var item in GetComponent<MarathonAgent>().MarathonJoints)
                {
                    var r = ((UnityEngine.Random.value * (OnGenerateApplyRandom * 2)) - OnGenerateApplyRandom);
                    // float r = 0f;
                    var childRb = item.Joint.GetComponent<Rigidbody>();
                    if (childRb != null)
                    {
                        ConfigurableJoint configurableJoint = item.Joint as ConfigurableJoint;
                        var t = Vector3.zero;
                        t.x = r * velocityScaler;
                        configurableJoint.targetAngularVelocity = t;
                        childRb.angularVelocity = t;
                        t = Vector3.zero;
                        t.x = ((UnityEngine.Random.value * (OnGenerateApplyRandom * 2)) - OnGenerateApplyRandom) * 5;
                        t.y = ((UnityEngine.Random.value * (OnGenerateApplyRandom * 2)) - OnGenerateApplyRandom) * 5 +
                              1;
                        t.z = ((UnityEngine.Random.value * (OnGenerateApplyRandom * 2)) - OnGenerateApplyRandom) * 5;
                        childRb.velocity = t;
                        var angX = configurableJoint.angularXDrive;
                        angX.positionSpring = 1f;
                        var scale = item.MaximumForce * Mathf.Pow(Mathf.Abs(r), 3);
                        angX.positionDamper = Mathf.Max(1f, scale);
                        angX.maximumForce = Mathf.Max(1f, scale);
                        configurableJoint.angularXDrive = angX;
                    }
                }
            }
        }

        void ParseCompilerOptions(XElement xdoc)
        {
            foreach (var element in xdoc.Elements("option"))
            {
                foreach (var attribute in element.Attributes())
                {
                    switch (attribute.Name.LocalName)
                    {
                        case "integrator":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        case "iterations":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        case "solver":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        case "timestep":
                            if (UseXmlTimestep)
                            {
                                var timestep = (float)Convert.ToDouble(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                                Time.fixedDeltaTime = timestep;
                            }
                            else
                                DebugPrint($"--*** IGNORING timestep=\"{attribute.Value}\" as UseXmlTimestep == false");

                            break;
                        case "gravity":
                            Physics.gravity = MarathonHelper.ParsePosition(attribute.Value);
                            break;
                        default:
                            DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                            throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                            break;
                    }
                }
            }

            foreach (var element in xdoc.Elements("compiler"))
            {
                foreach (var attribute in element.Attributes())
                {
                    switch (attribute.Name.LocalName)
                    {
                        case "angle":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        case "coordinate":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            if (attribute.Value.ToLower() == "global")
                                _useWorldSpace = true;
                            else if (attribute.Value.ToLower() == "local")
                                _useWorldSpace = false;
                            break;
                        case "inertiafromgeom":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        case "settotalmass":
                            DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                            break;
                        default:
                            DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                            throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                            break;
                    }
                }
            }
        }

        private class JointDocQueueItem
        {
            public XElement JointXDoc { get; set; }
            public GeomItem ParentGeom { get; set; }
            public GameObject ParentBody { get; set; }
        }

        private class GeomItem
        {
            public GameObject Geom;
            public float? Lenght;
            public float? Size;
            public Vector3 Lenght3D;
            public Vector3 Start;
            public Vector3 End;
            public List<GameObject> Bones;

            public GeomItem()
            {
                Bones = new List<GameObject>();
            }
        }

        List<KeyValuePair<string, Joint>> ParseBody(XElement xdoc, GameObject parentBody, GeomItem geom = null,
            GeomItem parentGeom = null, List<JointDocQueueItem> jointDocsQueue = null)
        {
            var joints = new List<KeyValuePair<string, Joint>>();
            jointDocsQueue = jointDocsQueue ?? new List<JointDocQueueItem>();
            var bodies = new List<GameObject>();

            var childClass = xdoc.Attribute("childclass");
            if (childClass != null)
            {
                var childDoc = _root.Element("default")
                    ?.Elements("default")
                    .FirstOrDefault(x => x.Attribute("class")?.Value == childClass.Value);
                _childClassStack.Push(childDoc);
            }

            foreach (var element in xdoc.Elements("light"))
            {
            }

            foreach (var element in xdoc.Elements("camera"))
            {
            }

            foreach (var element in xdoc.Elements("joint"))
            {
                jointDocsQueue.Add(new JointDocQueueItem
                {
                    JointXDoc = element,
                    ParentGeom = geom,
                    ParentBody = parentBody,
                });
            }

            foreach (var element in xdoc.Elements("geom"))
            {
                geom = ParseGeom(element, parentBody);

                if (parentGeom != null && jointDocsQueue?.Count > 0)
                {
                    foreach (var jointDocQueueItem in jointDocsQueue)
                    {
                        var js = ParseJoint(
                            jointDocQueueItem.JointXDoc,
                            jointDocQueueItem.ParentGeom,
                            geom,
                            jointDocQueueItem.ParentBody);
                        if (js != null) joints.AddRange(js);
                    }
                }
                else if (parentGeom != null)
                {
                    var fixedJoint = parentGeom.Geom.AddComponent<FixedJoint>();
                    fixedJoint.connectedBody = geom.Geom.GetComponent<Rigidbody>();
                }

                jointDocsQueue.Clear();
                parentGeom = geom;
            }

            foreach (var element in xdoc.Elements("body"))
            {
                var body = new GameObject();
                bodies.Add(body);
                body.transform.parent = this.transform;
                ApplyClassToBody(element, body, parentBody);
                var newJoints = ParseBody(element, body, geom, parentGeom, jointDocsQueue);
                if (newJoints != null) joints.AddRange(newJoints);
            }

            foreach (var item in bodies)
                GameObject.Destroy(item);

            if (childClass != null)
                _childClassStack.Pop();

            return joints;
        }

        void ApplyClassToBody(XElement classElement, GameObject body, GameObject parentBody)
        {
            foreach (var attribute in classElement.Attributes())
            {
                switch (attribute.Name.LocalName)
                {
                    case "name":
                        body.name = attribute.Value;
                        break;
                    case "pos":
                        if (_useWorldSpace)
                            body.transform.position = MarathonHelper.ParsePosition(attribute.Value);
                        else
                        {
                            body.transform.position =
                                MarathonHelper.ParsePosition(attribute.Value) + parentBody.transform.position;
                        }

                        break;
                    case "quat":
                        if (_useWorldSpace)
                            body.transform.rotation = MarathonHelper.ParseQuaternion(attribute.Value);
                        else
                        {
                            body.transform.rotation = MarathonHelper.ParseQuaternion(attribute.Value) *
                                                      parentBody.transform.rotation;
                        }

                        break;
                    case "childclass":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "euler":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    default:
                        DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                        throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                        break;
                }
            }
        }

        GeomItem ParseGeom(XElement xdoc, GameObject parent)
        {
            GeomItem geom = null;

            if (xdoc == null)
                return null;
            XElement element = BuildFromClasses("geom", xdoc);

            var type = element.Attribute("type")?.Value;
            if (type == null)
            {
                DebugPrint($"--- WARNING: ParseGeom: no type found in geom. Ignoring ({element.ToString()}");
                return geom;
            }

            float size;
            float? size2 = null;
            DebugPrint($"ParseGeom: Creating type:{type} name:{element.Attribute("name")?.Value}");
            geom = new GeomItem();
            Vector3 start;
            Vector3 end;
            Vector3 offset;
            switch (type)
            {
                case "capsule":
                    if (element.Attribute("size")?.Value?.Split()?.Length > 1)
                    {
                        size = (float)Convert.ToDouble(element.Attribute("size")?.Value.Split()[0], System.Globalization.CultureInfo.InvariantCulture);
                        size2 = (float)Convert.ToDouble(element.Attribute("size")?.Value.Split()[1], System.Globalization.CultureInfo.InvariantCulture);
                    }
                    else
                        size = (float)Convert.ToDouble(element.Attribute("size")?.Value, System.Globalization.CultureInfo.InvariantCulture);

                    var fromto = element.Attribute("fromto")?.Value;
                    if (fromto == null)
                    {
                        var posAttribute = element.Attribute("pos")?.Value;
                        Vector3 centerPos = Vector3.zero;
                        if (posAttribute != null)
                        {
                            var rawPos = MarathonHelper.ParsePosition(posAttribute);
                            centerPos = centerPos - rawPos;
                        }

                        start = centerPos;
                        end = centerPos;
                        var zaxisAttribute = element.Attribute("zaxis")?.Value;
                        Vector3 zaxis = Vector3.up;
                        if (zaxisAttribute != null)
                            zaxis = MarathonHelper.ParseAxis(zaxisAttribute);
                        var zaxisScaled = zaxis * size2.Value;
                        start -= zaxisScaled;
                        end += zaxisScaled;
                        // end += zaxisScaled * 2;
                        start = MarathonHelper.RightToLeft(start);
                        end = MarathonHelper.RightToLeft(end);
                        DebugPrint($"ParseGeom: Creating type:{type} size:{size}");
                    }
                    else
                    {
                        DebugPrint($"ParseGeom: Creating type:{type} fromto:{fromto} size:{size}");
                        start = MarathonHelper.ParseFrom(fromto);
                        end = MarathonHelper.ParseTo(fromto);
                    }

                    geom.Geom = parent.CreateBetweenPoints(start, end, size, _useWorldSpace, this.gameObject);
                    offset = end - start;
                    geom.Lenght = offset.magnitude; //
                    geom.Size = size;
                    geom.Lenght3D = offset;
                    geom.Start = start;
                    geom.End = end;

                    break;
                case "sphere":
                    size = (float)Convert.ToDouble(element.Attribute("size")?.Value, System.Globalization.CultureInfo.InvariantCulture);
                    var pos = element.Attribute("pos")?.Value ?? "0 0 0";
                    DebugPrint($"ParseGeom: Creating type:{type} pos:{pos} size:{size}");
                    geom.Geom = parent.CreateAtPoint(MarathonHelper.ParsePosition(pos), size, _useWorldSpace, this.gameObject);
                    geom.Size = size;
                    break;
                default:
                    DebugPrint(
                        $"--- WARNING: ParseGeom: {type} geom is not implemented. Ignoring ({element.ToString()}");
                    return null;
            }

            var rb = geom.Geom.AddComponent<Rigidbody>();
            rb.useGravity = true;
            rb.SetDensity(DefaultDensity);
            rb.mass = rb.mass; // ref: https://forum.unity.com/threads/rigidbody-setdensity-doesnt-work.322911/

            ApplyClassToGeom(element, geom.Geom, parent);

            return geom;
        }

        void ApplyClassToGeom(XElement classElement, GameObject geom, GameObject parentBody)
        {
            foreach (var attribute in classElement.Attributes())
            {
                switch (attribute.Name.LocalName)
                {
                    case "name": // optional
                        // Name of the geom.
                        geom.name = attribute.Value;
                        break;
                    case "class": // optional
                        // Defaults class for setting unspecified attributes.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "type": // [plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh], "sphere"
                        // Type of geometric shape.
                        // Handled in object init
                        break;
                    case "contype": // int, "1"
                        // This attribute and the next specify 32-bit integer bitmasks used for contact 
                        // filtering of dynamically generated contact pairs. See Collision detection in 
                        // the Computation chapter. Two geoms can collide if the contype of one geom is 
                        // compatible with the conaffinity of the other geom or vice versa. 
                        // Compatible means that the two bitmasks have a common bit set to 1.
                        // Note: contype="0" conaffinity="0" disables physics contacts
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "conaffinity": // int, "1"
                        // Bitmask for contact filtering; see contype above.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "condim": //  int, "3"
                        // The dimensionality of the contact space for a dynamically generated contact 
                        // pair is set to the maximum of the condim values of the two participating geoms. 
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "group": // int, "0"
                        // This attribute specifies an integer group to which the geom belongs.
                        // The only effect on the physics is at compile time, when body masses and inertias are
                        // inferred from geoms selected based on their group; see inertiagrouprange attribute of compiler.
                        // At runtime this attribute is used by the visualizer to enable and disable the rendering of
                        // entire geom groups. It can also be used as a tag for custom computations.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "size": // real(3), "0 0 0"
                        // Geom size parameters. The number of required parameters and their meaning depends on the
                        // geom type as documented under the type attribute. Here we only provide a summary.
                        // All required size parameters must be positive; the internal defaults correspond to invalid
                        // settings. Note that when a non-mesh geom type references a mesh, a geometric primitive of
                        // that type is fitted to the mesh. In that case the sizes are obtained from the mesh, and
                        // the geom size parameters are ignored. Thus the number and description of required size
                        // parameters in the table below only apply to geoms that do not reference meshes. 
                        // Type	Number	Description
                        // plane	3	X half-size; Y half-size; spacing between square grid lines for rendering.
                        // hfield	0	The geom sizes are ignored and the height field sizes are used instead.
                        // sphere	1	Radius of the sphere.
                        // capsule	1 or 2	Radius of the capsule; half-length of the cylinder part when not using the fromto specification.
                        // ellipsoid	3	X radius; Y radius; Z radius.
                        // cylinder	1 or 2	Radius of the cylinder; half-length of the cylinder when not using the fromto specification.
                        // box	3	X half-size; Y half-size; Z half-size.
                        // mesh	0	The geom sizes are ignored and the mesh sizes are used instead.
                        // Handled at object init
                        break;
                    case "material": //  optional
                        // If specified, this attribute applies a material to the geom. The material determines the visual properties of
                        // the geom. The only exception is color: if the rgba attribute below is different from its internal default, it takes
                        // precedence while the remaining material properties are still applied. Note that if the same material is referenced
                        // from multiple geoms (as well as sites and tendons) and the user changes some of its properties at runtime,
                        // these changes will take effect immediately for all model elements referencing the material. This is because the
                        // compiler saves the material and its properties as a separate element in mjModel, and the elements using this
                        // material only keep a reference to it.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "rgba": // real(4), "0.5 0.5 0.5 1"
                        // Instead of creating material assets and referencing them, this attribute can be used
                        // to set color and transparency only. This is not as flexible as the material mechanism,
                        // but is more convenient and is often sufficient. If the value of this attribute is
                        // different from the internal default, it takes precedence over the material.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "friction": //real(3), "1 0.005 0.0001"
                        // Contact friction parameters for dynamically generated contact pairs. 
                        // The first number is the sliding friction, acting along both axes of the tangent plane. 
                        // The second number is the torsional friction, acting around the contact normal.
                        // The third number is the rolling friction, acting around both axes of the tangent plane.
                        // The friction parameters for the contact pair are computed as the element-wise maximum of 
                        // the geom-specific parameters. See also Parameters section in the Computation chapter.
                        float? slidingFriction = null;
                        float? torsionalFriction = null;
                        float? rollingFriction = null;
                        var frictionSplit = attribute.Value.Split(' ');
                        if (frictionSplit?.Length >= 3)
                            rollingFriction = (float)Convert.ToDouble(frictionSplit[2], System.Globalization.CultureInfo.InvariantCulture);
                        if (frictionSplit?.Length >= 2)
                            torsionalFriction = (float)Convert.ToDouble(frictionSplit[1], System.Globalization.CultureInfo.InvariantCulture);
                        if (frictionSplit?.Length >= 1)
                            slidingFriction = (float)Convert.ToDouble(frictionSplit[0], System.Globalization.CultureInfo.InvariantCulture);
                        var physicMaterial = geom.GetComponent<Collider>()?.material;
                        physicMaterial.staticFriction = slidingFriction.Value;
                        if (rollingFriction.HasValue)
                            physicMaterial.dynamicFriction = rollingFriction.Value;
                        else if (torsionalFriction.HasValue)
                            physicMaterial.dynamicFriction = torsionalFriction.Value;
                        else
                            physicMaterial.dynamicFriction = slidingFriction.Value;
                        break;
                    case "mass": // optional
                        // If this attribute is specified, the density attribute below is ignored and the geom density
                        // is computed from the given mass, using the geom shape and the assumption of uniform density. 
                        // The computed density is then used to obtain the geom inertia. Recall that the geom mass and
                        // inerta are only used during compilation, to infer the body mass and inertia if necessary.
                        // At runtime only the body inertial properties affect the simulation;
                        // the geom mass and inertia are not even saved in mjModel.
                        // DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        geom.GetComponent<Rigidbody>().mass = (float)Convert.ToDouble(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        break;
                    case "density": //  "1000"
                        // Material density used to compute the geom mass and inertia. The computation is based on the
                        // geom shape and the assumption of uniform density. The internal default of 1000 is the density
                        // of water in SI units. This attribute is used only when the mass attribute above is unspecified.
                        var density = (float)Convert.ToDouble(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        var rb = geom.GetComponent<Rigidbody>();
                        rb.SetDensity(density);
                        rb.mass = rb
                            .mass; // ref: https://forum.unity.com/threads/rigidbody-setdensity-doesnt-work.322911/
                        break;
                    case "solmix": // "1"
                        // This attribute specifies the weight used for averaging of constraint solver parameters.
                        // Recall that the solver parameters for a dynamically generated geom pair are obtained as a 
                        // weighted average of the geom-specific parameters.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "solref":
                        // Constraint solver parameters for contact simulation. See Solver parameters.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "solimp":
                        // Constraint solver parameters for contact simulation. See Solver parameters.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "margin": //  "0"
                        // Distance threshold below which contacts are detected and included in the global array mjData.contact.
                        // This however does not mean that contact force will be generated. A contact is considered active only
                        // if the distance between the two geom surfaces is below margin-gap. Recall that constraint impedance
                        // can be a function of distance, as explained in Solver parameters. The quantity this function is
                        // applied to is the distance between the two geoms minus the margin plus the gap.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "gap": // "0"
                        // This attribute is used to enable the generation of inactive contacts, i.e. contacts that are ignored
                        //by the constraint solver but are included in mjData.contact for the purpose of custom computations.
                        // When this value is positive, geom distances between margin and margin-gap correspond to such
                        // inactive contacts.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "fromto": // optional
                        // This attribute can only be used with capsule and cylinder geoms. It provides an alternative specification
                        //  of the geom length as well as the frame position and orientation. The six numbers are the 3D coordinates
                        // of one point followed by the 3D coordinates of another point. The cylinder geom (or cylinder part of the
                        // capsule geom) connects these two points, with the +Z axis of the geom's frame oriented from the first
                        // towards the second point. The frame orientation is obtained with the same procedure as the zaxis
                        // attribute described in Frame orientations. The frame position is in the middle between the two points.
                        // If this attribute is specified, the remaining position and orientation-related attributes are ignored.
                        // Handled at object init
                        break;
                    case "pos": // "0 0 0"
                        // Position of the geom frame, in local or global coordinates as determined by the coordinate
                        // attribute of compiler.
                        // Handled at object init
                        break;
                    case "hfield": // optional
                        // This attribute must be specified if and only if the geom type is "hfield".
                        // It references the height field asset to be instantiated at the position and orientation of the geom frame.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "mesh": // optional
                        // If the geom type is "mesh", this attribute is required. It references the mesh asset to be instantiated.
                        // This attribute can also be specified if the geom type corresponds to a geometric primitive, namely one
                        // of "sphere", "capsule", "cylinder", "ellipsoid", "box". In that case the primitive is automatically
                        // fitted to the mesh asset referenced here. The fitting procedure uses either the equivalent
                        // inertia box or the axis-aligned bounding box of the mesh, as determined by the attribute fitaabb
                        // of compiler. The resulting size of the fitted geom is usually what one would expect, but if not,
                        // it can be further adjusted with the fitscale attribute below. In the compiled mjModel the geom is
                        // represented as a regular geom of the specified primitive type, and there is no reference to the mesh
                        // used for fitting.                        
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "quat": // "1 0 0 0"
                        // If the quaternion is known, this is the preferred was to specify the frame orientation because it does
                        // not involve conversions. Instead it is normalized to unit length and copied into mjModel during compilation.
                        // When a model is saved as MJCF, all frame orientations are expressed as quaternions using this attribute.
                        if (_useWorldSpace)
                            geom.transform.rotation = MarathonHelper.ParseQuaternion(attribute.Value);
                        else
                            geom.transform.localRotation =
                                MarathonHelper.ParseQuaternion(attribute.Value) * parentBody.transform.rotation;
                        break;
                    case "axisangle": // optional
                        // These are the quantities (x, y, z, a) mentioned above. The last number is the angle of rotation,
                        // in degrees or radians as specified by the angle attribute of compiler. The first three numbers determine
                        // a 3D vector which is the rotation axis. This vector is normalized to unit length during compilation,
                        // so the user can specify a vector of any non-zero length. Keep in mind that the rotation is right-handed;
                        // if the direction of the vector (x, y, z) is reversed this will result in the opposite rotation.
                        // Changing the sign of a can also be used to specify the opposite rotation.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "xyaxes": //  optional
                        // The first 3 numbers are the X axis of the frame. The next 3 numbers are the Y axis of the frame,
                        // which is automatically made orthogonal to the X axis. The Z axis is then defined as the
                        // cross-product of the X and Y axes.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "zaxis": //  optional
                        // The Z axis of the frame. The compiler finds the minimal rotation that maps the vector (0,0,1)
                        // into the vector specified here. This determines the X and Y axes of the frame implicitly.
                        // This is useful for geoms with rotational symmetry around the Z axis, as well as lights - which
                        // are oriented along the Z axis of their frame.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "euler": // optional
                        // Rotation angles around three coordinate axes. The sequence of axes around which these rotations are applied
                        // is determined by the eulerseq attribute of compiler and is the same for the entire model.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "fitscale": // "1"
                        // This attribute is used only when a primitive geometric type is being fitted to a mesh asset.
                        // The scale specified here is relative to the output of the automated fitting procedure. The default value 
                        // of 1 leaves the result unchanged, a value of 2 makes all sizes of the fitted geom two times larger.
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "user":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    default:
                    {
                        DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                        throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                        break;
                    }
                }
            }
        }

        Joint FixedJoint(GameObject parent)
        {
            parent.gameObject.AddComponent<FixedJoint>();
            var joint = parent.GetComponent<Joint>();
            return joint;
        }

        XElement BuildFromClasses(string type, XElement xdoc)
        {
            XElement subClass = new XElement(type);
            if (_childClassStack.Count > 0 && _childClassStack.Peek()?.Element(type) != null)
                subClass = _childClassStack.Peek()?.Element(type);
            var defaultClass = _root.Element("default")?.Element(type) ?? new XElement(type);
            xdoc = xdoc ?? new XElement(type);
            var attributes =
                defaultClass.Attributes()
                    .Concat(subClass.Attributes())
                    .Concat(xdoc.Attributes())
                    .GroupBy(x => x.Name)
                    .Select(x => x.Last());
            XElement element = new XElement(type, attributes);
            if (element.Attribute("class") != null)
                element = AddClass(type, element.Attribute("class").Value, element);
            return element;
        }

        XElement AddClass(string type, string subClassName, XElement xdoc, XElement nestingRef = null)
        {
            if (nestingRef == null)
                nestingRef = _root.Element("default");
            foreach (var item in nestingRef.Attributes("class"))
            {
                if (item.Value == subClassName)
                {
                    // found class
                    var subClass = nestingRef.Element(type) ?? new XElement(type);
                    var attributes =
                        xdoc.Attributes()
                            .Concat(subClass.Attributes())
                            .GroupBy(x => x.Name)
                            .Select(x => x.Last());
                    XElement element = new XElement(type, attributes);
                    return element;
                }
            }

            foreach (var item in nestingRef.Elements("default"))
            {
                if (nestingRef != null)
                    xdoc = AddClass(type, subClassName, xdoc, item);
            }

            return xdoc;
        }

        List<KeyValuePair<string, Joint>> ParseJoint(XElement xdoc, GeomItem parentGeom, GeomItem childGeom,
            GameObject body)
        {
            _jointXDocs.Add(xdoc.Attribute("name").Value, xdoc);
            var joints = new List<KeyValuePair<string, Joint>>();

            GameObject bone = null;
            var childRidgedBody = childGeom.Geom.GetComponent<Rigidbody>();
            var parentRidgedBody = parentGeom.Geom.GetComponent<Rigidbody>();

            if (xdoc == null)
                return joints;
            XElement element = BuildFromClasses("joint", xdoc);

            var type = element.Attribute("type")?.Value;
            if (type == null)
            {
                DebugPrint($"--- WARNING: ParseJoint: no type found. Assuming Hinge: ({element.ToString()}");
                type = "hinge";
            }

            Joint joint = null;
            Type jointType;
            string jointName = element.Attribute("name")?.Value;
            switch (type)
            {
                case "hinge":
                    DebugPrint($"ParseJoint: Creating type:{type} ");
                    jointType = typeof(HingeJoint);
                    break;
                case "free":
                    DebugPrint($"ParseJoint: Creating type:{type} ");
                    jointType = typeof(FixedJoint);
                    break;
                default:
                    DebugPrint(
                        $"--- WARNING: ParseJoint: joint type '{type}' is not implemented. Ignoring ({element.ToString()}");
                    return joints;
            }

            Joint existingJoint = childGeom.Bones
                .SelectMany(x => x.GetComponents<Joint>())
                .FirstOrDefault(y => y.connectedBody == parentRidgedBody);
            if (existingJoint)
            {
                bone = new GameObject();
                bone.transform.SetPositionAndRotation(childGeom.Geom.transform.position,
                    childGeom.Geom.transform.rotation);
                bone.transform.localScale = childGeom.Geom.transform.localScale;
                bone.transform.parent = childGeom.Geom.transform;
                bone.name = jointName;
                var boneRidgedBody = bone.AddComponent<Rigidbody>();
                boneRidgedBody.useGravity = false;
                joint = bone.AddComponent(jointType) as Joint;
                existingJoint.connectedBody = boneRidgedBody;
                childGeom.Bones.Add(bone);
            }
            else
            {
                joint = childGeom.Geom.AddComponent(jointType) as Joint;
                if (!childGeom.Bones.Contains(childGeom.Geom))
                    childGeom.Bones.Add(childGeom.Geom);
            }

            Collider boneCollider = null;
            if (bone != null)
                boneCollider = CopyCollider(bone, childGeom.Geom);
            joint.connectedBody = parentRidgedBody;

            ApplyClassToJoint(element, joint, childGeom, body, bone ?? childGeom.Geom);

            if (boneCollider != null)
                Destroy(boneCollider);

            // force as configurable
            if (jointType == typeof(HingeJoint))
                joint = ToConfigurable(joint as HingeJoint);

            joints.Add(new KeyValuePair<string, Joint>(jointName, joint));
            return joints;
        }

        Collider CopyCollider(GameObject target, GameObject source)
        {
            var sourceCollider = source.GetComponent<Collider>();
            var sourceCapsule = sourceCollider as CapsuleCollider;
            var sphereCollider = sourceCollider as SphereCollider;
            Collider targetCollider = null;
            if (sourceCapsule != null)
            {
                var targetCapsule = target.AddComponent<CapsuleCollider>();
                targetCollider = targetCapsule as Collider;
                targetCapsule.center = sourceCapsule.center;
                targetCapsule.radius = sourceCapsule.radius;
                targetCapsule.height = sourceCapsule.height;
                targetCapsule.direction = sourceCapsule.direction;
            }
            else if (sphereCollider != null)
            {
                var targetSphere = target.AddComponent<SphereCollider>();
                targetCollider = targetSphere as Collider;
                targetSphere.center = sphereCollider.center;
                targetSphere.radius = sphereCollider.radius;
            }
            else
                throw new NotImplementedException();

            if (sourceCollider != null)
            {
                targetCollider.isTrigger = sourceCollider.isTrigger;
                targetCollider.material = sourceCollider.material;
            }

            return targetCollider;
        }


        void ApplyClassToJoint(XElement classElement, Joint joint, GeomItem baseGeom, GameObject body, GameObject bone)
        {
            HingeJoint hingeJoint = joint as HingeJoint;
            FixedJoint fixedJoint = joint as FixedJoint;
            ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
            JointSpring spring = hingeJoint?.spring ?? new JointSpring();
            JointMotor motor = hingeJoint?.motor ?? new JointMotor();
            JointLimits limits = hingeJoint?.limits ?? new JointLimits();
            Vector3 jointOffset = Vector3.zero;
            foreach (var attribute in classElement.Attributes())
            {
                switch (attribute.Name.LocalName)
                {
                    case "armature":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "damping":
                        spring.damper = (float)Convert.ToDouble(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        break;
                    case "limited":
                        if (hingeJoint != null)
                            hingeJoint.useLimits = Convert.ToBoolean(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        break;
                    case "axis":
                        var axis = MarathonHelper.ParseAxis(attribute.Value);
                        axis = baseGeom.Geom.transform.InverseTransformDirection(axis);
                        joint.axis = axis;
                        break;
                    case "name":
                        if (bone != null && string.IsNullOrEmpty(bone.name))
                            bone.name = attribute.Value;
                        break;
                    case "pos":
                        jointOffset = MarathonHelper.ParsePosition(attribute.Value);
                        break;

                    case "range":
                        if (hingeJoint != null)
                        {
                            limits.min = MarathonHelper.ParseGetMin(attribute.Value);
                            limits.max = MarathonHelper.ParseGetMax(attribute.Value);
                            limits.bounceMinVelocity = 0f;
                            hingeJoint.useLimits = true;
                        }
                        else if (configurableJoint != null)
                        {
                            var low = configurableJoint.lowAngularXLimit;
                            low.limit = MarathonHelper.ParseGetMin(attribute.Value);
                            configurableJoint.lowAngularXLimit = low;
                            var high = configurableJoint.highAngularXLimit;
                            high.limit = MarathonHelper.ParseGetMax(attribute.Value);
                            configurableJoint.highAngularXLimit = high;
                        }

                        break;
                    case "class":
                        break;
                    case "type":
                        // NOTE: handle in setup
                        break;
                    case "solimplimit":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "solreflimit":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "stiffness":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    case "margin":
                        DebugPrint($"{name} {attribute.Name.LocalName}={attribute.Value}");
                        break;
                    default:
                        DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                        throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                        break;
                }
            }

            if (_useWorldSpace)
            {
                var jointPos = jointOffset;
                var localAxis = joint.transform.InverseTransformPoint(jointPos);
                joint.anchor = localAxis;
            }
            else
            {
                var jointPos = body.transform.position;
                jointPos -= bone.transform.position;
                jointPos += jointOffset;
                var localAxis = bone.transform.InverseTransformDirection(jointPos);
                joint.anchor = localAxis;
            }

            if (hingeJoint != null)
            {
                hingeJoint.spring = spring;
                hingeJoint.motor = motor;
                hingeJoint.limits = limits;
            }
        }

        static Vector3 GetLocalOrthoDirection(Transform transform, Vector3 worldDir)
        {
            worldDir = worldDir.normalized;

            float dotX = Vector3.Dot(worldDir, transform.right);
            float dotY = Vector3.Dot(worldDir, transform.up);
            float dotZ = Vector3.Dot(worldDir, transform.forward);

            float absDotX = Mathf.Abs(dotX);
            float absDotY = Mathf.Abs(dotY);
            float absDotZ = Mathf.Abs(dotZ);

            Vector3 orthoDirection = Vector3.right;
            if (absDotY > absDotX && absDotY > absDotZ) orthoDirection = Vector3.up;
            if (absDotZ > absDotX && absDotZ > absDotY) orthoDirection = Vector3.forward;

            if (Vector3.Dot(worldDir, transform.rotation * orthoDirection) < 0f) orthoDirection = -orthoDirection;

            return orthoDirection;
        }


        List<MarathonJoint> ParseGears(XElement xdoc, List<KeyValuePair<string, Joint>> joints)
        {
            var mujocoJoints = new List<MarathonJoint>();
            var name = "motor";

            var elements = xdoc?.Elements(name);
            if (elements == null)
                return mujocoJoints;
            foreach (var element in elements)
            {
                mujocoJoints.AddRange(ParseGear(element, joints));
            }

            foreach (var mujocoJoint in mujocoJoints)
            {
                mujocoJoint.TrueBase = FindTrueBase(mujocoJoint.Joint, mujocoJoints);
                mujocoJoint.TrueTarget = FindTrueTarget(mujocoJoint.Joint, mujocoJoints);
                mujocoJoint.MaximumForce = (mujocoJoint.Joint as ConfigurableJoint).angularXDrive.maximumForce;

                Vector3 worldSwingAxis = mujocoJoint.Joint.axis;
                Vector3 axis2 = GetLocalOrthoDirection(mujocoJoint.TrueTarget.transform, worldSwingAxis);
                Vector3 twistAxis = GetLocalOrthoDirection(mujocoJoint.TrueTarget.transform,
                    mujocoJoint.TrueTarget.transform.position - mujocoJoint.TrueBase.connectedBody.transform.position);
                Vector3 secondaryAxis = Vector3.Cross(axis2, twistAxis);
                (mujocoJoint.Joint as ConfigurableJoint).secondaryAxis = secondaryAxis;
            }

            return mujocoJoints;
        }

        ConfigurableJoint FindTrueBase(Joint joint, List<MarathonJoint> mJoints)
        {
            ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
            var rb = configurableJoint.GetComponent<Rigidbody>();
            if (rb.useGravity)
                return configurableJoint;
            ConfigurableJoint parentRb = mJoints
                    .Select(x => x.Joint)
                    .First(x => x.connectedBody == rb)
                as ConfigurableJoint;
            return FindTrueBase(parentRb, mJoints);
        }

        Transform FindTrueTarget(Joint joint, List<MarathonJoint> mJoints)
        {
            var targetRB = joint.connectedBody;
            var rb = joint.GetComponent<Rigidbody>();
            if (targetRB.useGravity)
                return targetRB.transform;
            var target = targetRB.GetComponent<ConfigurableJoint>();
            if (target == null)
                return targetRB.transform;
            return FindTrueTarget(target, mJoints);
        }

        List<MarathonJoint> ParseGear(XElement xdoc, List<KeyValuePair<string, Joint>> joints)
        {
            var mujocoJoints = new List<MarathonJoint>();
            XElement element = BuildFromClasses("gear", xdoc);

            string jointName = element.Attribute("joint")?.Value;
            if (jointName == null)
            {
                DebugPrint($"--- WARNING: ParseGears: no jointName found. Ignoring ({element.ToString()}");
                return mujocoJoints;
            }

            var matches = joints.Where(x => x.Key.ToLowerInvariant() == jointName.ToLowerInvariant())
                ?.Select(x => x.Value);
            if (matches == null)
            {
                DebugPrint(
                    $"--- ERROR: ParseGears: joint:'{jointName}' was not found in joints. Ignoring ({element.ToString()}");
                return mujocoJoints;
            }

            foreach (Joint joint in matches)
            {
                HingeJoint hingeJoint = joint as HingeJoint;
                ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
                JointSpring spring = new JointSpring();
                JointMotor motor = new JointMotor();
                if (hingeJoint != null)
                {
                    spring = hingeJoint.spring;
                    hingeJoint.useSpring = false;
                    hingeJoint.useMotor = true;
                    motor = hingeJoint.motor;
                    motor.freeSpin = true;
                }

                if (configurableJoint != null)
                {
                    configurableJoint.rotationDriveMode = RotationDriveMode.XYAndZ;
                }

                var mujocoJoint = new MarathonJoint
                {
                    Joint = joint,
                    JointName = jointName,
                };
                ApplyClassToGear(element, joint, mujocoJoint);

                mujocoJoints.Add(mujocoJoint);
            }

            return mujocoJoints;
        }

        void ApplyClassToGear(XElement classElement, Joint joint, MarathonJoint mujocoJoint)
        {
            HingeJoint hingeJoint = joint as HingeJoint;
            FixedJoint fixedJoint = joint as FixedJoint;
            ConfigurableJoint configurableJoint = joint as ConfigurableJoint;
            JointSpring spring = hingeJoint?.spring ?? new JointSpring();
            JointMotor motor = hingeJoint?.motor ?? new JointMotor();
            JointLimits limits = hingeJoint?.limits ?? new JointLimits();
            var angularXDrive = configurableJoint?.angularXDrive ?? new JointDrive();
            foreach (var attribute in classElement.Attributes())
            {
                switch (attribute.Name.LocalName)
                {
                    case "joint":
                        break;
                    case "ctrllimited":
                        var ctrlLimited = Convert.ToBoolean(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        mujocoJoint.CtrlLimited = ctrlLimited;
                        break;
                    case "ctrlrange":
                        var ctrlRange = MarathonHelper.ParseVector2(attribute.Value);
                        mujocoJoint.CtrlRange = ctrlRange;
                        break;
                    case "gear":
                        var gear = (float)Convert.ToDouble(attribute.Value, System.Globalization.CultureInfo.InvariantCulture);
                        gear *= MotorScale;
                        //var gear = 200;
                        mujocoJoint.Gear = gear;
                        spring.spring = gear;
                        motor.force = gear;
                        angularXDrive.maximumForce = gear;
                        angularXDrive.positionDamper = 1;
                        angularXDrive.positionSpring = 1;
                        break;
                    case "name":
                        var objName = attribute.Value;
                        mujocoJoint.Name = objName;
                        break;
                    default:
                        DebugPrint($"*** MISSING --> {name}.{attribute.Name.LocalName}");
                        throw new NotImplementedException(attribute.Name.LocalName);
#pragma warning disable
                        break;
                }
            }

            if (hingeJoint != null)
            {
                hingeJoint.spring = spring;
                hingeJoint.motor = motor;
                hingeJoint.limits = limits;
            }

            if (configurableJoint != null)
            {
                configurableJoint.angularXDrive = angularXDrive;
            }
        }

        List<MarathonSensor> ParseSensors(XElement xdoc, IEnumerable<Collider> colliders)
        {
            var mujocoSensors = new List<MarathonSensor>();
            var name = "touch";

            var elements = xdoc?.Elements(name);
            if (elements == null)
                return mujocoSensors;
            foreach (var element in elements)
            {
                var mujocoSensor = new MarathonSensor
                {
                    Name = element.Attribute("name")?.Value,
                    SiteName = element.Attribute("site")?.Value,
                };
                var match = colliders
                    .Where(x => x.name == mujocoSensor.SiteName)
                    .FirstOrDefault();
                if (match != null)
                    mujocoSensor.SiteObject = match;
                else
                    throw new NotImplementedException();
                mujocoSensors.Add(mujocoSensor);
            }

            return mujocoSensors;
        }

        public static Joint ToConfigurable(HingeJoint hingeJoint)
        {
            if (hingeJoint.useMotor)
            {
                throw new NotImplementedException();
            }

            ConfigurableJoint configurableJoint = hingeJoint.gameObject.AddComponent<ConfigurableJoint>();
            configurableJoint.anchor = hingeJoint.anchor;
            configurableJoint.autoConfigureConnectedAnchor = hingeJoint.autoConfigureConnectedAnchor;
            configurableJoint.axis = new Vector3(0 - hingeJoint.axis.x, 0 - hingeJoint.axis.y, 0 - hingeJoint.axis.z);
            configurableJoint.breakForce = hingeJoint.breakForce;
            configurableJoint.breakTorque = hingeJoint.breakTorque;
            configurableJoint.connectedAnchor = hingeJoint.connectedAnchor;
            configurableJoint.connectedBody = hingeJoint.connectedBody;
            configurableJoint.enableCollision = hingeJoint.enableCollision;
            configurableJoint.secondaryAxis = Vector3.zero;

            configurableJoint.xMotion = ConfigurableJointMotion.Locked;
            configurableJoint.yMotion = ConfigurableJointMotion.Locked;
            configurableJoint.zMotion = ConfigurableJointMotion.Locked;

            configurableJoint.angularXMotion =
                hingeJoint.useLimits ? ConfigurableJointMotion.Limited : ConfigurableJointMotion.Free;
            configurableJoint.angularYMotion = ConfigurableJointMotion.Locked;
            configurableJoint.angularZMotion = ConfigurableJointMotion.Locked;


            SoftJointLimit limit = new SoftJointLimit();
            limit.limit = hingeJoint.limits.max;
            limit.bounciness = hingeJoint.limits.bounciness;
            configurableJoint.highAngularXLimit = limit;

            limit = new SoftJointLimit();
            limit.limit = hingeJoint.limits.min;
            limit.bounciness = hingeJoint.limits.bounciness;
            configurableJoint.lowAngularXLimit = limit;

            SoftJointLimitSpring limitSpring = new SoftJointLimitSpring();
            limitSpring.damper = hingeJoint.useSpring ? hingeJoint.spring.damper : 0f;
            limitSpring.spring = hingeJoint.useSpring ? hingeJoint.spring.spring : 0f;
            configurableJoint.angularXLimitSpring = limitSpring;

            GameObject.DestroyImmediate(hingeJoint);

            return configurableJoint;
        }
    }
}