/******************************************************************************
 *
 * Project:  OGR
 * Purpose:  Geo-computations
 * Author:   Even Rouault, even dot rouault at spatialys.com
 *
 ******************************************************************************
 * Copyright (c) 2008-2010, Even Rouault <even dot rouault at spatialys.com>
 *
 * SPDX-License-Identifier: MIT
 ****************************************************************************/

#include "ogr_geo_utils.h"
#include "cpl_port.h"
#include "cpl_error.h"

constexpr double DEG2RAD = M_PI / 180.0;
constexpr double RAD2DEG = 1.0 / DEG2RAD;

static double OGR_Safe_acos(double x)
{
    if (x > 1)
        x = 1;
    else if (x < -1)
        x = -1;
    return acos(x);
}

/************************************************************************/
/*                      OGR_GreatCircle_Distance()                      */
/************************************************************************/

double OGR_GreatCircle_Distance(double LatA_deg, double LonA_deg,
                                double LatB_deg, double LonB_deg,
                                double dfRadius)
{
    const double cosP = cos((LonB_deg - LonA_deg) * DEG2RAD);
    const double LatA_rad = LatA_deg * DEG2RAD;
    const double LatB_rad = LatB_deg * DEG2RAD;
    const double cosa = cos(LatA_rad);
    const double sina = sin(LatA_rad);
    const double cosb = cos(LatB_rad);
    const double sinb = sin(LatB_rad);
    const double cos_angle = sina * sinb + cosa * cosb * cosP;
    return OGR_Safe_acos(cos_angle) * dfRadius;
}

/************************************************************************/
/*                      OGR_GreatCircle_InitialHeading()                */
/************************************************************************/

double OGR_GreatCircle_InitialHeading(double LatA_deg, double LonA_deg,
                                      double LatB_deg, double LonB_deg)
{
    if (fabs(LatA_deg - 90) < 1e-10 || fabs(LatB_deg + 90) < 1e-10)
    {
        return 180;
    }
    else if (fabs(LatA_deg + 90) < 1e-10 || fabs(LatB_deg - 90) < 1e-10)
    {
        return 0;
    }
    else if (fabs(fmod(LonA_deg - LonB_deg, 360.0)) < 1e-10 &&
             fabs(LatA_deg - LatB_deg) < 1e-10)
    {
        return 0;  // Arbitrary number
    }
    else if (fabs(LatA_deg) < 1e-10 && fabs(LatB_deg) < 1e-10)
    {
        return (LonB_deg > LonA_deg) ? 90.0 : 270.0;
    }
    else if (fabs(fmod(LonA_deg - LonB_deg, 360.0)) < 1e-10)
    {
        return (LatA_deg > LatB_deg) ? 180.0 : 0.0;
    }
    else
    {
        const double LatA_rad = LatA_deg * DEG2RAD;
        const double LatB_rad = LatB_deg * DEG2RAD;

        const double cos_LatA = cos(LatA_rad);
        const double sin_LatA = sin(LatA_rad);

        const double diffG = (LonA_deg - LonB_deg) * DEG2RAD;
        const double cos_diffG = cos(diffG);
        const double sin_diffG = sin(diffG);

        const double denom = sin_LatA * cos_diffG - cos_LatA * tan(LatB_rad);
        if (denom == 0.0)
        {
            // Can be the case if Lat_A = -Lat_B and abs(LonA - LonB) = 180
            return 0.0;
        }

        double track = atan(sin_diffG / denom) * RAD2DEG;

        if (denom > 0.0)
        {
            track = 180 + track;
        }
        else if (track < 0)
        {
            track = 360 + track;
        }

        return track;
    }
}

/************************************************************************/
/*                     OGR_GreatCircle_ExtendPosition()                 */
/************************************************************************/

int OGR_GreatCircle_ExtendPosition(double dfLatA_deg, double dfLonA_deg,
                                   double dfDistance, double dfHeadingInA,
                                   double dfRadius, double *pdfLatB_deg,
                                   double *pdfLonB_deg)
{
    const double dfHeadingRad = dfHeadingInA * DEG2RAD;
    const double cos_Heading = cos(dfHeadingRad);
    const double sin_Heading = sin(dfHeadingRad);

    const double dfDistanceRad = dfDistance / dfRadius;
    const double cos_Distance = cos(dfDistanceRad);
    const double sin_Distance = sin(dfDistanceRad);

    const double dfLatA_rad = dfLatA_deg * DEG2RAD;
    const double cos_complement_LatA = sin(dfLatA_rad);
    const double sin_complement_LatA = cos(dfLatA_rad);

    if (dfDistance == 0.0)
    {
        *pdfLatB_deg = dfLatA_deg;
        *pdfLonB_deg = dfLonA_deg;
        return 1;
    }

    if (fabs(dfLatA_deg) >= 90.0)
    {
        *pdfLatB_deg = dfLatA_deg;
        *pdfLonB_deg = dfLonA_deg;
        return 0;
    }

    if (fabs(sin_Heading) < 1e-8)
    {
        *pdfLonB_deg = dfLonA_deg;
        if (fabs(fmod(dfHeadingInA + 360.0, 360.0)) < 1e-8)
        {
            *pdfLatB_deg = dfLatA_deg + dfDistanceRad * RAD2DEG;
        }
        else
        {
            *pdfLatB_deg = dfLatA_deg - dfDistanceRad * RAD2DEG;
        }
        return 1;
    }

    if (fabs(cos_complement_LatA) < 1e-8 && fabs(cos_Heading) < 1e-8)
    {
        *pdfLatB_deg = dfLatA_deg;
        if (fabs(dfHeadingInA - 90.0) < 1e-8)
        {
            *pdfLonB_deg = dfLonA_deg + dfDistanceRad * RAD2DEG;
        }
        else
        {
            *pdfLonB_deg = dfLonA_deg - dfDistanceRad * RAD2DEG;
        }
        return 1;
    }

    const double cos_complement_latB =
        cos_Distance * cos_complement_LatA +
        sin_Distance * sin_complement_LatA * cos_Heading;

    const double complement_latB = OGR_Safe_acos(cos_complement_latB);

    const double dfDenomin = sin(complement_latB) * sin_complement_LatA;
    if (dfDenomin == 0.0)
        CPLDebug("OGR", "OGR_GreatCircle_Distance: dfDenomin == 0.0");
    const double Cos_dG =
        (cos_Distance - cos_complement_latB * cos_complement_LatA) / dfDenomin;
    *pdfLatB_deg = 90 - complement_latB * RAD2DEG;

    const double dG_deg = OGR_Safe_acos(Cos_dG) * RAD2DEG;

    if (sin_Heading < 0)
        *pdfLonB_deg = dfLonA_deg - dG_deg;
    else
        *pdfLonB_deg = dfLonA_deg + dG_deg;

    if (*pdfLonB_deg > 180)
        *pdfLonB_deg -= 360;
    else if (*pdfLonB_deg <= -180)
        *pdfLonB_deg += 360;

    return 1;
}