using ETSI.ARF.OpenAPI.WorldAnalysis;
using Newtonsoft.Json;
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class AnchorTrackableReferenceNode : MonoBehaviour
{
    /// <summary>
    /// Thread safe if using websockets...
    /// </summary>
    public bool isThreadSafe = false;

    /// <summary>
    /// UUID of the node or trackable
    /// </summary>
    public string _ARFNodeUUID;

    /// <summary>
    /// Validity in milliseconds for a subscription to the WA
    /// </summary>
    public int _ValiditySubscription = 100000;

    [Space(8)]
    [SerializeField]
    private bool deactivateAfterStart = true;

    [SerializeField]
    private List<GameObject> objectsToDeactivate;

    // Thread safe
    bool updateVisibility = false;
    bool updateTransformMatrix = false;
    bool updateTransformQuat = false;
    System.Guid subscriptionUUID = Guid.Empty;
    Matrix4x4 matrix;
    ETSI.ARF.OpenAPI.WorldAnalysis.VectorQuaternionPoseValue quat;

    // Start is called before the first frame update
    IEnumerator Start()
    {
        if (deactivateAfterStart)
            foreach (Transform child in transform)
            {
                child.gameObject.SetActive(false);
            }

        yield return new WaitForSeconds(5.0f);
        // wait for initialization : not perfect way
        int validity = _ValiditySubscription;

        Debug.Log("UUID to subscribe : " + _ARFNodeUUID);

        // test
        //Guid parentUUID = new Guid("6a555bf1-fceb-4a8c-a648-191087ad1b21");
        //WorldAnalysisInterface.Instance.SubscribeToPose(WorldAnalysisInstance.token + "," + parentUUID.ToString(), new System.Guid(_ARFNodeUUID), ETSI.ARF.OpenAPI.WorldAnalysis.Mode_WorldAnalysis.DEVICE_TO_TRACKABLES, PoseCallbackThreadSafe, ref validity, out subscriptionUUID);
        if (isThreadSafe) WorldAnalysisInterface.Instance.SubscribeToPose(WorldAnalysisInstance.token, new System.Guid(_ARFNodeUUID), ETSI.ARF.OpenAPI.WorldAnalysis.Mode_WorldAnalysis.DEVICE_TO_TRACKABLES, PoseCallbackThreadSafe, ref validity, out subscriptionUUID);
        else WorldAnalysisInterface.Instance.SubscribeToPose(WorldAnalysisInstance.token, new System.Guid(_ARFNodeUUID), ETSI.ARF.OpenAPI.WorldAnalysis.Mode_WorldAnalysis.DEVICE_TO_TRACKABLES, PoseCallback, ref validity, out subscriptionUUID);
    }

    private void OnDestroy()
    {
        if (subscriptionUUID != Guid.Empty) WorldAnalysisInterface.Instance.UnsubscribeFromPose(subscriptionUUID);
        subscriptionUUID = Guid.Empty;
    }

    bool nowVisible = false;
    private void FixedUpdate()
    {
        // Thread safe
        if (deactivateAfterStart)
            if (updateVisibility && !nowVisible)
            {
                updateVisibility = false;
                foreach (GameObject o in objectsToDeactivate)
                {
                    o.SetActive(true);
                }
                nowVisible = true;
            }
    }

    private void Update()
    {
        // Thread safe
        if (updateTransformMatrix)
        {
            updateTransformMatrix = false;
            transform.SetPositionAndRotation(matrix.GetPosition(), matrix.rotation);
            //transform.position = matrix.GetPosition();  // only pos
        }
        else if (updateTransformQuat)
        {
            updateTransformQuat = false;
            transform.position = WorldAnalysisUnityHelper.ConvertETSIVector3ToUnity(quat.Position);
            transform.rotation = WorldAnalysisUnityHelper.ConvertETSIARFQuaternionToUnity(quat.Rotation);
        }
    }

    /// <summary>
    /// The callback function used in the susbscription method. It instantiate a prefab or update its pose based on the current situation.
    /// </summary>
    /// <param name="result"></param> 
    /// <param name="pose"> The pose to update in the SM</param>
    public void PoseCallback(WorldAnalysisInterface.PoseEstimationResult result, ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose)
    {
        foreach (Transform child in transform)
        {
            if (!objectsToDeactivate.Contains(child.gameObject))
            {
                child.gameObject.SetActive(true);
            }
        }

        if (pose.Value.Type == ETSI.ARF.OpenAPI.WorldAnalysis.PoseValueType.MATRIX)
        {
            ETSI.ARF.OpenAPI.WorldAnalysis.MatrixPoseValue value = (ETSI.ARF.OpenAPI.WorldAnalysis.MatrixPoseValue)pose.Value;

            if (value.Transform.Count == 16)
            {
                transform.SetPositionAndRotation(matrix.GetPosition(), matrix.rotation);
            }
            else
            {
                // Ignore a wrong pose
            }
        }
        else if (pose.Value.Type == ETSI.ARF.OpenAPI.WorldAnalysis.PoseValueType.VECTOR_QUATERNION)
        {
            ETSI.ARF.OpenAPI.WorldAnalysis.VectorQuaternionPoseValue value = (ETSI.ARF.OpenAPI.WorldAnalysis.VectorQuaternionPoseValue)pose.Value;
            transform.position = WorldAnalysisUnityHelper.ConvertETSIVector3ToUnity(value.Position);
            transform.rotation = WorldAnalysisUnityHelper.ConvertETSIARFQuaternionToUnity(value.Rotation);
        }
        else
        {
            Debug.LogWarning("Pose value type not supported yet :" + pose.Value.Type); // Todo : manage other types
        }
    }

    public void PoseCallbackThreadSafe(WorldAnalysisInterface.PoseEstimationResult result, ETSI.ARF.OpenAPI.WorldAnalysis.Pose pose)
    {
        updateVisibility = true;

        if (pose.Value.Type == PoseValueType.MATRIX)
        {
            // Workaround (Thread safe)
            //MatrixPoseValue m = (MatrixPoseValue)pose.Value;
            string v = JsonConvert.SerializeObject(pose.Value);
            MatrixPoseValue value = JsonConvert.DeserializeObject<MatrixPoseValue>(v);

            if (value.Transform.Count == 16)
            {
                matrix = WorldAnalysisUnityHelper.ConvertETSIARFTransform3DToUnity(value.Transform);
                updateTransformMatrix = true;
            }
            else
            {
                // Ignore a wrong pose
            }
        }
        else if (pose.Value.Type == ETSI.ARF.OpenAPI.WorldAnalysis.PoseValueType.VECTOR_QUATERNION)
        {
            // Workaround (Thread safe)
            //VectorQuaternionPoseValue value = (ETSI.ARF.OpenAPI.WorldAnalysis.VectorQuaternionPoseValue)pose.Value;
            string v = JsonConvert.SerializeObject(pose.Value);
            quat = JsonConvert.DeserializeObject<VectorQuaternionPoseValue>(v);
            updateTransformQuat = true;
        }
        else
        {
            Debug.LogWarning("Pose value type not supported yet :" + pose.Value.Type); // Todo : manage other types
        }
    }
}