using UnityEngine;
using ARLocation;
using System;

public class ARPostionManager : MonoBehaviour
{
    // Start is called before the first frame update


    public static Vector3 GetGameObjectPositionForLocation(Transform arLocationRoot, Vector3 userPosition, Location userLocation, Location objectLocation, bool heightIsRelative)
    {

        //1. ARLocation库原本计算矢量的方法,FirstLocation作为坐标系原点(0,0,0)
        //var displacementVector = VectorFromTo(userLocation, objectLocation, objectLocation.IgnoreAltitude || heightIsRelative)
        //    .toVector3();
        //var displacementPosition = arLocationRoot ? arLocationRoot.TransformVector(displacementVector) : displacementVector;
        //return userPosition + displacementPosition + new Vector3(0, (heightIsRelative && !objectLocation.IgnoreAltitude) ? ((float)objectLocation.Altitude - userPosition.y) : 0, 0);

        //2 以ARMainCamera作为作为坐标系原点(0,0,0)的矢量计算方法
        var displacementVector = VectorFromTo(userLocation, objectLocation, objectLocation.IgnoreAltitude || heightIsRelative)
            .toVector3();
        return displacementVector;


        //3 以ARMainCamera作为作为坐标系原点(0,0,0)的矢量计算方法，计算出的世界坐标经过相机的旋转转换到相机目前的坐标系
        //Quaternion quaternionMainCamera = ARLocationManager.Instance.MainCamera.transform.rotation;
        //Vector3 displacementVectorRotated = ARLocationManager.Instance.MainCamera.transform.InverseTransformDirection(displacementVector);   //(3.66, 1.90, 9.68)
        //return displacementVectorRotated;
    }


    private static DVector3 VectorFromTo(Location l1, Location l2, bool ignoreHeight = false)
    {
        var horizontal = HorizontalVectorFromTo(l1, l2);
        var height = l2.Altitude - l1.Altitude;

        return new DVector3(horizontal.y, ignoreHeight ? 0 : height, horizontal.x);
    }

    private static DVector2 HorizontalVectorFromTo(Location l1, Location l2)
    {
#if ARGPS_USE_CUSTOM_GEO_CALC
            return ArGpsCustomGeoCalc.HorizontalVectorFromTo(l1, l2);
#else
        return VectorFromToEcefEnu(l1, l2);

#endif
    }


    private static DVector2 VectorFromToEcefEnu(Location l1, Location l2)
    {
        var rad = Math.PI / 180;
        var lat = l1.Latitude * rad;
        var lon = l1.Longitude * rad;
        var p1 = LocationToEcef(l1);
        var p2 = LocationToEcef(l2);
        var delta = p2 - p1;

        var slat = Math.Sin(lat);
        var clat = Math.Cos(lat);
        var slon = Math.Sin(lon);
        var clon = Math.Cos(lon);

        var e = -slon * delta.x + clon * delta.y;
        var n = -clon * slat * delta.x - slat * slon * delta.y + clat * delta.z;

        return new DVector2(n, e);
    }


    private static DVector3 LocationToEcef(Location l)
    {
        var rad = Math.PI / 180;

        var lat = l.Latitude * rad;
        var lon = l.Longitude * rad;
        var a = ARLocation.ARLocation.Config.EarthEquatorialRadiusInKM * 1000;
        var e2 = ARLocation.ARLocation.Config.EarthFirstEccentricitySquared;
        var N = a / Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(lat), 2));

        var x = N * Math.Cos(lat) * Math.Cos(lon);
        var y = N * Math.Cos(lat) * Math.Sin(lon);
        var z = (1 - e2) * N * Math.Sin(lat);

        return new DVector3(x, y, z);
    }



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

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


    }

    // Update is called once per frame
    void Update()
    {

    }
}