using UnityEngine;
//using Windows.Kinect;

using System;
using System.Collections;
using System.Collections.Generic;
using com.rfilkov.kinect;


namespace com.rfilkov.components
{
    /// <summary>
    /// Avatar controller is the component that transfers the captured user motion to a humanoid model (avatar).
    /// </summary>
    [RequireComponent(typeof(Animator))]
    public class AvatarController : MonoBehaviour
    {
        [Tooltip("Index of the player, tracked by this component. 0 means the 1st player, 1 - the 2nd one, 2 - the 3rd one, etc.")]
        public int playerIndex = 0;

        [Tooltip("Whether the avatar is facing the player or not.")]
        public bool mirroredMovement = false;

        [Tooltip("Whether the avatar is allowed to move vertically or not.")]
        public bool verticalMovement = true;

        [Tooltip("Whether the avatar is allowed to move horizontally or not.")]
        public bool horizontalMovement = true;

        [Tooltip("Whether the avatar's root motion is applied by other component or script.")]
        public bool externalRootMotion = false;

        [Tooltip("Whether the head rotation is controlled externally (e.g. by VR-headset).")]
        public bool externalHeadRotation = false;

        [Tooltip("Whether the hand and finger rotations are controlled externally (e.g. by LeapMotion controller)")]
        public bool externalHandRotations = false;

        [Tooltip("Whether the finger orientations are allowed or not.")]
        public bool fingerOrientations = false;

        [Tooltip("Rate at which the avatar will move through the scene.")]
        public float moveRate = 1f;

        [Tooltip("Smooth factor used for avatar movements and joint rotations.")]
        public float smoothFactor = 10f;

        [Tooltip("Whether to update the avatar in LateUpdate(), instead of in Update(). Needed for Mecanim animation blending.")]
        public bool lateUpdateAvatar = false;

        [Tooltip("Game object this transform is relative to (optional).")]
        public Transform offsetNode;

        [Tooltip("If enabled, makes the avatar position relative to this camera to be the same as the player's position to the sensor.")]
        public Camera posRelativeToCamera;

        [Tooltip("Whether the avatar's position should match the color image (in Pos-rel-to-camera mode only).")]
        public bool posRelOverlayColor = false;

        //[Tooltip("Plane used to render the color camera background to overlay.")]
        //public Transform backgroundPlane;

        [Tooltip("Whether z-axis movement needs to be inverted (Pos-Relative mode only).")]
        [HideInInspector]
        public bool posRelInvertedZ = false;

        [Tooltip("Whether the avatar's feet must stick to the ground.")]
        public bool groundedFeet = false;

        [Tooltip("Whether to apply the humanoid model's muscle limits or not.")]
        public bool applyMuscleLimits = false;

        [Tooltip("Whether to flip left and right, relative to the sensor.")]
        public bool flipLeftRight = false;


        [Tooltip("Horizontal offset of the avatar with respect to the position of user's spine-base.")]
        [Range(-0.5f, 0.5f)]
        public float horizontalOffset = 0f;

        [Tooltip("Vertical offset of the avatar with respect to the position of user's spine-base.")]
        [Range(-0.5f, 0.5f)]
        public float verticalOffset = 0f;

        [Tooltip("Forward offset of the avatar with respect to the position of user's spine-base.")]
        [Range(-0.5f, 0.5f)]
        public float forwardOffset = 0f;

        // suggested and implemented by Ruben Gonzalez
        [Tooltip("Whether to use unscaled or normal (scaled) time.")]
        public bool useUnscaledTime = false;

        [Tooltip("Radius of the joint sphere and bone capsule colliders, in meters. You can set it to 0.02 to try it out. 0 means no collider.")]
        [Range(0f, 0.1f)]
        public float boneColliderRadius = 0f;  // 0.02f;

        // userId of the player
        [NonSerialized]
        public ulong playerId = 0;


        // The body root node
        protected Transform bodyRoot;
        protected float hipCenterDist = 0f;

        // Variable to hold all them bones. It will initialize the same size as initialRotations.
        protected Transform[] bones;
        //protected Transform[] fingerBones;

        protected CapsuleCollider[] boneColliders;
        protected Transform[] boneColTrans;
        protected Transform[] boneColJoint;
        protected Transform[] boneColParent;

        // Rotations of the bones when the Kinect tracking starts.
        protected Quaternion[] initialRotations;
        protected Quaternion[] localRotations;
        protected bool[] isBoneDisabled;

        // Local rotations of finger bones
        protected Dictionary<HumanBodyBones, Quaternion> fingerBoneLocalRotations = new Dictionary<HumanBodyBones, Quaternion>();
        protected Dictionary<HumanBodyBones, Vector3> fingerBoneLocalAxes = new Dictionary<HumanBodyBones, Vector3>();

        // Initial position and rotation of the transform
        protected Vector3 initialPosition;
        protected Quaternion initialRotation;
        protected Vector3 initialHipsPosition;
        protected Quaternion initialHipsRotation;
        protected Vector3 initialUpVector;

        //protected Vector3 offsetNodePos;
        //protected Quaternion offsetNodeRot;
        protected Vector3 bodyRootPosition;

        // Calibration Offset Variables for Character Position.
        [NonSerialized]
        public bool offsetCalibrated = false;
        protected Vector3 offsetPos = Vector3.zero;
        //protected float xOffset, yOffset, zOffset;
        //private Quaternion originalRotation;
        //protected Vector3 offsetCamPos = Vector3.zero;
        //protected Quaternion offsetCamRot = Quaternion.identity;

        // whether the user pose has been applied on the avatar or not
        protected bool poseApplied = false;
        protected Quaternion pelvisRotation = Quaternion.identity;

        // sharp rotation angle
        protected const float SHARP_ROT_ANGLE = 90f;  // 90 degrees

        protected Animator animatorComponent = null;
        private HumanPoseHandler humanPoseHandler = null;
        private HumanPose humanPose = new HumanPose();

        // whether the parent transform obeys physics
        protected bool isRigidBody = false;

        // private instance of the KinectManager
        protected KinectManager kinectManager;

        //// last hand events
        //private InteractionManager.HandEventType lastLeftHandEvent = InteractionManager.HandEventType.Release;
        //private InteractionManager.HandEventType lastRightHandEvent = InteractionManager.HandEventType.Release;

        //// fist states
        //private bool bLeftFistDone = false;
        //private bool bRightFistDone = false;

        // grounder constants and variables
        //protected const int raycastLayers = ~2;  // Ignore Raycast
        protected const float MaxFootDistanceGround = 0.02f;  // maximum distance from lower foot to the ground
        protected const float MaxFootDistanceTime = 0.2f; // 1.0f;  // maximum allowed time, the lower foot to be distant from the ground
        protected Transform leftFoot, rightFoot;
        protected Vector3 leftFootPos, rightFootPos;

        //protected float fFootDistanceInitial = 0f;
        protected float fFootDistance = 0f;
        protected float fFootDistanceTime = 0f;
        protected Vector3 vFootCorrection = Vector3.zero;

        //// background plane rectangle
        //private Rect planeRect = new Rect();
        //private bool planeRectSet = false;

        // last time when the avatar was updated
        protected float lastUpdateTime = 0f;
        protected const float MaxUpdateTime = 0.5f;  // allow 0.5 seconds max for smooth updates


        /// <summary>
        /// Gets the number of bone transforms (array length).
        /// </summary>
        /// <returns>The number of bone transforms.</returns>
        public int GetBoneTransformCount()
        {
            return bones != null ? bones.Length : 0;
        }

        /// <summary>
        /// Gets the bone transform by index.
        /// </summary>
        /// <returns>The bone transform.</returns>
        /// <param name="index">Index</param>
        public Transform GetBoneTransform(int index)
        {
            if (index >= 0 && bones != null && index < bones.Length)
            {
                return bones[index];
            }

            return null;
        }

        /// <summary>
        /// Disables the bone and optionally resets its orientation.
        /// </summary>
        /// <param name="index">Bone index.</param>
        /// <param name="resetBone">If set to <c>true</c> resets bone orientation.</param>
        public void DisableBone(int index, bool resetBone)
        {
            if (index >= 0 && index < bones.Length)
            {
                isBoneDisabled[index] = true;

                if (resetBone && bones[index] != null)
                {
                    bones[index].rotation = localRotations[index];
                }
            }
        }

        /// <summary>
        /// Enables the bone, so AvatarController could update its orientation.
        /// </summary>
        /// <param name="index">Bone index.</param>
        public void EnableBone(int index)
        {
            if (index >= 0 && index < bones.Length)
            {
                isBoneDisabled[index] = false;
            }
        }

        /// <summary>
        /// Determines whether the bone orientation update is enabled or not.
        /// </summary>
        /// <returns><c>true</c> if the bone update is enabled; otherwise, <c>false</c>.</returns>
        /// <param name="index">Bone index.</param>
        public bool IsBoneEnabled(int index)
        {
            if (index >= 0 && index < bones.Length)
            {
                return !isBoneDisabled[index];
            }

            return false;
        }

        /// <summary>
        /// Gets the bone index by joint type.
        /// </summary>
        /// <returns>The bone index.</returns>
        /// <param name="joint">Joint type</param>
        /// <param name="bMirrored">If set to <c>true</c> gets the mirrored joint index.</param>
        public int GetBoneIndexByJoint(KinectInterop.JointType joint, bool bMirrored)
        {
            int boneIndex = -1;

            if (jointMap2boneIndex.ContainsKey(joint))
            {
                boneIndex = !bMirrored ? jointMap2boneIndex[joint] : mirrorJointMap2boneIndex[joint];
            }

            return boneIndex;
        }

        /// <summary>
        /// Gets the list of AC-controlled mecanim bones.
        /// </summary>
        /// <returns>List of AC-controlled mecanim bones</returns>
        public List<HumanBodyBones> GetMecanimBones()
        {
            List<HumanBodyBones> alMecanimBones = new List<HumanBodyBones>();

            for (int boneIndex = 0; boneIndex < bones.Length; boneIndex++)
            {
                if (!boneIndex2MecanimMap.ContainsKey(boneIndex) || boneIndex >= 21)
                    continue;

                alMecanimBones.Add(boneIndex2MecanimMap[boneIndex]);
            }

            return alMecanimBones;
        }


        // transform caching gives performance boost since Unity calls GetComponent<Transform>() each time you call transform 
        private Transform _transformCache;
        public new Transform transform
        {
            get
            {
                if (!_transformCache)
                {
                    _transformCache = base.transform;
                }

                return _transformCache;
            }
        }


        public virtual void Awake()
        {
            // check for double start
            if (bones != null)
                return;
            if (!gameObject.activeInHierarchy)
                return;

            // inits the bones array
            bones = new Transform[25];

            // get the animator reference
            animatorComponent = GetComponent<Animator>();

            // Map bones to the points the Kinect tracks
            MapBones();

            // get distance to hip center
            Vector3 bodyRootPos = bodyRoot != null ? bodyRoot.position : transform.position;
            Vector3 hipCenterPos = bodyRoot != null ? bodyRoot.position : (bones != null && bones.Length > 0 && bones[0] != null ? bones[0].position : transform.position);
            hipCenterDist = (hipCenterPos - bodyRootPos).magnitude;

            // Set model's arms to be in T-pose, if needed
            SetModelArmsInTpose();

            // Initial rotations and directions of the bones.
            initialRotations = new Quaternion[bones.Length];
            localRotations = new Quaternion[bones.Length];
            isBoneDisabled = new bool[bones.Length];

            // Get initial bone rotations
            GetInitialRotations();

            // enable all bones
            for (int i = 0; i < bones.Length; i++)
            {
                isBoneDisabled[i] = false;
            }

            // get initial distance to ground
            //fFootDistanceInitial = GetCorrDistanceToGround();
            fFootDistance = 0f;
            fFootDistanceTime = 0f;

            // get left & right foot positions
            leftFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.FootLeft, false));
            rightFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.FootRight, false));

            if (leftFoot == null || rightFoot == null)
            {
                leftFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.AnkleLeft, false));
                rightFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.AnkleRight, false));
            }

            leftFootPos = leftFoot != null ? leftFoot.position : Vector3.zero;
            rightFootPos = rightFoot != null ? rightFoot.position : Vector3.zero;

            // if parent transform uses physics
            isRigidBody = (gameObject.GetComponent<Rigidbody>() != null);

            // get the pose handler reference
            if (animatorComponent && animatorComponent.avatar && animatorComponent.avatar.isHuman)
            {
                Transform hipsTransform = animatorComponent.GetBoneTransform(HumanBodyBones.Hips);
                Transform rootTransform = hipsTransform;  // transform;

                humanPoseHandler = new HumanPoseHandler(animatorComponent.avatar, rootTransform);
                humanPoseHandler.GetHumanPose(ref humanPose);

                initialHipsPosition = (humanPose.bodyPosition - rootTransform.position);  // hipsTransform.position
                initialHipsRotation = humanPose.bodyRotation;
                //Debug.Log($"{gameObject.name} - initialHipsPos: {initialHipsPosition}, rot: {initialHipsRotation.eulerAngles}, humanPosePos: {humanPose.bodyPosition}, transformPos: {rootTransform.position}");
            }

            // create bone and joint colliders, if needed
            CreateBoneColliders();
        }


        public virtual void Update()
        {
            if(kinectManager == null)
            {
                kinectManager = KinectManager.Instance;
            }

            ulong userId = kinectManager ? kinectManager.GetUserIdByIndex(playerIndex) : 0;
            if (playerId != userId)
            {
                if (/**playerId == 0 &&*/ userId != 0)
                    SuccessfulCalibration(userId, false);
                else if (/**playerId != 0 &&*/ userId == 0)
                    ResetToInitialPosition();
            }

            if (!lateUpdateAvatar && playerId != 0)
            {
                //Vector3 playerPos = kinectManager.GetUserPosition(playerId);
                //Vector3 playerRot = kinectManager.GetJointOrientation(playerId, 0, true).eulerAngles;
                //Debug.Log(string.Format("Avatar userIndex: {0}, userId: {1}, pos: {2}, rot: {3}", playerIndex, playerId, playerPos, playerRot));

                UpdateAvatar(playerId);
            }
        }


        public virtual void LateUpdate()
        {
            if (lateUpdateAvatar && playerId != 0)
            {
                UpdateAvatar(playerId);
            }

            // update bone colliders, as needed
            UpdateBoneColliders();
        }


        // applies the muscle limits for humanoid avatar
        private void CheckMuscleLimits()
        {
            if (humanPoseHandler == null)
                return;

            humanPoseHandler.GetHumanPose(ref humanPose);

            //Debug.Log(playerId + " - Trans: " + transform.position + ", body: " + humanPose.bodyPosition);

            bool isPoseChanged = false;

            float muscleMin = -1f;
            float muscleMax = 1f;

            for (int i = 0; i < humanPose.muscles.Length; i++)
            {
                if (float.IsNaN(humanPose.muscles[i]))
                {
                    //humanPose.muscles[i] = 0f;
                    continue;
                }

                if (humanPose.muscles[i] < muscleMin)
                {
                    humanPose.muscles[i] = muscleMin;
                    isPoseChanged = true;
                }
                else if (humanPose.muscles[i] > muscleMax)
                {
                    humanPose.muscles[i] = muscleMax;
                    isPoseChanged = true;
                }
            }

            if (isPoseChanged)
            {
                //Quaternion localBodyRot = Quaternion.Inverse(transform.rotation) * humanPose.bodyRotation;
                Vector3 localBodyPos = Quaternion.Inverse(initialHipsRotation) * initialHipsPosition;
                Quaternion localBodyRot = Quaternion.Inverse(initialHipsRotation) * humanPose.bodyRotation;
                //Debug.Log($"{gameObject.name} - lBodyPos: {localBodyPos}, lBodyRot: {localBodyRot.eulerAngles}\ninitHipsPos: {initialHipsPosition}, initHipsRot: {initialHipsRotation.eulerAngles}");

                // recover the body position & orientation
                humanPose.bodyPosition = localBodyPos; // initialHipsPosition;
                humanPose.bodyRotation = localBodyRot; // Quaternion.identity;

                humanPoseHandler.SetHumanPose(ref humanPose);
                //Debug.Log("  Human pose updated.");
            }

        }


        /// <summary>
        /// Updates the avatar each frame.
        /// </summary>
        /// <param name="UserID">User ID</param>
        public virtual void UpdateAvatar(ulong UserID)
        {
            if (!gameObject.activeInHierarchy)
                return;

            // Get the KinectManager instance
            if (kinectManager == null)
            {
                kinectManager = KinectManager.Instance;
            }

            //// get the background plane rectangle if needed 
            //if (backgroundPlane && !planeRectSet && kinectManager && kinectManager.IsInitialized())
            //{
            //    planeRectSet = true;

            //    planeRect.width = 10f * Mathf.Abs(backgroundPlane.localScale.x);
            //    planeRect.height = 10f * Mathf.Abs(backgroundPlane.localScale.z);
            //    planeRect.x = backgroundPlane.position.x - planeRect.width / 2f;
            //    planeRect.y = backgroundPlane.position.y - planeRect.height / 2f;
            //}

            // move the avatar to its Kinect position
            if (!externalRootMotion)
            {
                MoveAvatar(UserID);
            }

            //// get the left hand state and event
            //if (kinectManager && kinectManager.GetJointTrackingState(UserID, (int)KinectInterop.JointType.HandLeft) != KinectInterop.TrackingState.NotTracked)
            //{
            //    KinectInterop.HandState leftHandState = kinectManager.GetLeftHandState(UserID);
            //    InteractionManager.HandEventType leftHandEvent = InteractionManager.HandStateToEvent(leftHandState, lastLeftHandEvent);

            //    if (leftHandEvent != InteractionManager.HandEventType.None)
            //    {
            //        lastLeftHandEvent = leftHandEvent;
            //    }
            //}

            //// get the right hand state and event
            //if (kinectManager && kinectManager.GetJointTrackingState(UserID, (int)KinectInterop.JointType.HandRight) != KinectInterop.TrackingState.NotTracked)
            //{
            //    KinectInterop.HandState rightHandState = kinectManager.GetRightHandState(UserID);
            //    InteractionManager.HandEventType rightHandEvent = InteractionManager.HandStateToEvent(rightHandState, lastRightHandEvent);

            //    if (rightHandEvent != InteractionManager.HandEventType.None)
            //    {
            //        lastRightHandEvent = rightHandEvent;
            //    }
            //}

            // check for sharp pelvis rotations
            float pelvisAngle = GetPelvisAngle(UserID, false);

            if (!poseApplied || pelvisAngle < SHARP_ROT_ANGLE)  
            {
                // rotate the avatar bones
                for (var boneIndex = 0; boneIndex < bones.Length; boneIndex++)
                {
                    if (!bones[boneIndex] || isBoneDisabled[boneIndex])  // check for missing or disabled bones
                        continue;

                    bool flip = !(mirroredMovement ^ flipLeftRight);
                    if (boneIndex2JointMap.ContainsKey(boneIndex))
                    {
                        KinectInterop.JointType joint = flip ? boneIndex2JointMap[boneIndex] : boneIndex2MirrorJointMap[boneIndex];

                        if (externalHeadRotation && joint == KinectInterop.JointType.Head)   // skip head if moved externally
                        {
                            continue;
                        }

                        if (externalHandRotations &&    // skip hands if moved externally
                            (joint == KinectInterop.JointType.WristLeft || joint == KinectInterop.JointType.WristRight ||
                                joint == KinectInterop.JointType.HandLeft || joint == KinectInterop.JointType.HandRight))
                        {
                            continue;
                        }

                        TransformBone(UserID, joint, boneIndex, flip);
                    }
                    else if (boneIndex >= 21 && boneIndex <= 24)
                    {
                        // fingers or thumbs
                        if (fingerOrientations && !externalHandRotations)
                        {
                            KinectInterop.JointType joint = flip ? boneIndex2FingerMap[boneIndex] : boneIndex2MirrorFingerMap[boneIndex];

                            TransformSpecialBoneFingers(UserID, (int)joint, boneIndex, flip);
                        }
                    }
                }

            }

            // save pelvis rotation
            SavePelvisRotation(UserID);

            // user pose has been applied
            poseApplied = true;

            if (applyMuscleLimits && kinectManager && kinectManager.IsUserTracked(UserID))
            {
                // check for limits
                CheckMuscleLimits();
            }

            // update time
            lastUpdateTime = Time.time;
        }

        /// <summary>
        /// Resets bones to their initial positions and rotations. This also releases avatar control from KM, by settings playerId to 0 
        /// </summary>
        public virtual void ResetToInitialPosition()
        {
            //Debug.Log("ResetToInitialPosition. UserId: " + playerId);
            playerId = 0;

            if (bones == null)
                return;

            // For each bone that was defined, reset to initial position.
            transform.rotation = Quaternion.identity;

            for (int pass = 0; pass < 2; pass++)  // 2 passes because clavicles are at the end
            {
                for (int i = 0; i < bones.Length; i++)
                {
                    if (bones[i] != null)
                    {
                        bones[i].rotation = initialRotations[i];
                    }
                }
            }

            // reset finger bones to initial position
            //Animator animatorComponent = GetComponent<Animator>();
            foreach (HumanBodyBones bone in fingerBoneLocalRotations.Keys)
            {
                Transform boneTransform = animatorComponent ? animatorComponent.GetBoneTransform(bone) : null;

                if (boneTransform)
                {
                    boneTransform.localRotation = fingerBoneLocalRotations[bone];
                }
            }

            //// Restore the offset's position and rotation
            //if (offsetNode != null)
            //{
            //    offsetNode.transform.position = offsetNodePos;
            //    offsetNode.transform.rotation = offsetNodeRot;
            //}

            transform.position = initialPosition;
            transform.rotation = initialRotation;
            initialUpVector = transform.up;
        }

        /// <summary>
        /// Invoked on the successful calibration of the player.
        /// </summary>
        /// <param name="userId">User identifier.</param>
        public virtual void SuccessfulCalibration(ulong userId, bool resetInitialTransform)
        {
            playerId = userId;
            //Debug.Log("SuccessfulCalibration. UserId: " + playerId);

            //// reset the models position
            //if (offsetNode != null)
            //{
            //    offsetNode.transform.position = offsetNodePos;
            //    offsetNode.transform.rotation = offsetNodeRot;
            //}

            // reset initial position / rotation if needed 
            if (resetInitialTransform)
            {
                bodyRootPosition = transform.position;
                initialPosition = transform.position;
                initialRotation = transform.rotation;
            }

            transform.position = initialPosition;
            transform.rotation = initialRotation;
            initialUpVector = transform.up;

            // re-calibrate the position offset
            offsetCalibrated = false;
            poseApplied = false;
        }

        /// <summary>
        /// Moves the avatar to its initial/base position 
        /// </summary>
        /// <param name="position"> world position </param>
        /// <param name="rotation"> rotation offset </param>
        public virtual void ResetInitialTransform(Vector3 position, Vector3 rotation)
        {
            bodyRootPosition = position;
            initialPosition = position;
            initialRotation = Quaternion.Euler(rotation);

            transform.position = initialPosition;
            transform.rotation = initialRotation;
            initialUpVector = transform.up;

            offsetCalibrated = false;  // this causes calibrating offset in MoveAvatar function 
            poseApplied = false;
        }

        /// <summary>
        /// Sets the avatar's offset position (position of initial user detection).
        /// </summary>
        /// <param name="pos">New offset position. If zero, sets the current player position as offset position.</param>
        public void SetOffsetPos(Vector3 pos)
        {
            if(pos == Vector3.zero)
            {
                pos = kinectManager.GetUserPosition(playerId);
            }

            if(pos != Vector3.zero)
            {
                offsetPos.x = pos.x;
                offsetPos.y = pos.y;
                offsetPos.z = !mirroredMovement && !posRelativeToCamera ? -pos.z : pos.z;

                offsetCalibrated = true;
                //Debug.LogWarning($"{gameObject.name} offset set to: {offsetPos:F2}");
            }
        }

        // Checks if the given joint is part of the legs
        protected bool IsLegJoint(KinectInterop.JointType joint)
        {
            return ((joint == KinectInterop.JointType.HipLeft) || (joint == KinectInterop.JointType.HipRight) ||
                    (joint == KinectInterop.JointType.KneeLeft) || (joint == KinectInterop.JointType.KneeRight) ||
                    (joint == KinectInterop.JointType.AnkleLeft) || (joint == KinectInterop.JointType.AnkleRight));
        }

        // saves current pelvis rotation
        protected void SavePelvisRotation(ulong userId)
        {
            if (kinectManager != null && kinectManager.IsJointTracked(userId, (int)KinectInterop.JointType.Pelvis))
            {
                Quaternion curPelvisRot = kinectManager.GetJointOrientation(userId, (int)KinectInterop.JointType.Pelvis, false);
                if (poseApplied)
                    pelvisRotation = Quaternion.RotateTowards(pelvisRotation, curPelvisRot, 90f * Time.deltaTime);  // 90 deg/s
                else
                    pelvisRotation = curPelvisRot;
                //Debug.Log($"    P{playerIndex}, id: {playerId} - Pel: {pelvisRotation.eulerAngles}, Cur: {curPelvisRot.eulerAngles} P: {poseApplied}, dT: {Time.deltaTime:F3}, P: {poseApplied}, dT: {Time.deltaTime:F3}");
            }
        }

        // returns the angle between the last and current pelvis orientations (in degrees 0-180), or -1 if anything goes wrong
        protected float GetPelvisAngle(ulong userId, bool flip)
        {
            int iJoint = (int)KinectInterop.JointType.Pelvis;
            if (kinectManager == null || !kinectManager.IsJointTracked(userId, iJoint))
                return -1f;

            // get Kinect joint orientation
            Quaternion jointRotation = kinectManager.GetJointOrientation(userId, iJoint, flip);
            if (jointRotation == Quaternion.identity)
                return -1f;

            float angle = Quaternion.Angle(pelvisRotation, jointRotation);

            return angle;
        }

        // Apply the rotations tracked by kinect to the joints.
        protected virtual void TransformBone(ulong userId, KinectInterop.JointType joint, int boneIndex, bool flip)
        {
            Transform boneTransform = bones[boneIndex];
            if (boneTransform == null || kinectManager == null)
                return;

            int iJoint = (int)joint;
            if ((iJoint < 0) || (kinectManager.GetJointTrackingState(userId, iJoint) < KinectInterop.TrackingState.Tracked))
                return;

            // Get Kinect joint orientation
            Quaternion jointRotation = kinectManager.GetJointOrientation(userId, iJoint, flip);
            if (jointRotation == Quaternion.identity && !IsLegJoint(joint))
                return;

            //if (joint == KinectInterop.JointType.WristLeft)
            //{
            //    //jointRotation = Quaternion.identity;
            //    Debug.Log(string.Format("AC {0:F3} {1}, user: {2}, state: {3}\npos: {4}, rot: {5}", Time.time, joint,
            //        userId, kinectManager.GetJointTrackingState(userId, iJoint),
            //        kinectManager.GetJointPosition(userId, iJoint), jointRotation.eulerAngles));
            //}

            // calculate the new orientation
            Quaternion newRotation = Kinect2AvatarRot(jointRotation, boneIndex);

            if (externalRootMotion)
            {
                newRotation = transform.rotation * newRotation;
            }

            // Smoothly transition to the new rotation
            bool isSmoothAllowed = (Time.time - lastUpdateTime) <= MaxUpdateTime;

            if (isSmoothAllowed && smoothFactor != 0f)
                boneTransform.rotation = Quaternion.Slerp(boneTransform.rotation, newRotation, smoothFactor * (useUnscaledTime ? Time.unscaledDeltaTime : Time.deltaTime));
            else
                boneTransform.rotation = newRotation;

            //if(boneIndex == 5 || boneIndex == 6)  // clavicles
            //{
            //    Debug.Log(boneIndex + " rot - joint: " + jointRotation.eulerAngles + ", k2a: " + newRotation.eulerAngles + ", trans: " + boneTransform.rotation.eulerAngles);
            //}
        }

        // Apply the rotations tracked by kinect to fingers (one joint = multiple bones)
        protected virtual void TransformSpecialBoneFingers(ulong userId, int joint, int boneIndex, bool flip)
        {
            //// check for hand grips
            //if (joint == (int)KinectInterop.JointType.HandtipLeft || joint == (int)KinectInterop.JointType.ThumbLeft)
            //{
            //    if (lastLeftHandEvent == InteractionManager.HandEventType.Grip)
            //    {
            //        if (!bLeftFistDone && !kinectManager.IsUserTurnedAround(userId))
            //        {
            //            float angleSign = !mirroredMovement /**(boneIndex == 21 || boneIndex == 22)*/ ? -1f : -1f;
            //            float angleRot = angleSign * 60f;

            //            TransformSpecialBoneFist(boneIndex, angleRot);
            //            bLeftFistDone = (boneIndex >= 29);
            //        }

            //        return;
            //    }
            //    else if (bLeftFistDone && lastLeftHandEvent == InteractionManager.HandEventType.Release)
            //    {
            //        TransformSpecialBoneUnfist(boneIndex);
            //        bLeftFistDone = !(boneIndex >= 29);
            //    }
            //}
            //else if (joint == (int)KinectInterop.JointType.HandtipRight || joint == (int)KinectInterop.JointType.ThumbRight)
            //{
            //    if (lastRightHandEvent == InteractionManager.HandEventType.Grip)
            //    {
            //        if (!bRightFistDone && !kinectManager.IsUserTurnedAround(userId))
            //        {
            //            float angleSign = !mirroredMovement /**(boneIndex == 21 || boneIndex == 22)*/ ? -1f : -1f;
            //            float angleRot = angleSign * 60f;

            //            TransformSpecialBoneFist(boneIndex, angleRot);
            //            bRightFistDone = (boneIndex >= 29);
            //        }

            //        return;
            //    }
            //    else if (bRightFistDone && lastRightHandEvent == InteractionManager.HandEventType.Release)
            //    {
            //        TransformSpecialBoneUnfist(boneIndex);
            //        bRightFistDone = !(boneIndex >= 29);
            //    }
            //}

            bool isJointTracked = kinectManager.IsJointTracked(userId, joint);
            if (!animatorComponent || !isJointTracked)
                return;

            // Get Kinect joint orientation
            Quaternion jointRotation = kinectManager.GetJointOrientation(userId, joint, flip);
            if (jointRotation == Quaternion.identity)
                return;

            // calculate the new orientation
            Quaternion newRotation = Kinect2AvatarRot(jointRotation, boneIndex);

            if (externalRootMotion)
            {
                newRotation = transform.rotation * newRotation;
            }

            // get the list of bones
            List<HumanBodyBones> alBones = boneIndex2MultiBoneMap[boneIndex];

            // Smoothly transition to the new rotation
            bool isSmoothAllowed = (Time.time - lastUpdateTime) <= MaxUpdateTime;

            for (int i = 0; i < alBones.Count; i++)
            {
                Transform boneTransform = animatorComponent.GetBoneTransform(alBones[i]);
                if (!boneTransform)
                    continue;

                if (isSmoothAllowed && smoothFactor != 0f)
                    boneTransform.rotation = Quaternion.Slerp(boneTransform.rotation, newRotation, smoothFactor * (useUnscaledTime ? Time.unscaledDeltaTime : Time.deltaTime));
                else
                    boneTransform.rotation = newRotation;
            }
        }

        // Apply the rotations needed to transform fingers to fist
        protected virtual void TransformSpecialBoneFist(int boneIndex, float angle)
        {
            if (!animatorComponent)
                return;

            List<HumanBodyBones> alBones = boneIndex2MultiBoneMap[boneIndex];
            for (int i = 0; i < alBones.Count; i++)
            {
                if (i < 1 && (boneIndex == 22 || boneIndex == 24))  // skip the first thumb bone
                    continue;

                HumanBodyBones bone = alBones[i];
                Transform boneTransform = animatorComponent.GetBoneTransform(bone);

                // set the fist rotation
                if (boneTransform && fingerBoneLocalAxes[bone] != Vector3.zero)
                {
                    Quaternion qRotFinger = Quaternion.AngleAxis(angle, fingerBoneLocalAxes[bone]);
                    boneTransform.localRotation = fingerBoneLocalRotations[bone] * qRotFinger;
                }
            }

        }

        // Apply the initial rotations fingers
        protected virtual void TransformSpecialBoneUnfist(int boneIndex)
        {
            if (!animatorComponent)
                return;

            List<HumanBodyBones> alBones = boneIndex2MultiBoneMap[boneIndex];
            for (int i = 0; i < alBones.Count; i++)
            {
                HumanBodyBones bone = alBones[i];
                Transform boneTransform = animatorComponent.GetBoneTransform(bone);

                // set the initial rotation
                if (boneTransform)
                {
                    boneTransform.localRotation = fingerBoneLocalRotations[bone];
                }
            }
        }

        // Moves the avatar - gets the tracked position of the user and applies it to avatar.
        protected virtual void MoveAvatar(ulong UserID)
        {
            if ((moveRate == 0f) || !kinectManager ||
               (kinectManager.GetJointTrackingState(UserID, (int)KinectInterop.JointType.Pelvis) < KinectInterop.TrackingState.Tracked))
            {
                return;
            }

            // get the position of user's spine base
            Vector3 trans = kinectManager.GetUserPosition(UserID);

            // move avatar transform
            DoMoveAvatar(UserID, trans);
        }

        // Moves the avatar transform
        protected void DoMoveAvatar(ulong UserID, Vector3 trans)
        {
            //Debug.Log("User " + playerIndex + " pos: " + trans);
            if (flipLeftRight)
                trans.x = -trans.x;

            if (posRelativeToCamera)
            {
                if (posRelOverlayColor)
                {
                    // disable grounded feet
                    if(groundedFeet)
                    {
                        groundedFeet = false;
                    }

                    // use the color overlay position
                    int sensorIndex = kinectManager.GetPrimaryBodySensorIndex();

                    //if (backgroundPlane && planeRectSet)
                    //{
                    //    // get the plane overlay position
                    //    trans = kinectManager.GetJointPosColorOverlay(UserID, (int)KinectInterop.JointType.Pelvis, sensorIndex, planeRect);
                    //    trans.z = backgroundPlane.position.z - posRelativeToCamera.transform.position.z - 0.1f;  // 10cm offset
                    //}
                    //else
                    {
                        Rect backgroundRect = posRelativeToCamera.pixelRect;
                        PortraitBackground portraitBack = PortraitBackground.Instance;

                        if (portraitBack && portraitBack.enabled)
                        {
                            backgroundRect = portraitBack.GetBackgroundRect();
                        }

                        trans = kinectManager.GetJointPosColorOverlay(UserID, (int)KinectInterop.JointType.Pelvis, sensorIndex, posRelativeToCamera, backgroundRect);
                    }
                }
                else
                {
                    // move according to the camera
                    Vector3 bodyRootPos = bodyRoot != null ? bodyRoot.position : transform.position;
                    Vector3 userLocalPos = kinectManager.GetUserKinectPosition(UserID, true);
                    trans = posRelativeToCamera.transform.TransformPoint(userLocalPos);
                    //Debug.Log("  trans: " + trans + ", localPos: " + userLocalPos + ", camPos: " + posRelativeToCamera.transform.position);

                    if (!horizontalMovement)
                    {
                        trans = new Vector3(bodyRootPos.x, trans.y, bodyRootPos.z);
                    }

                    if (verticalMovement)
                    {
                        trans.y -= hipCenterDist;
                    }
                    else
                    {
                        trans.y = bodyRootPos.y;
                    }

                    //Debug.Log("cameraPos: " + posRelativeToCamera.transform.position + ", cameraRot: " + posRelativeToCamera.transform.rotation.eulerAngles +
                    //    ", bodyRoot: " + bodyRootPos + ", hipCenterDist: " + hipCenterDist + ", localPos: " + userLocalPos + ", trans: " + trans);
                }

                if (flipLeftRight)
                    trans.x = -trans.x;

                if(posRelOverlayColor || !offsetCalibrated)
                {
                    if (bodyRoot != null)
                    {
                        bodyRoot.position = trans;
                    }
                    else
                    {
                        transform.position = trans;
                    }

                    bodyRootPosition = trans;
                    //Debug.Log($"BodyRootPos set: {trans:F2}");

                    // reset the body offset
                    offsetCalibrated = false;
                }
            }

            // invert the z-coordinate, if needed
            if (posRelativeToCamera && posRelInvertedZ)
            {
                trans.z = -trans.z;
            }

            //if (posRelativeToCamera /**&& horizontalMovement*/)
            //{
            //    //if(offsetCamPos != posRelativeToCamera.transform.position || offsetCamRot != posRelativeToCamera.transform.rotation)
            //    {
            //        //offsetCamPos = posRelativeToCamera.transform.position;
            //        //offsetCamRot = posRelativeToCamera.transform.rotation;
            //        //Debug.Log("Changed cam pos: " + offsetCamPos + ", rot: " + offsetCamRot.eulerAngles);
            //    }
            //}

            if (!offsetCalibrated)
            {
                offsetPos.x = trans.x;  // !mirroredMovement ? trans.x * moveRate : -trans.x * moveRate;
                offsetPos.y = trans.y;  // trans.y * moveRate;
                offsetPos.z = !mirroredMovement && !posRelativeToCamera ? -trans.z : trans.z;  // -trans.z * moveRate;

                offsetCalibrated = posRelativeToCamera || GetUserHipAngle(UserID) >= 170f;
                //Debug.LogWarning($"{gameObject.name} offset: {offsetPos:F2}, calibrated: {offsetCalibrated}, hipAngle: {GetUserHipAngle(UserID):F1}");
            }

            // transition to the new position
            Vector3 targetPos = bodyRootPosition + Kinect2AvatarPos(trans, verticalMovement, horizontalMovement);
            //Debug.Log("  targetPos: " + targetPos + ", trans: " + trans + ", offsetPos: " + offsetPos + ", bodyRootPos: " + bodyRootPosition);

            if (isRigidBody && !verticalMovement)
            {
                // workaround for obeying the physics (e.g. gravity falling)
                targetPos.y = bodyRoot != null ? bodyRoot.position.y : transform.position.y;
            }

            // fixed bone indices - thanks to Martin Cvengros!
            var biShoulderL = GetBoneIndexByJoint(KinectInterop.JointType.ShoulderLeft, false);  // you may replace 'false' with 'mirroredMovement'
            var biShoulderR = GetBoneIndexByJoint(KinectInterop.JointType.ShoulderRight, false);  // you may replace 'false' with 'mirroredMovement'
            var biPelvis = GetBoneIndexByJoint(KinectInterop.JointType.Pelvis, false);  // you may replace 'false' with 'mirroredMovement'
            var biNeck = GetBoneIndexByJoint(KinectInterop.JointType.Neck, false);  // you may replace 'false' with 'mirroredMovement'

            // added by r618
            if (horizontalMovement && horizontalOffset != 0f &&
                bones[biShoulderL] != null && bones[biShoulderR] != null)
            {
                // { 5, HumanBodyBones.LeftUpperArm},
                // { 11, HumanBodyBones.RightUpperArm},
                //Vector3 dirSpine = bones[5].position - bones[11].position;
                Vector3 dirShoulders = bones[biShoulderR].position - bones[biShoulderL].position;
                targetPos += dirShoulders.normalized * horizontalOffset;
            }

            if (verticalMovement && verticalOffset != 0f &&
                bones[biPelvis] != null && bones[biNeck] != null)
            {
                Vector3 dirSpine = bones[biNeck].position - bones[biPelvis].position;
                targetPos += dirSpine.normalized * verticalOffset;
            }

            if (horizontalMovement && forwardOffset != 0f &&
                bones[biPelvis] != null && bones[biNeck] != null && bones[biShoulderL] != null && bones[biShoulderR] != null)
            {
                Vector3 dirSpine = (bones[biNeck].position - bones[biPelvis].position).normalized;
                Vector3 dirShoulders = (bones[biShoulderR].position - bones[biShoulderL].position).normalized;
                Vector3 dirForward = Vector3.Cross(dirShoulders, dirSpine).normalized;

                targetPos += dirForward * forwardOffset;
            }

            if (groundedFeet && verticalMovement)  // without vertical movement, grounding produces an ever expanding jump up & down
            {
                float fNewDistance = GetCorrDistanceToGround();
                float fNewDistanceTime = useUnscaledTime ? Time.unscaledTime : Time.time;
                //Vector3 lastTargetPos = targetPos;

                if (Mathf.Abs(fNewDistance) >= MaxFootDistanceGround && Mathf.Abs(fFootDistance + fNewDistance) < 1f)  // limit the correction to 1 meter
                {
                    if ((fNewDistanceTime - fFootDistanceTime) >= MaxFootDistanceTime)
                    {
                        fFootDistance += fNewDistance;
                        fFootDistanceTime = fNewDistanceTime;

                        vFootCorrection = initialUpVector * fFootDistance;

                        //Debug.Log($"****{leftFoot.name} pos: {leftFoot.position}, ini: {leftFootPos}, dif: {leftFoot.position - leftFootPos}\n" +
                        //    $"****{rightFoot.name} pos: {rightFoot.position}, ini: {rightFootPos}, dif: {rightFoot.position - rightFootPos}\n" +
                        //    $"****footDist: {fNewDistance:F2}, footCorr: {vFootCorrection}, {transform.name} pos: {transform.position}");
                    }
                }
                else
                {
                    fFootDistanceTime = fNewDistanceTime;
                }

                targetPos += vFootCorrection;
                //Debug.Log($"Gnd targetPos: {targetPos}, lastPos: {lastTargetPos}, vFootCorrection: {vFootCorrection}\nfFootDistance: {fFootDistance:F2}, fNewDistance: {fNewDistance:F2}, upVector: {initialUpVector}, distTime: {(fNewDistanceTime - fFootDistanceTime):F3}");
            }

            bool isSmoothAllowed = (Time.time - lastUpdateTime) <= MaxUpdateTime;
            if (bodyRoot != null)
            {
                bodyRoot.position = isSmoothAllowed && smoothFactor != 0f ?
                    Vector3.Lerp(bodyRoot.position, targetPos, smoothFactor * (useUnscaledTime ? Time.unscaledDeltaTime : Time.deltaTime)) : targetPos;
            }
            else
            {
                transform.position = isSmoothAllowed && smoothFactor != 0f ?
                    Vector3.Lerp(transform.position, targetPos, smoothFactor * (useUnscaledTime ? Time.unscaledDeltaTime : Time.deltaTime)) : targetPos;
            }
        }

        // Returns the angle at user's hip (knee-hip-neck)
        protected float GetUserHipAngle(ulong userId)
        {
            float angle = 0f;

            if(kinectManager != null &&
                kinectManager.GetJointTrackingState(userId, (int)KinectInterop.JointType.Pelvis) >= KinectInterop.TrackingState.Tracked &&
                kinectManager.GetJointTrackingState(userId, (int)KinectInterop.JointType.Neck) >= KinectInterop.TrackingState.Tracked &&
                kinectManager.GetJointTrackingState(userId, (int)KinectInterop.JointType.KneeLeft) >= KinectInterop.TrackingState.Tracked &&
                kinectManager.GetJointTrackingState(userId, (int)KinectInterop.JointType.KneeRight) >= KinectInterop.TrackingState.Tracked)
            {
                Vector3 posPelvis = kinectManager.GetJointPosition(userId, (int)KinectInterop.JointType.Pelvis);
                Vector3 posNeck = kinectManager.GetJointPosition(userId, (int)KinectInterop.JointType.Neck);

                Vector3 posKneeL = kinectManager.GetJointPosition(userId, (int)KinectInterop.JointType.KneeLeft);
                Vector3 posKneeR = kinectManager.GetJointPosition(userId, (int)KinectInterop.JointType.KneeRight);
                Vector3 posKneeC = (posKneeL + posKneeR) / 2f;

                angle = Vector3.Angle(posNeck - posPelvis, posKneeC - posPelvis);
            }

            return angle;
        }

        // Set model's arms to be in T-pose
        protected virtual void SetModelArmsInTpose()
        {
            Vector3 vTposeLeftDir = transform.TransformDirection(Vector3.left);
            Vector3 vTposeRightDir = transform.TransformDirection(Vector3.right);

            Transform transLeftUarm = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.ShoulderLeft, false)); // animator.GetBoneTransform(HumanBodyBones.LeftUpperArm);
            Transform transLeftLarm = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.ElbowLeft, false)); // animator.GetBoneTransform(HumanBodyBones.LeftLowerArm);
            Transform transLeftHand = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.WristLeft, false)); // animator.GetBoneTransform(HumanBodyBones.LeftHand);

            if (transLeftUarm != null && transLeftLarm != null)
            {
                Vector3 vUarmLeftDir = transLeftLarm.position - transLeftUarm.position;
                float fUarmLeftAngle = Vector3.Angle(vUarmLeftDir, vTposeLeftDir);

                if (Mathf.Abs(fUarmLeftAngle) >= 5f)
                {
                    Quaternion vFixRotation = Quaternion.FromToRotation(vUarmLeftDir, vTposeLeftDir);
                    transLeftUarm.rotation = vFixRotation * transLeftUarm.rotation;
                }

                if (transLeftHand != null)
                {
                    Vector3 vLarmLeftDir = transLeftHand.position - transLeftLarm.position;
                    float fLarmLeftAngle = Vector3.Angle(vLarmLeftDir, vTposeLeftDir);

                    if (Mathf.Abs(fLarmLeftAngle) >= 5f)
                    {
                        Quaternion vFixRotation = Quaternion.FromToRotation(vLarmLeftDir, vTposeLeftDir);
                        transLeftLarm.rotation = vFixRotation * transLeftLarm.rotation;
                    }
                }
            }

            Transform transRightUarm = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.ShoulderRight, false)); // animator.GetBoneTransform(HumanBodyBones.RightUpperArm);
            Transform transRightLarm = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.ElbowRight, false)); // animator.GetBoneTransform(HumanBodyBones.RightLowerArm);
            Transform transRightHand = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.WristRight, false)); // animator.GetBoneTransform(HumanBodyBones.RightHand);

            if (transRightUarm != null && transRightLarm != null)
            {
                Vector3 vUarmRightDir = transRightLarm.position - transRightUarm.position;
                float fUarmRightAngle = Vector3.Angle(vUarmRightDir, vTposeRightDir);

                if (Mathf.Abs(fUarmRightAngle) >= 5f)
                {
                    Quaternion vFixRotation = Quaternion.FromToRotation(vUarmRightDir, vTposeRightDir);
                    transRightUarm.rotation = vFixRotation * transRightUarm.rotation;
                }

                if (transRightHand != null)
                {
                    Vector3 vLarmRightDir = transRightHand.position - transRightLarm.position;
                    float fLarmRightAngle = Vector3.Angle(vLarmRightDir, vTposeRightDir);

                    if (Mathf.Abs(fLarmRightAngle) >= 5f)
                    {
                        Quaternion vFixRotation = Quaternion.FromToRotation(vLarmRightDir, vTposeRightDir);
                        transRightLarm.rotation = vFixRotation * transRightLarm.rotation;
                    }
                }
            }

        }

        // If the bones to be mapped have been declared, map that bone to the model.
        protected virtual void MapBones()
        {
            for (int boneIndex = 0; boneIndex < bones.Length; boneIndex++)
            {
                if (!boneIndex2MecanimMap.ContainsKey(boneIndex))
                    continue;

                bones[boneIndex] = animatorComponent ? animatorComponent.GetBoneTransform(boneIndex2MecanimMap[boneIndex]) : null;
            }

            //// map finger bones, too
            //fingerBones = new Transform[fingerIndex2MecanimMap.Count];

            //for (int boneIndex = 0; boneIndex < fingerBones.Length; boneIndex++)
            //{
            //    if (!fingerIndex2MecanimMap.ContainsKey(boneIndex))
            //        continue;

            //    fingerBones[boneIndex] = animatorComponent ? animatorComponent.GetBoneTransform(fingerIndex2MecanimMap[boneIndex]) : null;
            //}
        }

        // creates the joint and bone colliders 
        protected virtual void CreateBoneColliders()
        {
            if (boneColliderRadius <= 0f)
                return;

            boneColliders = new CapsuleCollider[bones.Length];
            boneColTrans = new Transform[bones.Length];
            boneColJoint = new Transform[bones.Length];
            boneColParent = new Transform[bones.Length];

            for (int i = 0; i < bones.Length; i++)
            {
                if (bones[i] == null)
                    continue;

                SphereCollider jCollider = bones[i].gameObject.AddComponent<SphereCollider>();
                jCollider.radius = boneColliderRadius;

                if (i > 0)
                {
                    GameObject objBoneCollider = new GameObject("BoneCollider" + i);
                    objBoneCollider.transform.parent = bones[i];
                    boneColTrans[i] = objBoneCollider.transform;

                    CapsuleCollider bCollider = objBoneCollider.AddComponent<CapsuleCollider>();
                    bCollider.radius = boneColliderRadius;
                    bCollider.height = 0f;

                    boneColliders[i] = bCollider;
                }
            }

            for (int i = 0; i < bones.Length; i++)
            {
                if (boneColliders[i] == null)
                    continue;

                boneColJoint[i] = bones[i];
                Transform parentTrans = boneColJoint[i].parent;

                while (parentTrans != null)
                {
                    if (parentTrans.GetComponent<SphereCollider>() != null)
                        break;
                    parentTrans = parentTrans.parent;
                }

                if (parentTrans != null)
                    boneColParent[i] = parentTrans;
                else
                    boneColliders[i] = null;
            }
        }

        // updates the bone colliders, as needed
        protected void UpdateBoneColliders()
        {
            if (boneColliders == null)
                return;

            for (int i = 0; i < bones.Length; i++)
            {
                if (boneColliders[i] == null)
                    continue;

                Vector3 posJoint = boneColJoint[i].position;
                Vector3 posParent = boneColParent[i].position;

                Vector3 dirFromParent = posJoint - posParent;
                boneColTrans[i].position = posParent + dirFromParent / 2f;
                boneColTrans[i].up = dirFromParent.normalized;
                boneColliders[i].height = dirFromParent.magnitude;
            }
        }

        // Capture the initial rotations of the bones
        protected void GetInitialRotations()
        {
            //// save the initial rotation
            //if (offsetNode != null)
            //{
            //    offsetNodePos = offsetNode.transform.position;
            //    offsetNodeRot = offsetNode.transform.rotation;
            //}

            initialPosition = transform.position;
            initialRotation = transform.rotation;
            initialUpVector = transform.up;

            transform.rotation = Quaternion.identity;

            // save the body root initial position
            if (bodyRoot != null)
            {
                bodyRootPosition = bodyRoot.position;
            }
            else
            {
                bodyRootPosition = transform.position;
            }

            if (offsetNode != null)
            {
                bodyRootPosition = bodyRootPosition - offsetNode.position;
            }

            // save the initial bone rotations
            for (int i = 0; i < bones.Length; i++)
            {
                if (bones[i] != null)
                {
                    initialRotations[i] = bones[i].rotation;
                    localRotations[i] = bones[i].localRotation;
                }
            }

            // get finger bones' local rotations
            foreach (int boneIndex in boneIndex2MultiBoneMap.Keys)
            {
                List<HumanBodyBones> alBones = boneIndex2MultiBoneMap[boneIndex];

                for (int b = 0; b < alBones.Count; b++)
                {
                    HumanBodyBones bone = alBones[b];
                    Transform boneTransform = animatorComponent ? animatorComponent.GetBoneTransform(bone) : null;

                    // get the finger's 1st transform
                    Transform fingerBaseTransform = animatorComponent ? animatorComponent.GetBoneTransform(alBones[b - (b % 3)]) : null;

                    // get the finger's 2nd transform
                    Transform baseChildTransform = fingerBaseTransform && fingerBaseTransform.childCount > 0 ? fingerBaseTransform.GetChild(0) : null;
                    Vector3 vBoneDirChild = baseChildTransform && fingerBaseTransform ? (baseChildTransform.position - fingerBaseTransform.position).normalized : Vector3.zero;
                    Vector3 vOrthoDirChild = Vector3.Cross(vBoneDirChild, Vector3.up).normalized;

                    if (boneTransform)
                    {
                        fingerBoneLocalRotations[bone] = boneTransform.localRotation;

                        if (vBoneDirChild != Vector3.zero)
                        {
                            fingerBoneLocalAxes[bone] = boneTransform.InverseTransformDirection(vOrthoDirChild).normalized;
                        }
                        else
                        {
                            fingerBoneLocalAxes[bone] = Vector3.zero;
                        }
                    }
                }
            }

            // Restore the initial rotation
            transform.rotation = initialRotation;
        }

        // Converts kinect joint rotation to avatar joint rotation, depending on joint initial rotation and offset rotation
        protected Quaternion Kinect2AvatarRot(Quaternion jointRotation, int boneIndex)
        {
            Quaternion newRotation = jointRotation * initialRotations[boneIndex];
            //newRotation = initialRotation * newRotation;

            if (!externalRootMotion)  // fix by Mathias Parger
            {
                newRotation = initialRotation * newRotation;

                if (offsetNode != null)
                {
                    newRotation = offsetNode.rotation * newRotation;
                }
            }

            return newRotation;
        }

        // Converts Kinect position to avatar skeleton position, depending on initial position, mirroring and move rate
        protected Vector3 Kinect2AvatarPos(Vector3 jointPosition, bool bMoveVertically, bool bMoveHorizontally)
        {
            float xPos = (jointPosition.x - offsetPos.x) * moveRate;
            float yPos = (jointPosition.y - offsetPos.y) * moveRate;
            float zPos = !mirroredMovement && !posRelativeToCamera ? (-jointPosition.z - offsetPos.z) * moveRate : (jointPosition.z - offsetPos.z) * moveRate;

            Vector3 newPosition = new Vector3(bMoveHorizontally ? xPos : 0f, bMoveVertically ? yPos : 0f, bMoveHorizontally ? zPos : 0f);

            Quaternion posRotation = mirroredMovement ? Quaternion.Euler(0f, 180f, 0f) * initialRotation : initialRotation;
            newPosition = posRotation * newPosition;

            if (offsetNode != null)
            {
                //newPosition += offsetNode.transform.position;
                newPosition = offsetNode.position;
            }

            return newPosition;
        }

        // returns distance from the given transform to its initial position
        protected virtual float GetCorrDistanceToGround(Transform trans, Vector3 initialPos, bool isRightJoint)
        {
            if (!trans)
                return 0f;

            Vector3 deltaDir = trans.position - initialPos;
            Vector3 vTrans = new Vector3(deltaDir.x * initialUpVector.x, deltaDir.y * initialUpVector.y, deltaDir.z * initialUpVector.z);

            float fSign = Vector3.Dot(deltaDir, initialUpVector) < 0f ? 1f : -1f;  // change the sign, because it's a correction
            float deltaDist = fSign * vTrans.magnitude;

            return deltaDist;
        }

        // returns the min distance distance from left or right foot to the ground, or 0 if no LF/RF are found
        protected virtual float GetCorrDistanceToGround()
        {
            //if (leftFoot == null && rightFoot == null)
            //{
            //    leftFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.FootLeft, false));
            //    rightFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.FootRight, false));

            //    if (leftFoot == null || rightFoot == null)
            //    {
            //        leftFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.AnkleLeft, false));
            //        rightFoot = GetBoneTransform(GetBoneIndexByJoint(KinectInterop.JointType.AnkleRight, false));
            //    }

            //    leftFootPos = leftFoot != null ? leftFoot.position : Vector3.zero;
            //    rightFootPos = rightFoot != null ? rightFoot.position : Vector3.zero;
            //}

            float fDistMin = 1000f;
            float fDistLeft = leftFoot ? GetCorrDistanceToGround(leftFoot, leftFootPos, false) : fDistMin;
            float fDistRight = rightFoot ? GetCorrDistanceToGround(rightFoot, rightFootPos, true) : fDistMin;
            fDistMin = Mathf.Abs(fDistLeft) < Mathf.Abs(fDistRight) ? fDistLeft : fDistRight;

            if (fDistMin == 1000f)
            {
                fDistMin = 0f;
            }

            return fDistMin;
        }

        //	protected void OnCollisionEnter(Collision col)
        //	{
        //		Debug.Log("Collision entered");
        //	}
        //
        //	protected void OnCollisionExit(Collision col)
        //	{
        //		Debug.Log("Collision exited");
        //	}



        // dictionaries to speed up bone processing
        protected static readonly Dictionary<int, HumanBodyBones> boneIndex2MecanimMap = new Dictionary<int, HumanBodyBones>
        {
            {0, HumanBodyBones.Hips},
            {1, HumanBodyBones.Spine},
            {2, HumanBodyBones.Chest},
		    {3, HumanBodyBones.Neck},
    		{4, HumanBodyBones.Head},

            {5, HumanBodyBones.LeftShoulder},
            {6, HumanBodyBones.LeftUpperArm},
            {7, HumanBodyBones.LeftLowerArm},
            {8, HumanBodyBones.LeftHand},

            {9, HumanBodyBones.RightShoulder},
            {10, HumanBodyBones.RightUpperArm},
            {11, HumanBodyBones.RightLowerArm},
            {12, HumanBodyBones.RightHand},
		
		    {13, HumanBodyBones.LeftUpperLeg},
            {14, HumanBodyBones.LeftLowerLeg},
            {15, HumanBodyBones.LeftFoot},
    		{16, HumanBodyBones.LeftToes},
		
		    {17, HumanBodyBones.RightUpperLeg},
            {18, HumanBodyBones.RightLowerLeg},
            {19, HumanBodyBones.RightFoot},
    		{20, HumanBodyBones.RightToes},

		    {21, HumanBodyBones.LeftIndexProximal},
            {22, HumanBodyBones.LeftThumbProximal},
            {23, HumanBodyBones.RightIndexProximal},
            {24, HumanBodyBones.RightThumbProximal},
        };

        protected static readonly Dictionary<int, KinectInterop.JointType> boneIndex2JointMap = new Dictionary<int, KinectInterop.JointType>
        {
            {0, KinectInterop.JointType.Pelvis},
            {1, KinectInterop.JointType.SpineNaval},
            {2, KinectInterop.JointType.SpineChest},
            {3, KinectInterop.JointType.Neck},
            {4, KinectInterop.JointType.Head},

            {5, KinectInterop.JointType.ClavicleLeft},
            {6, KinectInterop.JointType.ShoulderLeft},
            {7, KinectInterop.JointType.ElbowLeft},
            {8, KinectInterop.JointType.WristLeft},

            {9, KinectInterop.JointType.ClavicleRight},
            {10, KinectInterop.JointType.ShoulderRight},
            {11, KinectInterop.JointType.ElbowRight},
            {12, KinectInterop.JointType.WristRight},

            {13, KinectInterop.JointType.HipLeft},
            {14, KinectInterop.JointType.KneeLeft},
            {15, KinectInterop.JointType.AnkleLeft},
            {16, KinectInterop.JointType.FootLeft},

            {17, KinectInterop.JointType.HipRight},
            {18, KinectInterop.JointType.KneeRight},
            {19, KinectInterop.JointType.AnkleRight},
            {20, KinectInterop.JointType.FootRight},
        };

        protected static readonly Dictionary<int, KinectInterop.JointType> boneIndex2MirrorJointMap = new Dictionary<int, KinectInterop.JointType>
        {
            {0, KinectInterop.JointType.Pelvis},
            {1, KinectInterop.JointType.SpineNaval},
            {2, KinectInterop.JointType.SpineChest},
            {3, KinectInterop.JointType.Neck},
            {4, KinectInterop.JointType.Head},

            {5, KinectInterop.JointType.ClavicleRight},
            {6, KinectInterop.JointType.ShoulderRight},
            {7, KinectInterop.JointType.ElbowRight},
            {8, KinectInterop.JointType.WristRight},

            {9, KinectInterop.JointType.ClavicleLeft},
            {10, KinectInterop.JointType.ShoulderLeft},
            {11, KinectInterop.JointType.ElbowLeft},
            {12, KinectInterop.JointType.WristLeft},

            {13, KinectInterop.JointType.HipRight},
            {14, KinectInterop.JointType.KneeRight},
            {15, KinectInterop.JointType.AnkleRight},
            {16, KinectInterop.JointType.FootRight},

            {17, KinectInterop.JointType.HipLeft},
            {18, KinectInterop.JointType.KneeLeft},
            {19, KinectInterop.JointType.AnkleLeft},
            {20, KinectInterop.JointType.FootLeft},
        };

        protected static readonly Dictionary<KinectInterop.JointType, int> jointMap2boneIndex = new Dictionary<KinectInterop.JointType, int>
        {
            {KinectInterop.JointType.Pelvis, 0},
            {KinectInterop.JointType.SpineNaval, 1},
            {KinectInterop.JointType.SpineChest, 2},
            {KinectInterop.JointType.Neck, 3},
            {KinectInterop.JointType.Head, 4},

            {KinectInterop.JointType.ClavicleLeft, 5},
            {KinectInterop.JointType.ShoulderLeft, 6},
            {KinectInterop.JointType.ElbowLeft, 7},
            {KinectInterop.JointType.WristLeft, 8},

            {KinectInterop.JointType.ClavicleRight, 9},
            {KinectInterop.JointType.ShoulderRight, 10},
            {KinectInterop.JointType.ElbowRight, 11},
            {KinectInterop.JointType.WristRight, 12},

            {KinectInterop.JointType.HipLeft, 13},
            {KinectInterop.JointType.KneeLeft, 14},
            {KinectInterop.JointType.AnkleLeft, 15},
            {KinectInterop.JointType.FootLeft, 16},

            {KinectInterop.JointType.HipRight, 17},
            {KinectInterop.JointType.KneeRight, 18},
            {KinectInterop.JointType.AnkleRight, 19},
            {KinectInterop.JointType.FootRight, 20},
        };

        protected static readonly Dictionary<KinectInterop.JointType, int> mirrorJointMap2boneIndex = new Dictionary<KinectInterop.JointType, int>
        {
            {KinectInterop.JointType.Pelvis, 0},
            {KinectInterop.JointType.SpineNaval, 1},
            {KinectInterop.JointType.SpineChest, 2},
            {KinectInterop.JointType.Neck, 3},
            {KinectInterop.JointType.Head, 4},

            {KinectInterop.JointType.ClavicleRight, 5},
            {KinectInterop.JointType.ShoulderRight, 6},
            {KinectInterop.JointType.ElbowRight, 7},
            {KinectInterop.JointType.WristRight, 8},

            {KinectInterop.JointType.ClavicleLeft, 9},
            {KinectInterop.JointType.ShoulderLeft, 10},
            {KinectInterop.JointType.ElbowLeft, 11},
            {KinectInterop.JointType.WristLeft, 12},

            {KinectInterop.JointType.HipRight, 13},
            {KinectInterop.JointType.KneeRight, 14},
            {KinectInterop.JointType.AnkleRight, 15},
            {KinectInterop.JointType.FootRight, 16},

            {KinectInterop.JointType.HipLeft, 17},
            {KinectInterop.JointType.KneeLeft, 18},
            {KinectInterop.JointType.AnkleLeft, 19},
            {KinectInterop.JointType.FootLeft, 20},
        };

        protected static readonly Dictionary<int, KinectInterop.JointType> boneIndex2FingerMap = new Dictionary<int, KinectInterop.JointType>
        {
            {21, KinectInterop.JointType.HandtipLeft},
            {22, KinectInterop.JointType.ThumbLeft},
            {23, KinectInterop.JointType.HandtipRight},
            {24, KinectInterop.JointType.ThumbRight},
        };

        protected static readonly Dictionary<int, KinectInterop.JointType> boneIndex2MirrorFingerMap = new Dictionary<int, KinectInterop.JointType>
        {
            {21, KinectInterop.JointType.HandtipRight},
            {22, KinectInterop.JointType.ThumbRight},
            {23, KinectInterop.JointType.HandtipLeft},
            {24, KinectInterop.JointType.ThumbLeft},
        };

        protected static readonly Dictionary<int, List<HumanBodyBones>> boneIndex2MultiBoneMap = new Dictionary<int, List<HumanBodyBones>>
        {
            {21, new List<HumanBodyBones> {  // left fingers
				    HumanBodyBones.LeftIndexProximal,
                    HumanBodyBones.LeftIndexIntermediate,
                    HumanBodyBones.LeftIndexDistal,
                    HumanBodyBones.LeftMiddleProximal,
                    HumanBodyBones.LeftMiddleIntermediate,
                    HumanBodyBones.LeftMiddleDistal,
                    HumanBodyBones.LeftRingProximal,
                    HumanBodyBones.LeftRingIntermediate,
                    HumanBodyBones.LeftRingDistal,
                    HumanBodyBones.LeftLittleProximal,
                    HumanBodyBones.LeftLittleIntermediate,
                    HumanBodyBones.LeftLittleDistal,
                }},
            {22, new List<HumanBodyBones> {  // left thumb
				    HumanBodyBones.LeftThumbProximal,
                    HumanBodyBones.LeftThumbIntermediate,
                    HumanBodyBones.LeftThumbDistal,
                }},
            {23, new List<HumanBodyBones> {  // right fingers
				    HumanBodyBones.RightIndexProximal,
                    HumanBodyBones.RightIndexIntermediate,
                    HumanBodyBones.RightIndexDistal,
                    HumanBodyBones.RightMiddleProximal,
                    HumanBodyBones.RightMiddleIntermediate,
                    HumanBodyBones.RightMiddleDistal,
                    HumanBodyBones.RightRingProximal,
                    HumanBodyBones.RightRingIntermediate,
                    HumanBodyBones.RightRingDistal,
                    HumanBodyBones.RightLittleProximal,
                    HumanBodyBones.RightLittleIntermediate,
                    HumanBodyBones.RightLittleDistal,
                }},
            {24, new List<HumanBodyBones> {  // right thumb
				    HumanBodyBones.RightThumbProximal,
                    HumanBodyBones.RightThumbIntermediate,
                    HumanBodyBones.RightThumbDistal,
                }},
        };

    }
}