/************************************************************************************
 * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
 * All rights reserved.
 * Refer to LICENSE for terms and conditions of use.
 ***********************************************************************************/
package vnsim.map.utils;

/**
 * @author Victor-Radu
  */



import java.io.RandomAccessFile;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.util.Collections;

import vnsim.map.object.*;


public class GPSutil {

	/* more information at:
	 * http://home.online.no/~sigurdhu/Deg_formats.htm
	 * http://home.online.no/~sigurdhu/Grid_1deg.htm
	 */


	/* mLat[latitude_degree] = 1 minute of latitude in meters
	 * latitude_degree is between 0 and 90
	 */
	static double[] mLat = {
	  1842.90, 1842.91, 1842.93, 1842.96, 1842.99, 1843.05, 1843.11,
	  1843.18, 1843.26, 1843.36, 1843.46, 1843.58, 1843.70, 1843.84, 
	  1843.99, 1844.14, 1844.31, 1844.49, 1844.67, 1844.87, 1845.07, 
	  1845.28, 1845.50, 1845.73, 1845.97, 1846.21, 1846.47, 1846.73, 
	  1846.99, 1847.26, 1847.54, 1847.82, 1848.11, 1848.41, 1848.71,
	  1849.01, 1849.32, 1849.63, 1849.94, 1850.26, 1850.58, 1850.90, 
	  1851.22, 1851.55, 1851.87, 1852.20, 1852.52, 1852.85, 1853.17, 
	  1853.50, 1853.82, 1854.14, 1854.46, 1854.77, 1855.08, 1855.39, 
	  1855.70, 1856.00, 1856.29, 1856.59, 1856.87, 1857.15, 1857.43, 
	  1857.69, 1857.96, 1858.21, 1858.46, 1858.70, 1858.93, 1859.15, 
	  1859.37, 1859.57, 1859.77, 1859.96, 1860.14, 1860.31, 1860.47, 
	  1860.61, 1860.75, 1860.88, 1861.00, 1861.11, 1861.20, 1861.29, 
	  1861.36, 1861.42, 1861.47, 1861.51, 1861.54, 1861.56, 1861.57
	};

	/* mLon[latitude_degree] = 1 minute of longitude in meters
	 * latitude_degree is between 0 and 90
	 */
	static double[] mLon = {
	  1855.32, 1855.04, 1854.20, 1852.80, 1850.84, 1848.31, 1845.23, 
	  1841.59, 1837.39, 1832.63, 1827.32, 1821.46, 1815.04, 1808.08,
	  1800.57, 1792.51, 1783.91, 1774.76, 1765.08, 1754.87, 1744.12,
	  1732.84, 1721.04, 1708.71, 1695.86, 1682.50, 1668.63, 1654.25, 
	  1639.36, 1623.98, 1608.10, 1591.74, 1574.89, 1557.55, 1539.75,
	  1521.47, 1502.73, 1483.53, 1463.87, 1443.77, 1423.23, 1402.25, 
	  1380.85, 1359.02, 1336.77, 1314.11, 1291.05, 1267.60, 1243.76,
	  1219.53, 1194.93, 1169.96, 1144.63, 1118.95, 1092.93, 1066.57, 
	  1039.88, 1012.87,  985.55,  957.92,  930.00,  901.79,  873.30, 
	   844.55,  815.53,  786.26,  756.75,  727.00,  697.03,  666.84, 
	   636.44,  605.85,  575.07,  544.11,  512.99,  481.70,  450.26, 
	   418.69,  386.99,  355.16,  323.22,  291.19,  259.06,  226.86,
	   194.58,  162.24,  129.85,   97.43,   64.97,   32.49,    0.00
	};

	/* return number of Km / latitude degree at current 
	 * latitude position 
	 */
	public static double getKmDegLat(double latitude)
	{
		double km;
		int k;
		if (latitude < 0.0) {
			latitude = -latitude;
		}
		k = (int) latitude;
		if (k >= 90) {
			km = mLat[90] * 0.06; // * 60 minutes / 1000 meters
		} else {
			km = (mLat[k] + (mLat[k + 1] - mLat[k]) * (latitude - k)) * 0.06;
		}
		return km;
	}

	/* return number of Km / longitude degree at current 
	 * _latitude_ position
	 */
	public static double getKmDegLong(double latitude)
	{
		double km;
		int k;
		if (latitude < 0.0) {
			latitude = -latitude;
		}
		k = (int) latitude;
		if (k >= 90) {
			km = mLon[90] * 0.06; // * 60 minutes / 1000 meters
		} else {
			km = (mLon[k] + (mLon[k + 1] - mLon[k]) * (latitude - k)) * 0.06;
		}
		return km;
	}

	/* distance [Km] from A to B, length of segment AB */
	public static double dist(Point a, Point b, 
			   double KmDegLong, double KmDegLat)
	{
		double x, y;
		x = (a.getLongitude() - b.getLongitude()) * KmDegLong;
		y = (a.getLatitude() - b.getLatitude()) * KmDegLat;
//		System.out.println(x+" "+y +" "+KmDegLong +" "+KmDegLat);
		return java.lang.Math.sqrt(x * x + y * y);
	}

	public static double distance(Point a, Point b)
	{
	  double dAB = (a.getLatitude() + b.getLatitude()) / 2.0;
	  return dist(a, b, getKmDegLong(dAB), getKmDegLat(dAB));
	}

	/* dot product between 2 segments AB and CD */
	public static double dotProduct(Point a, Point b, 
				 double KmDegLongAB, double KmDegLatAB, 
				 Point c, Point d, 
				 double KmDegLongCD, double KmDegLatCD)
	{
		return (KmDegLongAB * KmDegLongCD * 
		  (b.getLongitude() - a.getLongitude()) * 
		  (d.getLongitude() - c.getLongitude()) + 
		  KmDegLatAB * KmDegLatCD * 
		  (b.getLatitude() - a.getLatitude()) *
		  (d.getLatitude() - c.getLatitude()));
	}

	public static double crossProduct(Point a, Point b, 
			 double KmDegLongAB, double KmDegLatAB, 
			 Point c, Point d, 
			 double KmDegLongCD, double KmDegLatCD)
	{
	return (KmDegLatAB * KmDegLongCD * 
			(b.getLatitude() - a.getLatitude()) *
			(d.getLongitude() - c.getLongitude()) - 
			KmDegLongAB * KmDegLatCD * 
			(b.getLongitude() - a.getLongitude()) * 
			(d.getLatitude() - c.getLatitude()));
	}

	/* cosine of angle between segments AB and CD */
	public static double cosAngle(Point a, Point b, Point c, Point d)
	{
		double dAB = (a.getLatitude() + b.getLatitude()) / 2.0;
		double dCD = (c.getLatitude() + d.getLatitude()) / 2.0;
		double KmDegLatAB = getKmDegLat(dAB);
		double KmDegLatCD = getKmDegLat(dCD);
		double dLAB = (a.getLongitude() + b.getLongitude()) / 2.0;
		double dLCD = (c.getLongitude() + d.getLongitude()) / 2.0;
		double KmDegLongAB = getKmDegLong(dLAB);
		double KmDegLongCD = getKmDegLong(dLCD);
		return dotProduct(a, b, KmDegLongAB, KmDegLatAB, c, d,
				KmDegLongCD, KmDegLatCD) / 
				(dist(a, b, KmDegLongAB, KmDegLatAB) * 
				dist(c, d, KmDegLongCD, KmDegLatCD));
	}

	public static double sinAngle(Point a, Point b, Point c, Point d)
	{
		double dAB = (a.getLatitude() + b.getLatitude()) / 2.0;
		double dCD = (c.getLatitude() + d.getLatitude()) / 2.0;
		double KmDegLatAB = getKmDegLat(dAB);
		double KmDegLatCD = getKmDegLat(dCD);
		double dLAB = (a.getLongitude() + b.getLongitude()) / 2.0;
		double dLCD = (c.getLongitude() + d.getLongitude()) / 2.0;
		double KmDegLongAB = getKmDegLong(dLAB);
		double KmDegLongCD = getKmDegLong(dLCD);
		return crossProduct(a, b, KmDegLongAB, KmDegLatAB, c, d,
				KmDegLongCD, KmDegLatCD) / 
				(dist(a, b, KmDegLongAB, KmDegLatAB) * 
				dist(c, d, KmDegLongCD, KmDegLatCD));
	}
	
	//returns the distace in meters between the latitude of A and the latitude of B 
	public static double getMetersLatitude(Point A,Point B)
		{
		double result=0;
		int i;
		double up, down,j,fraction;
		up=Math.floor(B.getLatitude());
		down=Math.ceil(A.getLatitude());
		for(i=(int)down;i<up;i++)
			{
			result=result+GPSutil.getKmDegLat((double)i);		
			}
		
		j=getKmDegLat(up);
		fraction=B.getLatitude()-up;
		result=result+fraction*j;
		j=getKmDegLat(down-1);
		fraction=down-1-A.getLatitude();
		result=result+fraction*j;
		result=result*1000;//returns meters
		return result;
		
		}
	
	/**
	 * translates the point p along the longitude with the specified distance
	 * 
	 * @param p
	 * @param distance
	 * @return the new latitude coordinate
	 */
	public static double addToLatitude(Point p, double distance){
		//distance < 1 km
		double latitude;
		double up, down;
		double deg;
		double py = p.getLatitude();
		
		int k = (int) p.getLatitude();
		int idx = Math.abs(k);
		
		if (idx >= 90) {
			deg = distance / (mLat[90] * 0.06); // * 60 minutes / 1000 meters
			latitude = py + deg;
		} else {
			double a = (mLat[idx+1] - mLat[idx]) / 2;
			double b = (mLat[idx] - k * (mLat[idx+1] - mLat[idx]));
			double c = -mLat[idx] * py - py * py * (mLat[idx + 1] - mLat[idx]) / 2
					+ (mLat[idx + 1] - mLat[idx]) * k * py - distance / 0.06;
			
			latitude = (- b + Math.sqrt(b * b - 4 * a * c)) / (2 * a); 
		}
		return latitude;
	}
	
	/** translates the point p along the latitude with the specified distance
	 * 
	 * @param p
	 * @param distance
	 * @return the new longitude coordinate
	 */ 
	public static double addToLongitude(Point p, double distance){
		double longitude;
		double up, down;
		
		double deg;
		double py = p.getLatitude();
		
		int k = (int) p.getLatitude();
		int idx = Math.abs(k);
		
		if (idx >= 90) {
			deg = distance / (mLon[90] * 0.06); // * 60 minutes / 1000 meters
			longitude = p.getLongitude() + deg;
		} else {
			longitude = p.getLongitude() + distance
					/ ((mLon[idx] + (mLon[idx + 1] - mLon[idx]) * Math.abs(py - k)) * 0.06);
		}
		return longitude;
	}
	
	public static PeanoKey getMaxSearchBoundPK(Point p, double distance){
		PeanoKey origpk = new PeanoKey(p);
		double longitude = GPSutil.addToLongitude(p,distance);
		double latitude = GPSutil.addToLatitude(p,distance);
		PeanoKey maxpk = new PeanoKey(new Point(longitude, latitude));
		if (distance > 0){
			maxpk.setToUpperCorner(origpk);
		}else{
			maxpk.setToLowerCorner(origpk);
		}
		return maxpk;
	}
	
	//returns the distace in meters between the longitude of A and the longitude of B
	public static double getMetersLongitude(Point A,Point B)
	{
		double result=0;
		double up, down,j,fraction;
		up=Math.floor(B.getLatitude());
		down=Math.floor(A.getLatitude());
		j=GPSutil.getKmDegLong(up)+GPSutil.getKmDegLong(down);	
		j=j/2;		
		fraction=B.getLongitude()-A.getLongitude();
		result=fraction*j;		
		result=result*1000;//returns meters
		return result;
	
	}

	
}