import java.io.File;
import java.io.Reader;
import java.io.FileReader;
import java.io.BufferedReader;
import java.util.ArrayList;
import java.util.Iterator;
import org.apache.commons.math3.linear.RealVector;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.DecompositionSolver;
import org.apache.commons.math3.linear.SingularValueDecomposition;

/** 
    Compute location of stationary target based on acoustic range measured from
    multiple control points. 

    This application is based on the MatLab script provided by Andy Hamilton. 
    I've written this in Java so that it can run on the Wave Glider (no MatLab 
    on Wave Glider)
*/
public class LBLSurvey {

    static int _maxIteration = 10;   // TEST AT 1 ITERATION
    final static double START_DEPTH = -1000.;

    void run(String datafileName, boolean simulateRanges, double startDepth) throws Exception {

	ArrayList<Double> eastingList = new ArrayList<Double>();
	ArrayList<Double> northingList = new ArrayList<Double>();
	ArrayList<Double> rangeList = new ArrayList<Double>();

	// Load data file
	int nPoints = 
	    loadData(datafileName, eastingList, northingList, rangeList,
		     simulateRanges, startDepth);
	
	// Convert Lists to double arrays
	double [] x = new double[nPoints];
	for (int i = 0; i < nPoints; i++) {
	    x[i] = eastingList.get(i);
	}

	double [] y = new double[nPoints];
	for (int i = 0; i < nPoints; i++) {
	    y[i] = northingList.get(i);
	}

	double r[] = new double[nPoints];
	for (int i = 0; i < nPoints; i++) {
	    r[i] = rangeList.get(i);
	}

	double z[] = new double[nPoints];
	// Control points at surface; z is always 0
	for (int i = 0; i < nPoints; i++) {
	    z[i] = 0.;
	}

	// First guess at sound velocity
	double cFirst = 1500.; 

	// First guess at target location; average of surface x and y
	double x0 = mean(x);
	double y0 = mean(y);
	double z0 = startDepth;
	double c0 = cFirst;                // Sound velocity

	RealVector f = new ArrayRealVector(nPoints);

	Array2DRowRealMatrix B = 
	    new Array2DRowRealMatrix(nPoints, 4);

	// Iterate until residuals get very small
	for (int j = 0; j < _maxIteration; j++) {

	    System.out.println("Iter #" + j);
	    double c0Squared = Math.pow(c0, 2.);

	    for (int i = 0; i < nPoints; i++) {

		// Estimated travel time
		double tij0 = 
		    Math.sqrt(Math.pow((x[i] - x0), 2.) + 
			      Math.pow((y[i] - y0), 2.) + 
			      Math.pow((z[i] - z0), 2.)) / c0;


		double denom = tij0 * c0Squared;
		B.setEntry(i, 0, (x[i] - x0) / denom);
		B.setEntry(i, 1, (y[i] - y0) / denom);
		B.setEntry(i, 2, (z[i] - z0) / denom);
		B.setEntry(i, 3, tij0 / c0);

		f.setEntry(i, tij0 - (r[i] / cFirst));
	    }

	    // DEBUG
	    /* ***
	    for (int row = 0; row < nPoints; row++) {
		for (int col = 0; col < 4; col++) {
		    System.out.print("" + B.getEntry(row, col) + " ");
		}
		System.out.println("");
	    }
	    *** */

	    DecompositionSolver solver = 
		new SingularValueDecomposition(B).getSolver();

	    RealVector solution = solver.solve(f);

	    for (int i = 0; i < solution.getDimension(); i++) {
		System.out.println("sol[" + i + "]: " + 
				   solution.getEntry(i));
	    }

	    System.out.println("");
	    //	    System.out.println("x0: " + x0 + " y0: " + y0 + 
	    //		       " z0: " + z0 + " c0: " + c0);

	    // Adjust estimated target location and sound speed by residuals in same
	    x0 += solution.getEntry(0);
	    y0 += solution.getEntry(1);
	    z0 += solution.getEntry(2);
	    c0 += solution.getEntry(3);

	    System.out.println("x=" + x0);
	    System.out.println("y=" + y0);
	    System.out.println("z=" + z0);
	    System.out.println("c=" + c0);
	}

    }


    /** Return mean value of elements */
    double mean(double[] array) throws Exception {
	double sum = 0.;
	int n = array.length;
	if (n == 0) {
	    throw new Exception("mean(): zero-length input array");
	}

	for (int i = 0; i < n; i++) {
	    sum += array[i];
	}

	return sum / n;
    }


    /** Load data from ASCII file. Each row consists of 6 comma-separated 
	values;	epochSec, targetName,easting, northing, range, utmZone
	(last three expressed in meters). 
     */
    int loadData(String datafileName, 
		  ArrayList<Double> easting, ArrayList<Double> northing, 
		 ArrayList<Double> range, boolean simulateRange, double startDepth)
	throws Exception {


	// Read x, y, range data
	BufferedReader reader = 
	    new BufferedReader(new FileReader(new File(datafileName)));

	String line = null;
	int nLine = 0;
	while ((line = reader.readLine()) != null) {

	    // Remove newline
	    line.trim();
	    if (line.startsWith("#")) {
		// Comment - skip
		continue;
	    }
	    line = line.trim();
	    String[] tokens = line.split(",");
	    if (tokens.length != 6) {
		String msg = "Line # " + nLine + ": expected 6 tokens, got " + 
		    tokens.length + ":\n" + line + "\n";

		msg += "Expected:\nepochSec,targetName,easting,northing,range,utmZone";
		throw new Exception(msg);
	    }

	    easting.add(Double.parseDouble(tokens[2]));
	    northing.add(Double.parseDouble(tokens[3]));
	    range.add(Double.parseDouble(tokens[4]));

	    nLine++;
	}

	reader.close();

	if (nLine == 0) {
	    throw new Exception("No data in file");
	}

	if (simulateRange) {
	    /* Assume target x and y equal mean easting, northing of control 
	       points. Then compute actual ranges from each control point to 
	       the simulated target, add a bit of noise, and return these ranges
	       instead of the actual measured ones.
	    */

	    double targetX = 0.; 
	    double targetY = 0.;
	    double meanR = 0.;
	    for (int i = 0; i < nLine; i++) {
		targetX += easting.get(i);
		targetY += northing.get(i);
		meanR += range.get(i);
	    }
	    targetX /= nLine;
	    targetY /= nLine;
	    meanR /= nLine;
 
	    for (int i = 0; i < nLine; i++) {
		// Compute range to simulated target
		double x = easting.get(i);
		double y = northing.get(i);
		double z = startDepth;
		double r = Math.sqrt(Math.pow((x - targetX), 2.) + 
				     Math.pow((y - targetY), 2.) + 
				     Math.pow(z, 2.));
		// Perturb ranges for test purposes
		double perturb = meanR * .001 * (2 * (Math.random() - 0.5));  
		// System.out.println("r=" + r + ", perturb=" + perturb);
		r += perturb;

		// Put perturbed range value in output range ArrayList
		range.set(i, r);
	    }
	}


	return nLine;
    }

    public static void main(String[] args) {
	boolean simulateRanges = false;
        boolean error = false;
       
	if (args.length < 1) {
	    error = true;
	}

	double startDepth = START_DEPTH;
	for (int i = 0; i < args.length-1; i++) {
	    if (args[i].equals("-simrange")) {
		simulateRanges = true;
	    }
	    else if (args[i].equals("-iter") && i < args.length-2) {
		_maxIteration = Integer.parseInt(args[++i]);
	    }
	    else if (args[i].equals("-d") && i < args.length-2) {
		startDepth = Double.parseDouble(args[++i]);
	    }
	    else {
		System.err.println("Unknown option: " + args[i]);
		error = true;
	    }
	}

	if (error) {
	    System.err.println("usage: [-simrange][-iter n][-d startDepth] dataFile\n" + 
			       "(each line in ascii dataFile is:\nepochSec,targetName,controlX,controlY,targetRange,utmZone)");
	    return;
	}
        // Datafile is last argument
        String datafile = args[args.length-1];

	LBLSurvey survey = new LBLSurvey();

	try {
	    survey.run(datafile, simulateRanges, startDepth);
	}
	catch (Exception e) {
	    System.err.println("error:\n" + e.getMessage());
	    return;
	}
    }
}