/**
 *
 */
package de.intevation.gnv.utils;

import com.vividsolutions.jts.geom.Point;

/**
 * @author Tim Englich <tim.englich@intevation.de>
 *
 */
public class DistanceCalculator {

    private final static double flattening = 1.0 / 298.257233563;
    
    private final static double earthRadius = 6378137.0 / 1000.0 ;
    
    /**
     * Constructor
     */
    public DistanceCalculator() {
    }
    
    public double calculateDistance(Point p1, Point p2){
        double resultValue = 0.0;
        
        double b1 = p1.getY();
        double b2 = p2.getY();
        
        double l1 = p1.getX();
        double l2 = p2.getX();
        
        
        double F = (b1 + b2) / 2.0;
        double G = (b1 - b2) / 2.0;
        double l = (l1 - l2) / 2.0;
        
        F = (Math.PI / 180.0) * F;
        G = (Math.PI / 180.0) * G;
        l = (Math.PI / 180.0) * l;
        
        double S = ((Math.sin(G) * Math.sin(G)) * ((Math.cos(l) * Math.cos(l))))+
                   ((Math.cos(F) * Math.cos(F)) * ((Math.sin(l) * Math.sin(l))));
        
        double C = ((Math.cos(G) * Math.cos(G)) * ((Math.cos(l) * Math.cos(l))))+
                   ((Math.sin(F) * Math.sin(F)) * ((Math.sin(l) * Math.sin(l))));
        
        double w = Math.atan(Math.sqrt((S/C)));
        
        double D = 2.0 * w * earthRadius;
        
        double R = Math.sqrt((S*C)) / w;
        
        double H1 = (3.0 * R - 1.0 ) / (2.0 * C);
        double H2 = (3.0 * R + 1.0 ) / (2.0 * S);
        
        resultValue = D * (1 + (flattening * H1 * (Math.sin(F) * Math.sin(F)) * 
                                                  (Math.cos(G) * Math.cos(G))) - 
                           (flattening * H2 * (Math.cos(F) * Math.cos(F)) * 
                                              (Math.sin(G) * Math.sin(G))));
        
        return resultValue;
    }

}