/***************************************************************************
 *   Copyright (C) 2010 by Volker Lanz <vl@fidra.de>                       *
 *   Copyright (C) 2002 Robert Lipe, robertlipe@usa.net                    *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.          *
 ***************************************************************************/

// taken from grtcirc.c from gpsbabel source code

#include "util/gpsutil.h"

#include <errno.h>
#include <math.h>

static const double EARTH_RAD = 6378137.0;

double rad2Meters( double rads ) {
    return ( rads * EARTH_RAD );
}

double gcDist( double lat1, double lon1, double lat2, double lon2 )
{
    double res;
    double sdlat, sdlon;

    errno = 0;

    sdlat = sin((lat1 - lat2) / 2.0);
    sdlon = sin((lon1 - lon2) / 2.0);

    res = sqrt(sdlat * sdlat + cos(lat1) * cos(lat2) * sdlon * sdlon);

    if (res > 1.0) {
        res = 1.0;
    } else if (res < -1.0) {
        res = -1.0;
    }

    res = asin(res);

    if (
#if defined isnan
        /* This is a C99-ism. */
        (isnan(res)) ||
#endif
        (errno == EDOM)) { /* this should never happen: */
        errno = 0;         /* Math argument out of domain of
        function, */
        return 0;          /* or value returned is not a number */
    }

    return 2.0 * res;
}
