/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 * All rights reserved.
 *
 * Licensed under the Oculus SDK License Agreement (the "License");
 * you may not use the Oculus SDK except in compliance with the License,
 * which is provided at the time of installation or download, or which
 * otherwise accompanies this software in either electronic or hard copy form.
 *
 * You may obtain a copy of the License at
 *
 * https://developer.oculus.com/licenses/oculussdk/
 *
 * Unless required by applicable law or agreed to in writing, the Oculus SDK
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using Oculus.Interaction.Input;
using Oculus.Interaction.PoseDetection;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;
using UnityEngine.Assertions;

namespace Oculus.Interaction.GrabAPI
{
    /// <summary>
    /// This <see cref="IFingerAPI"/> uses the curl value of the fingers to detect if they are grabbing.
    /// </summary>
    /// <remarks>
    /// The implementation details of this grab calculation are more low-level than Unity and are thus encapsulated
    /// below the managed-native boundary. This type merely provides an API surface through which to invoke the
    /// native functionality.
    /// </remarks>
    public class FingerPalmGrabAPI : IFingerAPI
    {
        // Temporary structure used to pass data to and from native components
        [StructLayout(LayoutKind.Sequential)]
        public class HandData
        {
            private const int NumHandJoints = 24;
            [MarshalAs(UnmanagedType.ByValArray, SizeConst = NumHandJoints * 7, ArraySubType = UnmanagedType.R4)]
            private float[] jointValues;
            private float _rootRotX;
            private float _rootRotY;
            private float _rootRotZ;
            private float _rootRotW;
            private float _rootPosX;
            private float _rootPosY;
            private float _rootPosZ;
            private int _handedness;

            public HandData()
            {
                jointValues = new float[NumHandJoints * 7];
            }

            public void SetData(IReadOnlyList<Pose> joints, Pose root, Handedness handedness)
            {
                Assert.AreEqual(NumHandJoints, joints.Count);
                int jointValueIndex = 0;
                for (int jointIndex = 0; jointIndex < NumHandJoints; jointIndex++)
                {
                    Pose joint = joints[jointIndex];
                    jointValues[jointValueIndex++] = joint.rotation.x;
                    jointValues[jointValueIndex++] = joint.rotation.y;
                    jointValues[jointValueIndex++] = joint.rotation.z;
                    jointValues[jointValueIndex++] = joint.rotation.w;
                    jointValues[jointValueIndex++] = joint.position.x;
                    jointValues[jointValueIndex++] = joint.position.y;
                    jointValues[jointValueIndex++] = joint.position.z;
                }
                this._rootRotX = root.rotation.x;
                this._rootRotY = root.rotation.y;
                this._rootRotZ = root.rotation.z;
                this._rootRotW = root.rotation.w;
                this._rootPosX = root.position.x;
                this._rootPosY = root.position.y;
                this._rootPosZ = root.position.z;
                this._handedness = (int)handedness;
            }
        }

        #region DLLImports
        enum ReturnValue { Success = 0, Failure = -1 };


        [DllImport("InteractionSdk")]
        private static extern int isdk_FingerPalmGrabAPI_Create();

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_UpdateHandData(int handle, [In] HandData data);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetFingerIsGrabbing(int handle, HandFinger finger, out bool grabbing);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetFingerIsGrabbingChanged(int handle, HandFinger finger, bool targetGrabState, out bool changed);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetFingerGrabScore(int handle, HandFinger finger, out float score);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetCenterOffset(int handle, out Vector3 score);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetConfigParamFloat(int handle, PalmGrabParamID paramID, out float outVal);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_SetConfigParamFloat(int handle, PalmGrabParamID paramID, float inVal);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_GetConfigParamVec3(int handle, PalmGrabParamID paramID, out Vector3 outVal);

        [DllImport("InteractionSdk")]
        private static extern ReturnValue isdk_FingerPalmGrabAPI_SetConfigParamVec3(int handle, PalmGrabParamID paramID, Vector3 inVal);

        #endregion

        private int apiHandle_ = -1;
        private HandData handData_;

        public FingerPalmGrabAPI()
        {
            handData_ = new HandData();
        }

        private int GetHandle()
        {

            if (apiHandle_ == -1)
            {
                apiHandle_ = isdk_FingerPalmGrabAPI_Create();
                Debug.Assert(apiHandle_ != -1, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_Create failed");
            }

            return apiHandle_;
        }

        /// <summary>
        /// Implementation of <see cref="IFingerAPI.GetFingerIsGrabbing(HandFinger)"/>; for details, please refer to
        /// the related documentation provided for that property.
        /// </summary>
        public bool GetFingerIsGrabbing(HandFinger finger)
        {
            ReturnValue rv = isdk_FingerPalmGrabAPI_GetFingerIsGrabbing(GetHandle(), finger, out bool grabbing);
            Debug.Assert(rv != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetFingerIsGrabbing failed");
            return grabbing;
        }

        /// <summary>
        /// Implementation of <see cref="IFingerAPI.GetFingerIsGrabbingChanged(HandFinger, bool)"/>; for details, please refer to
        /// the related documentation provided for that property.
        /// </summary>
        public bool GetFingerIsGrabbingChanged(HandFinger finger, bool targetGrabState)
        {
            ReturnValue rv = isdk_FingerPalmGrabAPI_GetFingerIsGrabbingChanged(GetHandle(), finger, targetGrabState, out bool grabbing);
            Debug.Assert(rv != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetFingerIsGrabbingChanged failed");
            return grabbing;
        }

        /// <summary>
        /// Implementation of <see cref="IFingerAPI.GetFingerGrabScore(HandFinger)"/>; for details, please refer to
        /// the related documentation provided for that property.
        /// </summary>
        public float GetFingerGrabScore(HandFinger finger)
        {
            ReturnValue rv = isdk_FingerPalmGrabAPI_GetFingerGrabScore(GetHandle(), finger, out float score);
            Debug.Assert(rv != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetFingerGrabScore failed");
            return score;
        }

        /// <summary>
        /// Implementation of <see cref="IFingerAPI.Update(IHand)"/>; for details, please refer to
        /// the related documentation provided for that property.
        /// </summary>
        public void Update(IHand hand)
        {
            if (!hand.GetRootPose(out Pose rootPose))
            {
                return;
            }

            if (!hand.GetJointPosesFromWrist(out ReadOnlyHandJointPoses poses))
            {
                return;
            }

            handData_.SetData(poses, rootPose, hand.Handedness);
            ReturnValue rv = isdk_FingerPalmGrabAPI_UpdateHandData(GetHandle(), handData_);
            Debug.Assert(rv != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_UpdateHandData failed");
        }

        /// <summary>
        /// Gets the offset between the wrist and the pinch point.
        /// </summary>
        /// <returns>The offset between the wrist and the pinch point</returns>
        public Vector3 GetWristOffsetLocal()
        {
            ReturnValue rv = isdk_FingerPalmGrabAPI_GetCenterOffset(GetHandle(), out Vector3 center);
            Debug.Assert(rv != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetCenterOffset failed");
            return center;
        }

        /// <summary>
        /// Sets the value of a floating point configuration.
        /// </summary>
        /// <param name="paramId">The <see cref="PalmGrabParamID"/> of the configuration value to set</param>
        /// <param name="paramVal">The new value <paramref name="paramId"/> should be set to</param>
        public void SetConfigParamFloat(PalmGrabParamID paramId, float paramVal)
        {
            ReturnValue rc = isdk_FingerPalmGrabAPI_SetConfigParamFloat(GetHandle(), paramId, paramVal);
            Debug.Assert(rc != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_SetConfigParamFloat failed");
        }

        /// <summary>
        /// Gets the current value of <paramref name="paramId"/>.
        /// </summary>
        /// <param name="paramId">The <see cref="PalmGrabParamID"/> to get the value of</param>
        /// <returns>The current value of <paramref name="paramId"/></returns>
        public float GetConfigParamFloat(PalmGrabParamID paramId)
        {
            ReturnValue rc = isdk_FingerPalmGrabAPI_GetConfigParamFloat(GetHandle(), paramId, out float paramVal);
            Debug.Assert(rc != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetConfigParamFloat failed");
            return paramVal;
        }

        /// <summary>
        /// Sets the value of a Vector3 configuration.
        /// </summary>
        /// <param name="paramId">The <see cref="PalmGrabParamID"/> of the configuration value to set</param>
        /// <param name="paramVal">The new value <paramref name="paramId"/> should be set to</param>
        public void SetConfigParamVec3(PalmGrabParamID paramId, Vector3 paramVal)
        {
            ReturnValue rc = isdk_FingerPalmGrabAPI_SetConfigParamVec3(GetHandle(), paramId, paramVal);
            Debug.Assert(rc != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_SetConfigParamVec3 failed");
        }

        /// <summary>
        /// Gets the current value of <paramref name="paramId"/>.
        /// </summary>
        /// <param name="paramId">The <see cref="PalmGrabParamID"/> to get the value of</param>
        /// <returns>The current value of <paramref name="paramId"/></returns>
        public Vector3 GetConfigParamVec3(PalmGrabParamID paramId)
        {
            ReturnValue rc = isdk_FingerPalmGrabAPI_GetConfigParamVec3(GetHandle(), paramId, out Vector3 paramVal);
            Debug.Assert(rc != ReturnValue.Failure, "FingerPalmGrabAPI: isdk_FingerPalmGrabAPI_GetConfigParamVec3 failed");
            return paramVal;
        }
    }
}