/*
 * $Id: Bearing.java,v 1.24 2006/11/18 19:03:12 dmurray Exp $
 *
 * Copyright  1997-2004 Unidata Program Center/University Corporation for
 * Atmospheric Research, P.O. Box 3000, Boulder, CO 80307,
 * support@unidata.ucar.edu.
 *
 * This library is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2.1 of the License, or (at
 * your option) any later version.
 *
 * This library 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 Lesser
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation,
 * Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

package slash.navigation.util;

import slash.common.io.Transfer;

/**
 * Computes the distance, azimuth, and back azimuth between
 * two lat-lon positions on the Earth's surface. Reference ellipsoid is the WGS-84.
 *
 * Modified to return meters with millimeter precision
 *
 * @author Unidata Development Team
 * @version $Id: Bearing.java,v 1.24 2006/11/18 19:03:12 dmurray Exp $
 */

public class Bearing {

    /**
     * the azimuth, degrees, 0 = north, clockwise positive
     */
    private double azimuth;

    /**
     * the back azimuth, degrees, 0 = north, clockwise positive
     */
    private double backazimuth;

    /**
     * separation in meters
     */
    private double distance;

    /**
     * Earth radius in meters
     */
    public static final double EARTH_RADIUS = 6378137.0;

    /**
     * Some constant
     */
    private static final double F = 1.0 / 298.257223563;

    /**
     * epsilon
     */
    private static final double EPS = 0.5E-13;

    /**
     * constant R
     */
    private static final double R = 1.0 - F;

    /**
     * conversion for degrees to radians
     */
    private static final double rad = Math.toRadians(1.0);

    /**
     * conversion for radians to degrees
     */
    private static final double deg = Math.toDegrees(1.0);

    public Bearing() {
    }

    public Bearing(double azimuth, double backazimuth, double distance) {
        this.azimuth = azimuth;
        this.backazimuth = backazimuth;
        this.distance = distance;
    }

    /**
     * Get the azimuth in degrees, 0 = north, clockwise positive
     *
     * @return azimuth in degrees
     */
    public double getAngle() {
        return azimuth;
    }

    /**
     * Get the back azimuth in degrees, 0 = north, clockwise positive
     *
     * @return back azimuth in degrees
     */
    public double getBackAzimuth() {
        return backazimuth;
    }

    /**
     * Get the distance in meters
     *
     * @return distance in m
     */
    public double getDistance() {
        return distance;
    }

    /**
     * Computes distance (in meters), azimuth (degrees clockwise positive
     * from North, 0 to 360), and back azimuth (degrees clockwise positive
     * from North, 0 to 360), from latitude-longituide point pt1 to
     * latitude-longituide pt2.<p>
     * Algorithm from U.S. National Geodetic Survey, FORTRAN program "inverse,"
     * subroutine "INVER1," by L. PFEIFER and JOHN G. GERGEN.
     * See http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
     * <P>Original documentation:
     * <br>SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER T.VINCENTY
     * <br>MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
     * <br>EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
     * <br>STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE
     * </P>
     * Reference ellipsoid is the WGS-84 ellipsoid.
     * <br>See http://www.colorado.edu/geography/gcraft/notes/datum/elist.html
     * <p/>
     * Requires close to 1.4 E-5 seconds wall clock time per call
     * on a 550 MHz Pentium with Linux 7.2.
     *
     * @param longitude1 Lon of point 1
     * @param latitude1 Lat of point 1
     * @param longitude2 Lon of point 2
     * @param latitude2 Lat of point 2
     * @return a Bearing object with distance (in meters), azimuth from
     *         pt1 to pt2 (degrees, 0 = north, clockwise positive)
     */
    public static Bearing calculateBearing(double longitude1, double latitude1,
                                           double longitude2, double latitude2) {
        if ((latitude1 == latitude2) && (longitude1 == longitude2))
            return new Bearing(0, 0, 0);

        // Algorithm from National Geodetic Survey, FORTRAN program "inverse,"
        // subroutine "INVER1," by L. PFEIFER and JOHN G. GERGEN.
        // http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
        // Conversion to JAVA from FORTRAN was made with as few changes as possible
        // to avoid errors made while recasting form, and to facilitate any future
        // comparisons between the original code and the altered version in Java.
        // Original documentation:
        // SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER T.VINCENTY
        // MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
        // EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
        // STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE
        // A IS THE SEMI-MAJOR AXIS OF THE REFERENCE ELLIPSOID
        // F IS THE FLATTENING (NOT RECIPROCAL) OF THE REFERNECE ELLIPSOID
        // LATITUDES GLAT1 AND GLAT2
        // AND LONGITUDES GLON1 AND GLON2 ARE IN RADIANS POSITIVE NORTH AND EAST
        // FORWARD AZIMUTHS AT BOTH POINTS RETURNED IN RADIANS FROM NORTH
        //
        // Reference ellipsoid is the WGS-84 ellipsoid.
        // See http://www.colorado.edu/geography/gcraft/notes/datum/elist.html
        // FAZ is forward azimuth in radians from pt1 to pt2;
        // BAZ is backward azimuth from point 2 to 1;
        // S is distance in meters.
        //
        // Conversion to JAVA from FORTRAN was made with as few changes as possible
        // to avoid errors made while recasting form, and to facilitate any future
        // comparisons between the original code and the altered version in Java.
        //
        // IMPLICIT REAL*8 (A-H,O-Z)
        // COMMON/CONST/PI,RAD
        // COMMON/ELIPSOID/EARTH_RADIUS,F
        double GLAT1 = rad * latitude1;
        double GLAT2 = rad * latitude2;
        double TU1 = R * Math.sin(GLAT1) / Math.cos(GLAT1);
        double TU2 = R * Math.sin(GLAT2) / Math.cos(GLAT2);
        double CU1 = 1. / Math.sqrt(TU1 * TU1 + 1.);
        double SU1 = CU1 * TU1;
        double CU2 = 1. / Math.sqrt(TU2 * TU2 + 1.);
        double S = CU1 * CU2;
        double BAZ = S * TU2;
        double FAZ = BAZ * TU1;
        double GLON1 = rad * longitude1;
        double GLON2 = rad * longitude2;
        double X = GLON2 - GLON1;
        double D, SX, CX, SY, CY, Y, SA, C2A, CZ, E, C;
        do {
            SX = Math.sin(X);
            CX = Math.cos(X);
            TU1 = CU2 * SX;
            TU2 = BAZ - SU1 * CU2 * CX;
            SY = Math.sqrt(TU1 * TU1 + TU2 * TU2);
            CY = S * CX + FAZ;
            Y = Math.atan2(SY, CY);
            SA = S * SX / SY;
            C2A = -SA * SA + 1.;
            CZ = FAZ + FAZ;
            if (C2A > 0.) {
                CZ = -CZ / C2A + CY;
            }
            E = CZ * CZ * 2. - 1.;
            C = ((-3. * C2A + 4.) * F + 4.) * C2A * F / 16.;
            D = X;
            X = ((E * CY * C + CZ) * SY * C + Y) * SA;
            X = (1. - C) * X * F + GLON2 - GLON1;
            //IF(DABS(D-X).GT.EPS) GO TO 100
        } while (Math.abs(D - X) > EPS);

        FAZ = Math.atan2(TU1, TU2);
        BAZ = Math.atan2(CU1 * SX, BAZ * CX - SU1 * CU2) + Math.PI;
        X = Math.sqrt((1. / R / R - 1.) * C2A + 1.) + 1.;
        X = (X - 2.) / X;
        C = 1. - X;
        C = (X * X / 4. + 1.) / C;
        D = (0.375 * X * X - 1.) * X;
        X = E * CY;
        S = 1. - E - E;
        S = ((((SY * SY * 4. - 3.) * S * CZ * D / 6. - X) * D / 4. + CZ) * SY * D + Y) * C * EARTH_RADIUS * R;

        double azimuth = FAZ * deg;   // radians to degrees
        if (azimuth < 0.0) {
            azimuth += 360.0;  // reset azs from -180 to 180 to 0 to 360
        }
        double backazimuth = BAZ * deg;  // radians to degrees; already in 0 to 360 range
        return new Bearing(azimuth, backazimuth, Transfer.roundMeterToMillimeterPrecision(S));
    }
}

