using Passer.Tracking;

namespace Passer.Humanoid {


    /// <summary>
    /// A tracker wich can be used for custom tracking solutions
    /// </summary>
    /// THis tracking option supports custom trackers and sensors for a humanoid.
	[System.Serializable]
	public class CustomTracker : HumanoidTracker {

        /// \copydoc HumanoidTracker::name
        public override string name => "Custom Tracker";

        #region Manage

        /// <summary>
        /// A skeleton for the body
        /// </summary>
        /// When this is set, the tracking will be taken from this skeleton
        public BodySkeleton bodySkeleton;

        #endregion Manage

        #region Update

        public override void UpdateTracker() {
            base.UpdateTracker();

            if (bodySkeleton != null) {
                status = bodySkeleton.status;

                UpdateBodyFromSkeleton();
            }
        }

        protected void UpdateBodyFromSkeleton() {
            if (bodySkeleton == null || bodySkeleton.status != Tracker.Status.Tracking)
                return;

            UpdateTorso();
            UpdateLeftArm();
            UpdateRightArm();
            UpdateLeftLeg();
            UpdateRightLeg();

            humanoid.CopyRigToTargets();
        }

        protected virtual void UpdateTorso() {
            UpdateBone(humanoid.hipsTarget.hips.target, Tracking.Bone.Hips);
            UpdateBoneRotation(humanoid.hipsTarget.spine.target, Tracking.Bone.Spine);
            UpdateBoneRotation(humanoid.hipsTarget.chest.target, Tracking.Bone.Chest);
            UpdateBoneRotation(humanoid.headTarget.head.target, Tracking.Bone.Head);
        }

        protected virtual void UpdateLeftArm() {
            UpdateBoneRotation(humanoid.leftHandTarget.shoulder.target, Tracking.Bone.LeftShoulder);
            UpdateBoneRotation(humanoid.leftHandTarget.upperArm.target, Tracking.Bone.LeftUpperArm);
            UpdateBoneRotation(humanoid.leftHandTarget.forearm.target, Tracking.Bone.LeftForearm);
            UpdateBoneRotation(humanoid.leftHandTarget.hand.target, Tracking.Bone.LeftHand);
        }

        protected virtual void UpdateRightArm() {
            UpdateBoneRotation(humanoid.rightHandTarget.shoulder.target, Tracking.Bone.RightShoulder);
            UpdateBoneRotation(humanoid.rightHandTarget.upperArm.target, Tracking.Bone.RightUpperArm);
            UpdateBoneRotation(humanoid.rightHandTarget.forearm.target, Tracking.Bone.RightForearm);
            UpdateBoneRotation(humanoid.rightHandTarget.hand.target, Tracking.Bone.RightHand);
        }

        protected virtual void UpdateLeftLeg() {
            UpdateBoneRotation(humanoid.leftFootTarget.upperLeg.target, Tracking.Bone.LeftUpperLeg);
            UpdateBoneRotation(humanoid.leftFootTarget.lowerLeg.target, Tracking.Bone.LeftLowerLeg);
            UpdateBoneRotation(humanoid.leftFootTarget.foot.target, Tracking.Bone.LeftFoot);
        }

        protected virtual void UpdateRightLeg() {
            UpdateBoneRotation(humanoid.rightFootTarget.upperLeg.target, Tracking.Bone.RightUpperLeg);
            UpdateBoneRotation(humanoid.rightFootTarget.lowerLeg.target, Tracking.Bone.RightLowerLeg);
            UpdateBoneRotation(humanoid.rightFootTarget.foot.target, Tracking.Bone.RightFoot);
        }

        private void UpdateBone(HumanoidTarget.TargetTransform target, Tracking.Bone boneId) {
            TrackedBone trackedBone = bodySkeleton.GetBone(boneId);
            if (trackedBone == null)
                return;

            float confidence = trackedBone.rotationConfidence;
            if (confidence > 0) {
                target.confidence.rotation = confidence;
                target.transform.rotation = bodySkeleton.transform.rotation * trackedBone.transform.rotation;
            }

            confidence = trackedBone.positionConfidence;
            if (confidence > 0) {
                target.confidence.position = confidence;
                target.transform.position = bodySkeleton.transform.TransformPoint(trackedBone.transform.position);
            }
        }

        private void UpdateBoneRotation(HumanoidTarget.TargetTransform target, Tracking.Bone boneId) {
            TrackedBone trackedBone = bodySkeleton.GetBone(boneId);
            if (trackedBone == null)
                return;

            float confidence = trackedBone.rotationConfidence;
            if (confidence > 0) {
                target.confidence.rotation = confidence;
                target.transform.rotation = bodySkeleton.transform.rotation * trackedBone.transform.rotation;
            }
        }

        #endregion Update
    }
}