/*
 * File:   wgs84.cc
 * Author: Filip Niksic, fniksic@gmail.com
 *
 * Created on January 23, 2008, 11:01 PM
 */

#include <cmath>

#include "globals.h"
#include "wgs84.h"

static inline double sgn(double x) {
    if (x > 0.0)
        return 1.0;
    else if (x < 0.0)
        return -1.0;
    else // x == 0.0 ili NaN
        return x;
}

/*
 * Funkcija geodesic() racuna udaljenost prema Vincentyjevoj inverznoj
 * formuli (algoritmu) iz clanka:
 *
 *   T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid
 *       with Application of Nested Equations", Survey Review, vol. 23, no. 176,
 *       travanj 1975., pp 88-93
 *
 * Dakle, uzima se u obzir elipsoidni oblik Zemlje.
 *
 * Ulazni parametri: WGS84 koordinate tocaka P i Q. Klasa Coords osigurava
 *     ispravnost koordinata.
 */
Real_T WGS84::geodesic(const Coords &P, const Coords &Q) {
    // Kalkulacije obavljam u double tocnosti, na kraju prepustam kompajleru
    // implicitnu konverziju u Real_T.
    
    double P_lat = P.latitude() * deg2rad;
    double P_lon = P.longitude() * deg2rad;
    double Q_lat = Q.latitude() * deg2rad;
    double Q_lon = Q.longitude() * deg2rad;
    
    // Korekcije ako smo blizu polova, preuzeto iz Matlab koda Michaela Kledera.
    if (fabs(pipola - fabs(P_lat)) < 1e-10)
        P_lat = sgn(P_lat) * fabs(pipola - 1e-10);
    if (fabs(pipola - fabs(Q_lat)) < 1e-10)
        Q_lat = sgn(Q_lat) * fabs(pipola - 1e-10);
    
    double U1 = atan((1 - flat) * tan(P_lat));
    double U2 = atan((1 - flat) * tan(Q_lat));
    
    if (P_lon < 0.0) P_lon += dvapi;
    if (Q_lon < 0.0) Q_lon += dvapi;
    
    double L = fabs(Q_lon - P_lon);
    
    if (L > pi) L = dvapi - L;
    
    double lambda = L; // Pocetna aproksimacija
    double lambdaold = 0.0;
    double sinsigma, cossigma, sigma, alpha, cos2sigmam, cosalpha, C, kobasa1, kobasa2;
    int iteracije = 0;
    
    do {
        ++iteracije;
        if (iteracije > 50) { // Puno iteracija -> antipodalne tocke
            lambda = pi;
            break;
        }
        lambdaold = lambda;
        kobasa1 = cos(U2) * sin(lambda);
        kobasa2 = cos(U1) * sin(U2) - sin(U1) * cos(U2) * cos(lambda);
        sinsigma = sqrt(kobasa1 * kobasa1 + kobasa2 * kobasa2);
        cossigma = sin(U1) * sin(U2) + cos(U1) * cos(U2) * cos(lambda);
        sigma = atan2(sinsigma, cossigma);
        alpha = asin(cos(U1) * cos(U2) * sin(lambda) / sin(sigma));
        cosalpha = cos(alpha);
        cos2sigmam = cos(sigma) - 2.0 * sin(U1) * sin(U2) / cosalpha / cosalpha;
        C = flat / 16.0 * cosalpha * cosalpha * (4.0 + flat * (4.0 - 3.0 * cosalpha * cosalpha));
        lambda = L + (1.0 - C) * flat * sin(alpha) * (sigma + C * sin(sigma) *
                (cos2sigmam + C * cos(sigma) * (-1.0 + 2.0 * cos2sigmam * cos2sigmam)));
        // Korekcija neuspjesne konvergencije u slucaju antipodalnih tocaka (Michael Kleder)
        if (lambda > pi) {
            lambda = pi;
            break;
        }
    } while (fabs(lambda - lambdaold) > eps);
    
    double u2 = cosalpha * cosalpha * (a_poluos * a_poluos - b_poluos * b_poluos) / b_poluos / b_poluos;
    double A = 1.0 + u2 / 16384.0 * (4096.0 + u2 * (-768.0 + u2 * (320.0 - 175.0 * u2)));
    double B = u2 / 1024.0 * (256.0 + u2 * (-128.0 + u2 * (74.0 - 47.0 * u2)));
    double deltasigma = B * sin(sigma) * (cos2sigmam + B / 4.0 * (cos(sigma) * (-1.0 + 2.0 * cos2sigmam * cos2sigmam)
            - B / 6.0 * cos2sigmam * (-3.0 + 4.0 * sin(sigma) * sin(sigma))*(-3.0 + 4.0 * cos2sigmam * cos2sigmam)));
    
    return b_poluos * A * (sigma - deltasigma);
}