//Daniel Castaño Estrella
//daniel.c.estrella@gmail.com
//2019

using UnityEngine;
using UnityEngine.Experimental.XR.Interaction;
using UnityEngine.SpatialTracking;
using static UnityEngine.SpatialTracking.TrackedPoseDriver;

namespace ARLocation
{
    public class RtkPoseProvider : BasePoseProvider
    {
        [SerializeField]
        TrackedPoseDriver.DeviceType m_Device = TrackedPoseDriver.DeviceType.GenericXRDevice;
        [SerializeField]
        TrackedPoseDriver.TrackedPose m_PoseSource = TrackedPoseDriver.TrackedPose.ColorCamera;

        void Start()
        {
            Location.useCameraAsOrigin = true;
        }




        //public Vector3 position;
        //public Quaternion rotation;
        public override PoseDataFlags GetPoseFromProvider(out Pose output)
        {
            PoseDataFlags retDataFlags = PoseDataSource.GetDataFromSource(m_PoseSource, out output);

            if (retDataFlags == PoseDataFlags.NoData)
                return retDataFlags;

            PoseDataFlags retData = PoseDataFlags.NoData;
            if ((retDataFlags & PoseDataFlags.Rotation) > 0)
                retData |= PoseDataFlags.Rotation;


            //LocationReading locReadFirst = ARLocationManager.Instance.arLocationProvider.Provider.FirstLocation;
            //Location userLocationFirst = locReadFirst.ToLocation();

            //LocationReading locReadCurrent = ARLocationManager.Instance.arLocationProvider.Provider.CurrentLocation;
            //Location userLocationCurrent = locReadCurrent.ToLocation();


            //Vector3 displacementVector = Location.VectorFromTo(userLocationFirst, userLocationCurrent, false).toVector3();
            Vector3 displacementVector = new Vector3(0, 0, 0);

            Pose outPose = new Pose(displacementVector, output.rotation);
            output = outPose;

            //Pose localPose = TransformPoseByOriginIfNeeded(currentPose);
            //SetLocalTransform(localPose.position, localPose.rotation, poseFlags);

            retData |= PoseDataFlags.Position;
            return retData;
        }

        public override bool TryGetPoseFromProvider(out Pose output)
        {
            //resultPose = Pose.identity;
            //return false;

            output = new Pose(transform.position, transform.rotation);
            return gameObject.activeInHierarchy;
        }
    }
}