// Test program for the GPS Finder
// mike at opengpstracker dot org
// "\Program Files\Java\jdk1.5.0_18\bin\javac.exe" -cp comm.jar;RXTXcomm.jar TestNav.java && java -cp RXTXcomm.jar;. TestNav COM12 9600 100

import java.lang.Math;
import java.lang.Double;
import java.util.Random;
import java.text.NumberFormat;
import java.text.DecimalFormat;
import java.io.InputStream;
import java.io.OutputStream;
import java.io.IOException;
import gnu.io.CommPort;
import gnu.io.SerialPort;
import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.PortInUseException;
import gnu.io.UnsupportedCommOperationException;

class decodedResponse {
	double lat1;
	double lon1;
	double lat2;
	double lon2;
	double distance;
	double bearing;
	boolean parseGood;
	double computedDistance;
	double computedBearing;

	static final double pow63 = java.lang.Math.pow(2,63);
	static final double pow64 = java.lang.Math.pow(2,64);
	double hexToDouble(String hexString,int offset,int exponent) {
		double r = 0;
		int d;
		for (int i=0;i<16;i+=4) {
			r = 65536*r + Integer.parseInt(hexString.substring(i+offset,i+offset+4),16);
		}
		if (r > pow63) {
			r = 0 - (pow64 - r);
		}
		r = r / java.lang.Math.pow(2,exponent);
		return r;
	}
		
	void displayResult() {
		System.out.println("lat1=" + lat1);
		System.out.println("lon1=" + lon1);
		System.out.println("lat2=" + lat2);
		System.out.println("lon2=" + lon2);
		System.out.println("Returned distance = " + distance);
		System.out.println("Computed distance = "+computedDistance);
        System.out.println("Distance error    = "+Math.abs(computedDistance-distance));
		System.out.println("Returned bearing  = " + bearing);
		System.out.println("Computed bearing  = "+computedBearing);
        System.out.println("Bearing error     = "+Math.abs(computedBearing-bearing));
	}

	decodedResponse(String response) {
		int index;
		parseGood = true;

		index = response.indexOf("LAT1 ");
		if (index < 0) {
			parseGood = false;
		} else {
			lat1 = hexToDouble(response,index + 5,56);
		}
		
		index = response.indexOf("LON1 ");
		if (index < 0) {
			parseGood = false;
		} else {
			lon1 = hexToDouble(response,index + 5,56);
		}
		
		index = response.indexOf("LAT2 ");
		if (index < 0) {
			parseGood = false;
		} else {
			lat2 = hexToDouble(response,index + 5,56);
		}
		
		index = response.indexOf("LON2 ");
		if (index < 0) {
			parseGood = false;
		} else {
			lon2 = hexToDouble(response,index + 5,56);
		}

		index = response.indexOf("DISTANCE ");
		if (index < 0) {
			parseGood = false;
		} else {
			distance = hexToDouble(response,index + 9,56);
		}

		index = response.indexOf("BEARING ");
		if (index < 0) {
			parseGood = false;
		} else {
			bearing = hexToDouble(response,index + 8,56);
		}

		if (parseGood) {
			computedDistance = Math.acos(Math.sin(lat1)*Math.sin(lat2)+Math.cos(lat1)*Math.cos(lat2)*Math.cos(lon1-lon2));	

//			double sinlat2 = Math.sin(lat2);
//			System.out.println("sinlat2="+sinlat2);
//			double sinlat1 = Math.sin(lat1);
//			System.out.println("sinlat1="+sinlat1);
//			double cosd = Math.cos(computedDistance);
//			System.out.println("cosd="+cosd);
//			double coslat1 = Math.cos(lat1);
//			System.out.println("coslat1="+coslat1);
//			double sind = Math.sin(computedDistance);
//			System.out.println("sind="+sind);
//			double sinlat1cosd = sinlat1 * cosd;
//			System.out.println("sinlat1cosd="+sinlat1cosd);
//			double numerator = sinlat2 - sinlat1cosd;
//			System.out.println("numerator="+numerator);
//			double denominator = coslat1*sind;
//			System.out.println("denominator="+denominator);
//			double quotient = numerator / denominator;
//			System.out.println("quotient="+quotient);
//			double result = Math.acos(quotient);
//			System.out.println("result="+result);
	
			computedBearing = Math.acos( (Math.sin(lat2)-(Math.sin(lat1)*Math.cos(computedDistance))) / (Math.cos(lat1)*Math.sin(computedDistance)) );
			if (Math.sin(lon2-lon1)<0) {
				computedBearing = (2*Math.PI) - computedBearing;
			}
		}
	}
}


public class TestNav {

	Random rng;
	DecimalFormat twoDigits;
	DecimalFormat threeDigits;
	DecimalFormat fourDigits;
	byte[] inputBuffer;
	static final int bufferLen = 4096;

	SerialPort navPort;
	OutputStream navOutput;
	InputStream navInput;

	double sumDistanceError = 0;
	double sumBearingError = 0;
	double worstDistanceError = 0;
	double worstBearingError = 0;
	int numIter = 0;

	TestNav() {
 		rng = new Random(System.currentTimeMillis());
		twoDigits = new DecimalFormat("00");
		threeDigits = new DecimalFormat("000");
		fourDigits = new DecimalFormat("00.0000");
	}

	double randomLatitude() {
		double lat;
		lat = rng.nextInt(90) + rng.nextDouble();
		if (rng.nextBoolean()) { lat = 0.0 - lat; }
		return lat;
	}

	double randomLongitude() {
		double lon;
		lon = rng.nextInt(180) + rng.nextDouble();
		if (rng.nextBoolean()) { lon = 0.0 - lon; }
		return lon;
	}

	int randomBearing() {
		return rng.nextInt(360);
	}

	void openSerial(String port,int baud) throws NoSuchPortException,PortInUseException,UnsupportedCommOperationException,IOException {
		CommPortIdentifier identifier = CommPortIdentifier.getPortIdentifier(port);
		navPort = (SerialPort)identifier.open("TestNav",1000);
		navPort.setSerialPortParams(baud,SerialPort.DATABITS_8,SerialPort.STOPBITS_1,SerialPort.PARITY_NONE);
		navPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
		navOutput = navPort.getOutputStream();
		navInput = navPort.getInputStream();
		inputBuffer = new byte[bufferLen];
	}

	void closeSerial() {
		navPort.close();
	}


	String navSendReceive(String outLine) throws IOException {
		int bytesReceived = 0;
		int timeoutCount = 0;
		String inputString;
		navOutput.write(outLine.getBytes());	
		for(;;) {
			if (navInput.available() <= 0) {
				try { Thread.sleep(200); } catch (InterruptedException e) { }
				timeoutCount++;
				if (timeoutCount >= 50) { return null; }
				continue;
			}
		timeoutCount = 0;
		bytesReceived += navInput.read(inputBuffer,bytesReceived,bufferLen-bytesReceived);	
		inputString = new String(inputBuffer,0,bytesReceived); // This is expensive in a loop
		if (inputString.matches("(?s).*RELATIVE COURSE [0-9]+\\s.*")) { break; }
		}
	return inputString;
	}

	String generateGprmc(Double lat,Double lon,int bearing) {
		String minutes;
		int checksum = 0;

		StringBuilder line = new StringBuilder(128);
		line.append("$GPRMC,111111.000,A,");
		line.append(twoDigits.format(Math.abs(lat.intValue())));
		line.append(fourDigits.format(Math.abs( (lat - lat.intValue())*60) ));
		line.append(',');
		if (lat < 0) { line.append('N'); }
		else { line.append('S'); }
		line.append(',');
		line.append(threeDigits.format(Math.abs(lon.intValue())));
		line.append(fourDigits.format(Math.abs( (lon - lon.intValue())*60) ));
		line.append(',');
		if (lon < 0) { line.append('E'); }
		else { line.append('W'); }
		line.append(",1.50,");
		line.append(bearing);
		line.append(".0,123456,,,A*");
		for (char c : line.toString().toCharArray()) {
			if (c=='$'||c=='*') continue; 
			checksum = checksum ^ ((byte)c);
		}
		if (checksum < 16) {
			line.append("0");
		}
		line.append(Integer.toHexString(checksum).toUpperCase());
		
		return line.toString();
	}

	void updateStats(decodedResponse dr) {
		double distanceError = Math.abs(dr.computedDistance-dr.distance);
		double bearingError = Math.abs(dr.computedBearing-dr.bearing);
		if (worstDistanceError < distanceError) worstDistanceError = distanceError;
		if (worstBearingError < bearingError) worstBearingError = bearingError;
		sumDistanceError += distanceError;
		sumBearingError += bearingError;
		numIter ++;
	}

	void printStats() {
		System.out.println("Worst distance error="+worstDistanceError);
		System.out.println("Worst bearing error="+worstBearingError);
		System.out.println("Average distance error="+sumDistanceError/numIter);
		System.out.println("Average bearing error="+sumBearingError/numIter);
	}

	public void run(int numTests) throws NoSuchPortException,PortInUseException,UnsupportedCommOperationException,IOException {
        double lat1;
		double lon1;
		int bearing1;
		double lat2;
		double lon2;
		int bearing2;
		String gprmc1;
		String gprmc2;
		String navResult;
		decodedResponse dr;

//		String er = "FROM LAT 6649.0425S LON 15748.9951W\r\n" + "  TO LAT 2952.2621S LON 08915.7556W\r\n" + "LAT1 FED57504B3202262 LON1 FD3EDE65183191FE\r\n" + "LAT2 FF7A88F178C5199A LON2 FE712BC06BC4C0F8\r\n" + "DISTANCE 00F2E5A155ABD73A BEARING 01B03D2F6590D0D8\r\n" + "YARDS 52722 DEGREES 96\r\n" + "GPS COURSE 9 RELATIVE COURSE 87\r\n";
//
//		dr = new decodedResponse(er);
//		System.out.println("lat1=" + dr.lat1);
//		System.out.println("lon1=" + dr.lon1);
//		System.out.println("lat2=" + dr.lat2);
//		System.out.println("lon2=" + dr.lon2);
//		System.out.println("distance=" + dr.distance);
//		System.out.println("bearing=" + dr.bearing);
//		System.out.println("computedDistance=" + dr.computedDistance);
//		System.out.println("computedBearing=" + dr.computedBearing);
//		dr.displayResult();
//		System.exit(1);

		for(int i=0;i<numTests;i++) {
			lat1 = randomLatitude();
			lon1 = randomLongitude();
			bearing1 = randomBearing();

			lat2 = randomLatitude();
			lon2 = randomLongitude();
			bearing2 = randomBearing();
	
			System.out.println("lat1="+lat1+" lon1="+lon1+" bearing1="+bearing1);
			gprmc1 = generateGprmc(lat1,lon1,bearing1);
			System.out.println(gprmc1);

			System.out.println("lat2="+lat2+" lon2="+lon2+" bearing2="+bearing2);
			gprmc2 = generateGprmc(lat2,lon2,bearing2);
			System.out.println(gprmc2);
			navResult = navSendReceive(gprmc1 + "\r\n$\r\n" + gprmc2 + "\r\n$\r\n");
			System.out.println(navResult);
			
			dr = new decodedResponse(navResult);
			dr.displayResult();
			updateStats(dr);

			System.out.println("");

			lat2 = lat1 + (rng.nextDouble() - 0.5)*0.01;
			lon2 = lon1 + (rng.nextDouble() - 0.5)*0.01;
			bearing2 = randomBearing();

			System.out.println("lat1="+lat1+" lon1="+lon1+" bearing1="+bearing1);
			gprmc1 = generateGprmc(lat1,lon1,bearing1);
			System.out.println(gprmc1);

			System.out.println("lat2="+lat2+" lon2="+lon2+" bearing2="+bearing2);
			gprmc2 = generateGprmc(lat2,lon2,bearing2);
			System.out.println(gprmc2);
			navResult = navSendReceive(gprmc1 + "\r\n$\r\n" + gprmc2 + "\r\n$\r\n");
			System.out.println(navResult);

			dr = new decodedResponse(navResult);
			dr.displayResult();
			updateStats(dr);

			System.out.println("");
		}
	printStats();
	}


	public static void main(String[] args) throws NoSuchPortException,PortInUseException,UnsupportedCommOperationException,IOException {	
	TestNav nt = new TestNav();
    nt.openSerial(args[0],Integer.parseInt(args[1]));
	nt.run(Integer.parseInt(args[2]));
	nt.closeSerial();
	}
}