/******************************************************************************
 * 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 class LeapCylindricalSpace : LeapRadialSpace
    {

        protected override ITransformer CosntructBaseTransformer()
        {
            return new Transformer()
            {
                space = this,
                anchor = this,
                angleOffset = 0,
                heightOffset = 0,
                radiusOffset = radius,
                radiansPerMeter = 1.0f / radius
            };
        }

        protected override ITransformer ConstructTransformer(LeapSpaceAnchor anchor)
        {
            return new Transformer()
            {
                space = this,
                anchor = anchor
            };
        }

        protected override void UpdateRadialTransformer(ITransformer transformer, ITransformer parent, Vector3 rectSpaceDelta)
        {
            var radialTransformer = transformer as Transformer;
            var radialParent = parent as Transformer;

            radialTransformer.angleOffset = radialParent.angleOffset + rectSpaceDelta.x / radialParent.radiusOffset;
            radialTransformer.heightOffset = radialParent.heightOffset + rectSpaceDelta.y;
            radialTransformer.radiusOffset = radialParent.radiusOffset + rectSpaceDelta.z;
            radialTransformer.radiansPerMeter = 1.0f / (radialTransformer.radiusOffset);
        }

        public class Transformer : IRadialTransformer
        {
            public LeapCylindricalSpace space { get; set; }
            public LeapSpaceAnchor anchor { get; set; }

            public float angleOffset;
            public float heightOffset;
            public float radiusOffset;
            public float radiansPerMeter;

            public Vector3 TransformPoint(Vector3 localRectPos)
            {
                Vector3 anchorDelta;

                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                anchorDelta = localRectPos - anchorRectPos;

                float angle = angleOffset + anchorDelta.x / radiusOffset;
                float height = heightOffset + anchorDelta.y;
                float radius = radiusOffset + anchorDelta.z;

                Vector3 position;
                position.x = Mathf.Sin(angle) * radius;
                position.y = height;
                position.z = Mathf.Cos(angle) * radius - space.radius;
                return position;
            }

            public Vector3 InverseTransformPoint(Vector3 localWarpedPos)
            {
                localWarpedPos.z += space.radius;

                float angle = Mathf.Atan2(localWarpedPos.x, localWarpedPos.z);
                float height = localWarpedPos.y;
                float radius = new Vector2(localWarpedPos.x, localWarpedPos.z).magnitude;

                Vector3 anchorDelta;
                anchorDelta.x = (angle - angleOffset) * radiusOffset;
                anchorDelta.y = height - heightOffset;
                anchorDelta.z = radius - radiusOffset;

                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                Vector3 localRectPos = anchorRectPos + anchorDelta;

                return localRectPos;
            }

            public Quaternion TransformRotation(Vector3 localRectPos, Quaternion localRectRot)
            {
                Vector3 anchorDelta;

                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                anchorDelta = localRectPos - anchorRectPos;

                float angle = angleOffset + anchorDelta.x / radiusOffset;

                Quaternion rotation = Quaternion.Euler(0, angle * Mathf.Rad2Deg, 0);

                return rotation * localRectRot;
            }

            public Quaternion InverseTransformRotation(Vector3 localWarpedPos, Quaternion localWarpedRot)
            {
                localWarpedPos.z += space.radius;

                float angle = Mathf.Atan2(localWarpedPos.x, localWarpedPos.z);

                return Quaternion.Euler(0, -angle * Mathf.Rad2Deg, 0) * localWarpedRot;
            }

            public Vector3 TransformDirection(Vector3 localRectPos, Vector3 localRectDirection)
            {
                Vector3 anchorDelta;

                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                anchorDelta = localRectPos - anchorRectPos;

                float angle = angleOffset + anchorDelta.x / radiusOffset;

                Quaternion rotation = Quaternion.Euler(0, angle * Mathf.Rad2Deg, 0);

                return rotation * localRectDirection;
            }

            public Vector3 InverseTransformDirection(Vector3 localWarpedPos, Vector3 localWarpedDirection)
            {
                localWarpedPos.z += space.radius;

                float angle = Mathf.Atan2(localWarpedPos.x, localWarpedPos.z);

                return Quaternion.Euler(0, -angle * Mathf.Rad2Deg, 0) * localWarpedDirection;
            }

            public Matrix4x4 GetTransformationMatrix(Vector3 localRectPos)
            {
                Vector3 anchorDelta;

                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                anchorDelta = localRectPos - anchorRectPos;

                float angle = angleOffset + anchorDelta.x / radiusOffset;
                float height = heightOffset + anchorDelta.y;
                float radius = radiusOffset + anchorDelta.z;

                Vector3 position;
                position.x = Mathf.Sin(angle) * radius;
                position.y = height;
                position.z = Mathf.Cos(angle) * radius - space.radius;

                Quaternion rotation = Quaternion.Euler(0, angle * Mathf.Rad2Deg, 0);

                return Matrix4x4.TRS(position, rotation, Vector3.one);
            }

            public Vector4 GetVectorRepresentation(Transform element)
            {
                Vector3 elementRectPos = space.transform.InverseTransformPoint(element.position);
                Vector3 anchorRectPos = space.transform.InverseTransformPoint(anchor.transform.position);
                Vector3 delta = elementRectPos - anchorRectPos;

                Vector4 rep;
                rep.x = angleOffset + delta.x / radiusOffset;
                rep.y = heightOffset + delta.y;
                rep.z = radiusOffset + delta.z;
                rep.w = 1.0f / radiusOffset;
                return rep;
            }
        }
    }
}