/*
    This file is part of RouteConverter.

    RouteConverter 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.

    RouteConverter 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 RouteConverter; if not, write to the Free Software
    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    Copyright (C) 2007 Christian Pesch. All Rights Reserved.
*/

package slash.navigation.util;

import slash.common.TestCase;
import slash.common.io.Transfer;

public class BearingTest extends TestCase {

    /*
    * Calculate geodesic distance (in m) between two points specified by latitude/longitude (in numeric degrees)
    * using Vincenty inverse formula for ellipsoids.
    *
    * From http://www.movable-type.co.uk/scripts/latlong-vincenty.html
    */
    private static double vincentyDistance(double lon1, double lat1, double lon2, double lat2) {
        double a = 6378137.0, b = 6356752.3142, f = 1 / 298.257223563;  // WGS-84 ellipsiod
        double L = Math.toRadians(lon2 - lon1);
        double U1 = Math.atan((1 - f) * Math.tan(Math.toRadians(lat1)));
        double U2 = Math.atan((1 - f) * Math.tan(Math.toRadians(lat2)));
        double sinU1 = Math.sin(U1), cosU1 = Math.cos(U1);
        double sinU2 = Math.sin(U2), cosU2 = Math.cos(U2);

        double lambda = L, lambdaP = 2 * Math.PI, sinSigma = 0.0, cosSigma = 0.0,
                sigma = 0.0, cosSqAlpha = 0.0, cos2SigmaM = 0.0;
        int iterLimit = 20;
        while (Math.abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0) {
            double sinLambda = Math.sin(lambda), cosLambda = Math.cos(lambda);
            sinSigma = Math.sqrt((cosU2 * sinLambda) * (cosU2 * sinLambda) +
                    (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda) * (cosU1 * sinU2 - sinU1 * cosU2 * cosLambda));
            if (sinSigma == 0.0)
                return 0.0;  // co-incident points
            cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
            sigma = Math.atan2(sinSigma, cosSigma);
            double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
            cosSqAlpha = 1 - sinAlpha * sinAlpha;
            cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
            if (Double.isNaN(cos2SigmaM))
                cos2SigmaM = 0.0;  // equatorial line: cosSqAlpha=0
            double C = f / 16.0 * cosSqAlpha * (4.0 + f * (4.0 - 3.0 * cosSqAlpha));
            lambdaP = lambda;
            lambda = L + (1.0 - C) * f * sinAlpha *
                    (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM)));
        }
        if (iterLimit == 0)
            return Double.NaN;

        double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
        double A = 1 + uSq / 16384.0 * (4096.0 + uSq * (-768.0 + uSq * (320.0 - 175.0 * uSq)));
        double B = uSq / 1024.0 * (256.0 + uSq * (-128.0 + uSq * (74.0 - 47.0 * uSq)));
        double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4.0 * (cosSigma * (-1.0 + 2.0 * cos2SigmaM * cos2SigmaM) -
                B / 6.0 * cos2SigmaM * (-3.0 + 4.0 * sinSigma * sinSigma) * (-3.0 + 4.0 * cos2SigmaM * cos2SigmaM)));
        double s = b * A * (sigma - deltaSigma);
        s = Transfer.roundMeterToMillimeterPrecision(s); // round to 1mm precision
        return s;
    }

    public void testVincentyDistance() {
        assertEquals(0.0, vincentyDistance(10.03200, 53.56948, 10.03200, 53.56948));
        assertEquals(0.1295, vincentyDistance(10.03200, 53.56948, 10.032001, 53.569481));
        assertEquals(1.2952, vincentyDistance(10.03200, 53.56948, 10.03201, 53.56949));
        assertEquals(12.9522, vincentyDistance(10.0320, 53.5694, 10.0321, 53.5695));
        assertEquals(144472.5478, vincentyDistance(01.5040, 53.0902, 00.0833, 52.1219));
    }

    public void testIdenticalPositionBearing() {
        Bearing bearing = Bearing.calculateBearing(10.03200, 53.56948, 10.03200, 53.56948);
        assertEquals(0.0, bearing.getAngle());
        assertEquals(0.0, bearing.getBackAzimuth());
        assertEquals(0.0, bearing.getDistance());
    }

    public void testModifiedLongitudes6thPositionBehindTheCommaBearing() {
        Bearing bearing = Bearing.calculateBearing(10.03200, 53.56948, 10.032001, 53.56948);
        assertEquals(90.0, bearing.getAngle());
        assertEquals(270.0000006116776, bearing.getBackAzimuth()); // should be 270.0 ?
        assertEquals(0.0662, bearing.getDistance());
    }

    public void testModifiedLatitudes6thPositionBehindTheCommaBearing() {
        Bearing bearing = Bearing.calculateBearing(10.03200, 53.56948, 10.03200, 53.569481);
        assertEquals(0.0, bearing.getAngle());
        assertEquals(180.0, bearing.getBackAzimuth());
        assertEquals(0.1112, bearing.getDistance());
    }

    public void testBearingAgainstVincentyDistance() {
        Bearing bearing = Bearing.calculateBearing(01.5040, 53.0902, 00.0833, 52.1219);
        assertEquals(222.33867425245487, bearing.getAngle());
        assertEquals(41.20989654768979, bearing.getBackAzimuth());
        assertEquals(144472.5478, bearing.getDistance());
        assertEquals(vincentyDistance(01.5040, 53.0902, 00.0833, 52.1219), bearing.getDistance());
    }
}
