using UnityEngine;

namespace UDE_HAND_INTERACTION
{ 
    public static class PoseManager
    {
        public static void SetPose(this Transform transform, in Pose pose, Space space = Space.World)
        {
            if (space == Space.World)
            {
                transform.SetPositionAndRotation(pose.position, pose.rotation);
            }
            else
            {
                transform.localRotation = pose.rotation;
                transform.localPosition = pose.position;
            }
        }
        public static Pose GetPose(this Transform transform, Space space = Space.World)
        {
            if (space == Space.World)
            {
                return new Pose(transform.position, transform.rotation);
            }
            else
            {
                return new Pose(transform.localPosition, transform.localRotation);
            }
        }

        public static Pose GlobalPoseScaled(Transform relativeTo, Pose offset)
        {
            Pose pose;
            if (relativeTo == null) return new();
            pose.position = relativeTo.TransformPoint(offset.position);
            pose.rotation = relativeTo.rotation * offset.rotation;
            return pose;
        }

        public static Pose DeltaScaled(Transform from, Transform to)
        {
            Pose delta;
            delta.position = from.InverseTransformPoint(to.position);
            delta.rotation = Quaternion.Inverse(from.rotation) * to.rotation;
            return delta;
        }

        private static Pose Delta(Vector3 fromPosition, Quaternion fromRotation, Vector3 toPosition, Quaternion toRotation)
        {
            Pose result = new Pose();
            Delta(fromPosition, fromRotation, toPosition, toRotation, ref result);
            return result;
        }
        private static void Delta(Vector3 fromPosition, Quaternion fromRotation, Vector3 toPosition, Quaternion toRotation, ref Pose result)
        {
            Quaternion inverseFromRot = Quaternion.Inverse(fromRotation);
            result.position = inverseFromRot * (toPosition - fromPosition);
            result.rotation = inverseFromRot * toRotation;
        }

        public static Pose Delta(this Transform from, in Pose to)
        {
            return Delta(from.position, from.rotation, to.position, to.rotation);
        }

        public static void Multiply(in Pose a, in Pose b, ref Pose result)
        {
            result.position = a.position + a.rotation * b.position;
            result.rotation = a.rotation * b.rotation;
        }

        public static Pose Multiply(in Pose a, in Pose b)
        {
            Pose result = new Pose();
            Multiply(a, b, ref result);
            return result;
        }
    }
}