using UnityEngine;
using Unity.Mathematics;
using CesiumForUnity;

public static class GeoUtils
{
    public static double3 GetLonLatHeight(CesiumGeoreference geoRef, Vector3 unityPos)
    {
        double3 ecef = geoRef.TransformUnityPositionToEarthCenteredEarthFixed(math.double3(unityPos));
        double3 llh = CesiumWgs84Ellipsoid.EarthCenteredEarthFixedToLongitudeLatitudeHeight(ecef);
        return llh;
    }

    public static double Haversine(double lat1, double lon1, double lat2, double lon2)
    {
        double R = 6371000.0; 
        double latRad1 = Mathf.Deg2Rad * (float)lat1;
        double latRad2 = Mathf.Deg2Rad * (float)lat2;
        double dLat = Mathf.Deg2Rad * (float)(lat2 - lat1);
        double dLon = Mathf.Deg2Rad * (float)(lon2 - lon1);

        double a = Mathf.Sin((float)dLat / 2) * Mathf.Sin((float)dLat / 2) +
                   Mathf.Cos((float)latRad1) * Mathf.Cos((float)latRad2) *
                   Mathf.Sin((float)dLon / 2) * Mathf.Sin((float)dLon / 2);

        double c = 2 * Mathf.Atan2(Mathf.Sqrt((float)a), Mathf.Sqrt((float)(1 - a)));
        return R * c;
    }
}