/******************************************************************************
 * Copyright (C) Ultraleap, Inc. 2011-2021.                                   *
 *                                                                            *
 * Use subject to the terms of the Apache License 2.0 available at            *
 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement           *
 * between Ultraleap and you, your company or other organization.             *
 ******************************************************************************/

using UnityEngine;

namespace Leap.Unity.Space
{

    public interface ITransformer
    {

        LeapSpaceAnchor anchor { get; }

        /// <summary>
        /// Transform a point from rect space to warped space.
        /// </summary>
        Vector3 TransformPoint(Vector3 localRectPos);

        /// <summary>
        /// Transform a point from warped space to rect space.
        /// </summary>
        Vector3 InverseTransformPoint(Vector3 localWarpedSpace);

        /// <summary>
        /// Transform a rotation from rect space to warped space.
        /// </summary>
        Quaternion TransformRotation(Vector3 localRectPos, Quaternion localRectRot);

        /// <summary>
        /// Transform a rotation from warped space to rect space.
        /// </summary>
        Quaternion InverseTransformRotation(Vector3 localWarpedPos, Quaternion localWarpedRot);

        /// <summary>
        /// Transform a direction from rect space to warped space.
        /// </summary>
        Vector3 TransformDirection(Vector3 localRectPos, Vector3 localRectDirection);

        /// <summary>
        /// Transform a direction from warped space to rect space.
        /// </summary>
        Vector3 InverseTransformDirection(Vector3 localWarpedSpace, Vector3 localWarpedDirection);

        /// <summary>
        /// Get a transformation matrix that maps a position in rect space 
        /// to a position in warped space.
        /// </summary>
        Matrix4x4 GetTransformationMatrix(Vector3 localRectPos);
    }

    public class IdentityTransformer : ITransformer
    {
        public static readonly IdentityTransformer single = new IdentityTransformer();

        public LeapSpaceAnchor anchor
        {
            get
            {
                return null;
            }
        }

        public Vector3 TransformPoint(Vector3 localRectPos)
        {
            return localRectPos;
        }

        public Vector3 InverseTransformPoint(Vector3 localWarpedSpace)
        {
            return localWarpedSpace;
        }

        public Quaternion TransformRotation(Vector3 localRectPos, Quaternion localRectRot)
        {
            return localRectRot;
        }

        public Quaternion InverseTransformRotation(Vector3 localWarpedPos, Quaternion localWarpedRot)
        {
            return localWarpedRot;
        }

        public Vector3 TransformDirection(Vector3 localRectPos, Vector3 localRectDirection)
        {
            return localRectDirection;
        }

        public Vector3 InverseTransformDirection(Vector3 localWarpedSpace, Vector3 localWarpedDirection)
        {
            return localWarpedDirection;
        }

        public Matrix4x4 GetTransformationMatrix(Vector3 localRectPos)
        {
            return Matrix4x4.TRS(localRectPos, Quaternion.identity, Vector3.one);
        }
    }

    public static class ITransformerExtensions
    {

        /// <summary>
        /// Given a transformer and a world-space position and rotation, this method interprets that position
        /// and rotation as being within the transformers "warped" space (e.g. cylindrical space for LeapCylindricalSpace)
        /// and outputs the world-space position and rotation that would result if the space was no longer warped,
        /// i.e., standard Unity rectilinear space.
        /// </summary>
        public static void WorldSpaceUnwarp(this ITransformer transformer,
                                            Vector3 worldWarpedPosition, Quaternion worldWarpedRotation,
                                            out Vector3 worldRectilinearPosition, out Quaternion worldRectilinearRotation)
        {
            Transform spaceTransform = transformer.anchor.space.transform;

            Vector3 anchorLocalWarpedPosition = spaceTransform.InverseTransformPoint(worldWarpedPosition);
            Quaternion anchorLocalWarpedRotation = spaceTransform.InverseTransformRotation(worldWarpedRotation);

            Vector3 anchorLocalRectPosition = transformer.InverseTransformPoint(anchorLocalWarpedPosition);
            worldRectilinearPosition = spaceTransform.TransformPoint(anchorLocalRectPosition);

            Quaternion anchorLocalRectRotation = transformer.InverseTransformRotation(anchorLocalWarpedPosition, anchorLocalWarpedRotation);
            worldRectilinearRotation = spaceTransform.TransformRotation(anchorLocalRectRotation);
        }

    }

}