using System.Collections.Generic;
using System;
using UnityEngine;
using UnityEngine.XR.ARFoundation;
using ETSI.ARF.OpenAPI.WorldAnalysis;
using static WorldAnalysisInterface;
using System.Collections;
using System.Linq;
using ETSI.ARF.OpenAPI.WorldStorage;
using ETSI.ARF.WorldStorage.REST;
using ;

//Implementation of the WorldAnalysis interface
public class WorldAnalysisARFoundation : MonoBehaviour, WorldAnalysisInterface
{
    /// <summary>
    /// Dictionnary of susbscription informations for poses, for each item, stored using the UUID of the item (anchor/trackable)
    /// </summary>
    private Dictionary<Guid, SubscriptionInfo> m_subscriptionsPoses;
    /// <summary>
    /// Dictionnary of relocalization informations for each item, stored using their UUID
    /// </summary>
    private Dictionary<Guid, RelocalizationInformation> m_relocalizationInformations;
    /// <summary>
    /// Dictionnary of poses for each item, stored using their UUID
    /// </summary>
    private Dictionary<Guid, ETSI.ARF.OpenAPI.WorldAnalysis.Pose> m_computedPoses;

    /// <summary>
    ///  all trackable modules
    /// </summary>
    private List<WorldAnalysisARFoundationModule>  m_trackableModules;

    /// <summary>
    /// World Storage Info object
    /// </summary>
    private WorldStorageInfo m_storageInfo;

    /// <summary>
    /// WorldStorageServer
    /// </summary>
    private ETSI.ARF.WorldStorage.WorldStorageServer m_worldStorageServer;

    /// <summary>
    /// Manages the AR Camera system
    /// </summary>
    private ARCameraManager m_arCameraManager;

    /// <summary>
    /// Is true if the ArSession.CheckAvailability method is running
    /// </summary>
    private bool isChekingAvailabilityRunning = false;

    /// <summary>
    /// Informations regarding a subscription to a pose
    /// </summary>
    public struct SubscriptionInfo
    {
        public Guid uuidTarget; // id trackable or anchor
        public float timeValidity; //The duration of the validity of the subscription
        public ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose;
        public PoseCallback callback;
    }

    #region Unity_Methods

    /// <summary>
    /// Unity Awake Method
    /// </summary>
    protected void Awake()
    {
        Instance = this;
        this.gameObject.AddComponent<WorldAnalysisARFoundationCoroutineHelper>();
        m_relocalizationInformations = new Dictionary<Guid, ETSI.ARF.OpenAPI.WorldStorage.RelocalizationInformation>();
        m_computedPoses = new Dictionary<Guid, ETSI.ARF.OpenAPI.WorldAnalysis.Pose>();
        m_subscriptionsPoses = new Dictionary<Guid, SubscriptionInfo>();

        m_trackableModules = new List<WorldAnalysisARFoundationModule>();
        WorldAnalysisARFoundationModuleImage imageModule = new WorldAnalysisARFoundationModuleImage();
        m_trackableModules.Add(imageModule);
#if UNITY_IOS
        WorldAnalysisARFoundationModuleMesh meshModule = new WorldAnalysisARFoundationModuleMesh();
        m_trackableModules.Add(meshModule);
#endif
#if ETSIARF_ARCORE_EXTENSIONS
    
#if UNITY_ANDROID
        // todo add script define symbol for using arcore extensions
        WorldAnalysisARFoundationModuleARCoreAnchor arCoreAnchorModule = new WorldAnalysisARFoundationModuleARCoreAnchor();
        m_trackableModules.Add(arCoreAnchorModule);
#else
        /// on other os : if arcore extensions is in the scene we disable it
        Google.XR.ARCoreExtensions.ARCoreExtensions arCoreExtensions = Component.FindObjectOfType<Google.XR.ARCoreExtensions.ARCoreExtensions>();
        if (arCoreExtensions != null)
        {
            arCoreExtensions.enabled = false; 
        }
#endif
#endif

        foreach (WorldAnalysisARFoundationModule module in m_trackableModules)
        {
            module.Initialize();
        } 

        m_storageInfo = FindObjectOfType<WorldStorageInfo>();
        m_worldStorageServer = m_storageInfo.worldStorageServer;
    }

    /// <summary>
    /// Unity Start Method
    /// </summary>
    IEnumerator Start()
    {
        isChekingAvailabilityRunning = true;
        ///Wait until AR session is launched
        yield return ARSession.CheckAvailability();

        if (ARSession.state == ARSessionState.NeedsInstall)
        {
            Debug.Log("ETSI ARF : AR is supported on this device, but requires installation.");
        }
        else if (ARSession.state == ARSessionState.Unsupported)
        {
            Debug.Log("ETSI ARF : AR is not supported on this device.");
        }
        else
        {
            Debug.Log("ETSI ARF : AR is supported on this device.");
        }

        m_arCameraManager = FindObjectOfType<ARCameraManager>();

        isChekingAvailabilityRunning = false;
    }

    /// <summary>
    /// Unity Update Method
    /// </summary>
    protected void Update()
    {
        if (!isChekingAvailabilityRunning)
        {
            UnityEngine.Vector3 cameraTransformPos = m_arCameraManager.transform.position;

            /// Then compute poses
            foreach (KeyValuePair<Guid, SubscriptionInfo> subPose in m_subscriptionsPoses)
            {
                RelocalizationInformation information = m_relocalizationInformations[subPose.Value.uuidTarget];
                RelocObjects firstRelocInfo = information.RelocObjects.First();
                Dictionary<Guid, RelocObjects> temp = information.RelocObjects.ToDictionary(relocObject => relocObject.Trackable.UUID, relocObject => relocObject);

                /// Trackable selection

                Guid bestTrackableCandidateID = Guid.Empty;

                List<RelocObjects> trackedCandidates = new List<RelocObjects>();
                List<RelocObjects> limitedTrackingCandidates = new List<RelocObjects>();
                List<RelocObjects> notTrackedCandidates = new List<RelocObjects>();

                //Hierarchy of types
                string[] types = { "MESH", "IMAGE_MARKER", "FIDUCIAL_MARKER", "MAP", "OTHER" }; //TODO : GEOPOSE

                //We fill in the confidence level lists.
                foreach (KeyValuePair<Guid, RelocObjects> relocObject in temp)
                {
                    WorldAnalysisARFoundationModule.TrackableInfo inf = GetPoseTrackableUUID(relocObject.Value.Trackable.UUID); // for now only image module
                    if (inf != null)
                    {
                        if (inf.confidence == 100)
                        {
                            trackedCandidates.Add(relocObject.Value);
                        }
                        else if (inf.confidence == 50)
                        {
                            limitedTrackingCandidates.Add(relocObject.Value);
                        }
                        else
                        {
                            notTrackedCandidates.Add(relocObject.Value);
                        }
                    }
                }

                //uses the types[] array indexes as key, and UUIDs as values
                List<KeyValuePair<string, Guid>> typesSortedList = new List<KeyValuePair<string, Guid>>();
                foreach (var relocObject in temp)
                {
                    typesSortedList.Add(new KeyValuePair<string, Guid>(relocObject.Value.Trackable.TrackableType.ToString(), relocObject.Value.Trackable.UUID));
                }
                var sortedItems = typesSortedList.OrderBy(item => Array.IndexOf(types, item.Key)).ToList();


                if (trackedCandidates.Count > 0)
                {
                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(trackedCandidates, typesSortedList, cameraTransformPos);
                }
                else if (limitedTrackingCandidates.Count > 0)
                {
                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(limitedTrackingCandidates, typesSortedList, cameraTransformPos);
                }
                else if (notTrackedCandidates.Count > 0)
                {
                    bestTrackableCandidateID = SelectTrackableWithTypeAndDistance(notTrackedCandidates, typesSortedList, cameraTransformPos);
                }

                if (bestTrackableCandidateID != Guid.Empty)
                {
                    /// We have found the best trackable
                    WorldAnalysisARFoundationModule.TrackableInfo info = GetPoseTrackableUUID(temp[bestTrackableCandidateID].Trackable.UUID);
                    if (info == null)
                    {
                        // For now we just ignore it : we could also send not tracked todo
                        continue;
                    }

                    Matrix4x4 tr = WorldAnalysisUnityHelper.ConvertETSIARFTransform3DToUnity(firstRelocInfo.Transform3D);
                    UnityEngine.Vector3 tr_trans = WorldAnalysisUnityHelper.ExtractTranslationFromMatrix(tr);
                    UnityEngine.Quaternion tr_rot = WorldAnalysisUnityHelper.ExtractRotationFromMatrix(tr);

                    UnityEngine.Vector3 newPos = info.position + info.rotation * tr_trans;
                    UnityEngine.Quaternion newRot = (info.rotation * tr_rot);

                    if (subPose.Value.pose.Mode == Mode_WorldAnalysis.TRACKABLES_TO_DEVICE)
                    {
                        // Inverse
                        newPos = -newPos;
                        newRot = UnityEngine.Quaternion.Inverse(newRot);
                    }

                    //Pose metadata
                    ETSI.ARF.OpenAPI.WorldAnalysis.Pose poseToUpdate = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
                    poseToUpdate.Confidence = info.confidence;
                    poseToUpdate.Uuid = subPose.Value.uuidTarget;
                    poseToUpdate.Timestamp = info.timeStamp;
                    poseToUpdate.EstimationState = info.state;
                    poseToUpdate.InstructionInfo = ""; // not supported for now
                    poseToUpdate.Mode = subPose.Value.pose.Mode;

                    //Pose value
                    VectorQuaternionPoseValue newPoseValue = new VectorQuaternionPoseValue();
                    newPoseValue.Type = PoseValueType.VECTOR_QUATERNION;
                    newPoseValue.Unit = ETSI.ARF.OpenAPI.WorldAnalysis.UnitSystem.M;

                    newPoseValue.Position = new ETSI.ARF.OpenAPI.WorldAnalysis.Vector3(newPos.x, newPos.y, newPos.z);
                    newPoseValue.Rotation = new ETSI.ARF.OpenAPI.WorldAnalysis.Quaternion(newRot.x, newRot.y, newRot.z, newRot.w);
                    poseToUpdate.Value = newPoseValue;

                    //Save pose
                    m_computedPoses[subPose.Value.uuidTarget] = poseToUpdate; // Todo : in case of multiple subscription on the same pose, we compute it multiple times (can be needed in case of multiple mode, units etc.), in that case here the last one overrides

                    if (subPose.Value.callback != null)
                    {
                        subPose.Value.callback(PoseEstimationResult.OK, poseToUpdate);
                    }
                }
            }
            ManageSubscriptionValidity();
        }
        else
        {
            Debug.Log("ETSI ARF : Starting...");
        }
    }

    #endregion

    #region Lifecycle

    /// <summary>
    /// Check the validity of all subscriptions and delete one if needed
    /// </summary>
    protected void ManageSubscriptionValidity()
    {
        List<Guid> subscriptionToDelete = new List<Guid>();
        foreach (KeyValuePair<Guid, SubscriptionInfo> sub in m_subscriptionsPoses)
        {
            float validity = sub.Value.timeValidity;
            if (Time.time > validity)
            {
                subscriptionToDelete.Add(sub.Key);
            }
        }
        foreach (Guid s in subscriptionToDelete)
        {
            Debug.Log("ETSI ARF : Subscription deleted " + s);
            m_subscriptionsPoses.Remove(s);
        }
    }

    #endregion

    #region ARF_API

    public AskFrameRateResult SetPoseEstimationFramerate(string token, PoseConfigurationTrackableType type, ETSI.ARF.OpenAPI.WorldAnalysis.EncodingInformationStructure encodingInformation, int minimumFramerate)
    {
        return AskFrameRateResult.NOT_SUPPORTED; ///We cannot set any framerate for tracking on ARKit and ARCore
    }

    public PoseEstimationResult GetLastPose(string token, Guid uuid, Mode_WorldAnalysis mode, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose)
    {
        pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
        if (!m_computedPoses.ContainsKey(uuid))
        {
            Debug.LogWarning("ETSI ARF : Ask for the pose for an uuid without associated subscription");
            return PoseEstimationResult.UNKNOWN_ID;
        }
        ETSI.ARF.OpenAPI.WorldAnalysis.Pose computedPose = m_computedPoses[uuid];
        pose = computedPose;
        if (mode != pose.Mode)
        {
            // Inverse pose
            VectorQuaternionPoseValue poseValue = (VectorQuaternionPoseValue)pose.Value;
            UnityEngine.Vector3 toUnityPos = WorldAnalysisUnityHelper.ConvertETSIVector3ToUnity(poseValue.Position);
            toUnityPos = -toUnityPos;
            UnityEngine.Quaternion toUnityRot = WorldAnalysisUnityHelper.ConvertETSIARFQuaternionToUnity(poseValue.Rotation);
            toUnityRot = UnityEngine.Quaternion.Inverse(toUnityRot);

            poseValue.Position = new ETSI.ARF.OpenAPI.WorldAnalysis.Vector3(toUnityPos.x, toUnityPos.y, toUnityPos.z);
            poseValue.Rotation = new ETSI.ARF.OpenAPI.WorldAnalysis.Quaternion(toUnityRot.x, toUnityRot.y, toUnityRot.z, toUnityRot.w);
        }
        return PoseEstimationResult.OK;
    }

    public PoseEstimationResult[] GetLastPoses(string token, Guid[] uuids, Mode_WorldAnalysis[] modes, out ETSI.ARF.OpenAPI.WorldAnalysis.Pose[] poses)
    {
        if (uuids.Length != modes.Length)
        {
            Debug.LogError("ETSI ARF : Get poses: uuids and modes array do no have the same length");
            poses = null;
            return null;
        }
        PoseEstimationResult[] resul = new PoseEstimationResult[uuids.Length];
        poses = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose[uuids.Length];
        for (int i = 0; i < uuids.Length; i++)
        {
            ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose;
            PoseEstimationResult poseResul = GetLastPose(token, uuids[i], modes[i], out pose);
            resul[i] = poseResul;
            poses[i] = pose;
        }
        return resul;
    }

    public InformationSubscriptionResult SubscribeToPose(string token, Guid uuid, Mode_WorldAnalysis mode, PoseCallback callback, ref int validity, out Guid subscriptionUUID)
    {
        RelocalizationInformation relocInfo = null;
        if (!m_relocalizationInformations.ContainsKey(uuid))
        {
            // In case of another subscription to the same uuid : we do not get the reloc information again
            List<Guid> uuids = new List<Guid>
            {
               uuid
            };

            List<Mode_WorldStorage> modes = new List<Mode_WorldStorage>
            {
                Mode_WorldStorage.TRACKABLES_TO_REQUEST
            };

            ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] currentCapabilities;
            // Filter with capabilities of current world analysis
            GetCapabilities(token, out currentCapabilities);

            ///Convert into world storage capabilities
            List<ETSI.ARF.OpenAPI.WorldStorage.Capability> capabilities = new List<ETSI.ARF.OpenAPI.WorldStorage.Capability>();
            foreach (ETSI.ARF.OpenAPI.WorldAnalysis.Capability capability in currentCapabilities)
            {
                capabilities.Add(WorldAnalysisUnityHelper.ConvertWorldAnalysisCapability(capability));
            }

            /// Collect relocalization information
            ETSI.ARF.OpenAPI.WorldStorage.Response response = RelocalizationInformationRequest.GetRelocalizationInformation(m_worldStorageServer, uuids, modes, capabilities);
            relocInfo = response.RelocInfo.First(); //Only one uuid requested
        }
        else
        {
            relocInfo = m_relocalizationInformations[uuid];
        }

        if (relocInfo == null)
        {
            subscriptionUUID = Guid.Empty;
            /// Subscription not possible
            return InformationSubscriptionResult.NONE;
        }

        return CreateSubscriptionWithRelocalizationInformation(relocInfo, uuid, mode, callback, ref validity, out subscriptionUUID);
    }

    public InformationSubscriptionResult[] SubscribeToPoses(string token, Guid[] uuids, Mode_WorldAnalysis[] modes, PoseCallback callback, ref int validity, out Guid[] subscriptionUUIDs)
    {
        InformationSubscriptionResult[] resul = new InformationSubscriptionResult[uuids.Length];
        for (int i = 0; i < uuids.Length; i++)
        {
            resul[i] = InformationSubscriptionResult.NONE; // Init with None in case we do not collect relocalization info for that id
        }
        subscriptionUUIDs = new Guid[uuids.Length];
        if (uuids.Length != modes.Length)
        {
            Debug.LogError("ETSI ARF : You tried to subscribe to poses with two arrray of different length uuids and modes");
            return resul;
        }

        ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] currentCapabilities;
        // Filter with capabilities of current world analysis
        GetCapabilities(token, out currentCapabilities);

        ///Convert into world storage capabilities
        List<ETSI.ARF.OpenAPI.WorldStorage.Capability> capabilities = new List<ETSI.ARF.OpenAPI.WorldStorage.Capability>();
        foreach (ETSI.ARF.OpenAPI.WorldAnalysis.Capability capability in currentCapabilities)
        {
            capabilities.Add(WorldAnalysisUnityHelper.ConvertWorldAnalysisCapability(capability));
        }

        List<Guid> l_uuids = new List<Guid>();
        List<Mode_WorldStorage> l_modes = new List<Mode_WorldStorage>();
        foreach (Guid uuid in uuids)
        {
            l_uuids.Add(uuid);
            l_modes.Add(Mode_WorldStorage.TRACKABLES_TO_REQUEST);
        }
        /// Collect relocalization information
        ETSI.ARF.OpenAPI.WorldStorage.Response response = RelocalizationInformationRequest.GetRelocalizationInformation(m_worldStorageServer, l_uuids, l_modes, capabilities);

        /// Check every reloc information object
        foreach (RelocalizationInformation relocInfo in response.RelocInfo)
        {
            int indexUUID = l_uuids.IndexOf(relocInfo.RequestUUID);
            Guid subscriptionUUID;
            /// Create a subscription for each found reloc information
            resul[indexUUID] = CreateSubscriptionWithRelocalizationInformation(relocInfo, relocInfo.RequestUUID, modes[indexUUID], callback, ref validity, out subscriptionUUID);
            subscriptionUUIDs[indexUUID] = subscriptionUUID;
        }
        return resul;
    }

    public InformationSubscriptionResult GetSubsription(string token, Guid subscriptionUUID, out PoseCallback callback, out Guid target, out Mode_WorldAnalysis mode, out int validity)
    {
        if (!m_subscriptionsPoses.ContainsKey(subscriptionUUID))
        {
            subscriptionUUID = Guid.Empty;
            callback = null;
            target = Guid.Empty;
            mode = Mode_WorldAnalysis.TRACKABLES_TO_DEVICE;
            validity = 0;
            return InformationSubscriptionResult.UNKNOWN_ID;
        }
        else
        {
            SubscriptionInfo subInfo = m_subscriptionsPoses[subscriptionUUID];
            callback = subInfo.callback;
            target = subInfo.uuidTarget;
            mode = subInfo.pose.Mode;
            float validitySeconds = subInfo.timeValidity - Time.time;
            validity = (int)validitySeconds * 1000;// conversion in ms
            return InformationSubscriptionResult.OK;
        }
    }

    public InformationSubscriptionResult UpdateSubscription(string token, Guid subscriptionUUID, Mode_WorldAnalysis mode, int validity, PoseCallback callback)
    {
        if (m_subscriptionsPoses.ContainsKey(subscriptionUUID))
        {
            SubscriptionInfo currrent = m_subscriptionsPoses[subscriptionUUID];
            SubscriptionInfo newSubInfo = new SubscriptionInfo();
            newSubInfo.callback = callback;
            newSubInfo.uuidTarget = currrent.uuidTarget;
            newSubInfo.pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
            newSubInfo.pose.Mode = mode;
            newSubInfo.timeValidity = Time.time + (validity / 1000.0f);
            m_subscriptionsPoses[subscriptionUUID] = newSubInfo;
            return InformationSubscriptionResult.OK;
        }
        else
        {
            return InformationSubscriptionResult.UNKNOWN_ID;
        }
    }

    public InformationSubscriptionResult UnSubscribeToPose(Guid subscriptionUUID)
    {
        if (m_subscriptionsPoses.ContainsKey(subscriptionUUID))
        {
            m_subscriptionsPoses.Remove(subscriptionUUID);
            return InformationSubscriptionResult.OK;
        }
        else
        {
            return InformationSubscriptionResult.UNKNOWN_ID;
        }
    }

    public CapabilityResult GetCapabilities(string token, out ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] capabilities)
    {
        List<ETSI.ARF.OpenAPI.WorldAnalysis.Capability> capabilitiesList = new List<ETSI.ARF.OpenAPI.WorldAnalysis.Capability>();

        foreach(WorldAnalysisARFoundationModule module in m_trackableModules)
        {
            capabilitiesList.Add(module.GetSupportedCapability());
        }

        capabilities = capabilitiesList.ToArray();
        return CapabilityResult.OK;
    }

    public CapabilityResult GetCapability(string token, Guid uuid, out bool isSupported, out ETSI.ARF.OpenAPI.WorldAnalysis.TypeWorldStorage type, out ETSI.ARF.OpenAPI.WorldAnalysis.Capability[] capability)
    {
        throw new NotImplementedException();
        /*   isSupported = false;
           type = ETSI.ARF.OpenAPI.WorldAnalysis.TypeWorldStorage.UNKNOWN;
           capability = null;
           return CapabilityResult.OK;*/
    }

    #endregion

    #region Helper 

    /// <summary>
    /// Find in all modules the tracking info of a trackable with its name
    /// </summary>
    /// <param name="uuid"></param>
    /// <returns>Tracking info if found else null</returns>
    private WorldAnalysisARFoundationModule.TrackableInfo GetPoseTrackableUUID(Guid uuid)
    {
        foreach (WorldAnalysisARFoundationModule module in m_trackableModules)
        {
            ///Improve latter : not good to check all modules
            WorldAnalysisARFoundationModule.TrackableInfo info = module.GetPoseTrackable(uuid);
            if (info !=null)
            {
                return info ;
            }
        }
        return null ;
    }

    /// <summary>
    /// To select the best trackable
    /// </summary>
    /// <param name="candidates"></param>
    /// <param name="typesSortedList"></param>
    /// <param name="cameraTransformPos"></param>
    /// <returns></returns>
    private Guid SelectTrackableWithTypeAndDistance(List<RelocObjects> candidates, List<KeyValuePair<string, Guid>> typesSortedList, UnityEngine.Vector3 cameraTransformPos)
    {
        float bestDistance = 100.0f; //valeur par d�faut  = grande distance (� changer)
        Guid selectedTrackableUUID = Guid.Empty;

        //if only one trackable is candidate
        if (candidates.Count > 0)
        {
            if (candidates.Count == 1)
            {
                selectedTrackableUUID = candidates[0].Trackable.UUID; //selected candidate
            }
            else if (candidates.Count >= 2)
            {
                //if there is more than one candidate, whe must chose using the hierarchy of the trackables
                //The list is already sorted, so whe just keep those who have the higher rank
                List<Guid> bestTypeCandidates = new List<Guid>();

                foreach (KeyValuePair<string, Guid> kvp in typesSortedList)
                {
                    if (kvp.Key == typesSortedList.First().Key)
                    {
                        bestTypeCandidates.Add(kvp.Value);
                    }
                    else
                    {
                        break;
                    }
                }

                //Then if there is only one candidate with the higher rank 
                if (bestTypeCandidates.Count == 1)
                {
                    selectedTrackableUUID = bestTypeCandidates[0]; //selected candidate
                }
                else
                {
                    // Shortest distance among those in the confidence class with the same best trackable type rank
                    foreach (Guid distanceCandidate in bestTypeCandidates)
                    {
                        foreach (RelocObjects tmp in candidates)
                        {
                            // select the corresponding candidate from the list of candidates
                            if (tmp.Trackable.UUID == distanceCandidate)
                            {
                                WorldAnalysisARFoundationModule.TrackableInfo inf = GetPoseTrackableUUID(tmp.Trackable.UUID);

                                float distance = UnityEngine.Vector3.Distance(cameraTransformPos, inf.position);

                                if (distance <= bestDistance)
                                {
                                    bestDistance = distance;
                                    selectedTrackableUUID = tmp.Trackable.UUID;
                                }
                            }
                        }
                    }
                }
            }
        }

        return selectedTrackableUUID;
    }

    private InformationSubscriptionResult CreateSubscriptionWithRelocalizationInformation(RelocalizationInformation relocInfo, Guid uuid, Mode_WorldAnalysis mode, PoseCallback callback, ref int validity, out Guid subscriptionUUID)
    {
        subscriptionUUID = Guid.Empty;
        bool hasSupport = false;
        foreach (RelocObjects relocObj in relocInfo.RelocObjects)
        {
            foreach(WorldAnalysisARFoundationModule module in m_trackableModules)
            {
                hasSupport |= module.AddTrackable(relocObj.Trackable);
            }
        }

        if (!hasSupport)
        {
            /// We did not find any trackable to track this uuid
            subscriptionUUID = Guid.Empty;
            return InformationSubscriptionResult.NOT_SUPPORTED;
        }
        // We found at least one trackable
        m_relocalizationInformations[uuid] = relocInfo;

        /// We add the subscription
        SubscriptionInfo sub = new SubscriptionInfo();
        sub.timeValidity = Time.time + (validity / 1000.0f);
        sub.pose = new ETSI.ARF.OpenAPI.WorldAnalysis.Pose();
        sub.pose.Mode = mode;
        sub.uuidTarget = uuid;
        sub.callback = callback;
        subscriptionUUID = Guid.NewGuid();
        m_subscriptionsPoses.Add(subscriptionUUID, sub);
        return InformationSubscriptionResult.OK;
    }

    #endregion
}