// WaveSolver
// Version 1.0.0

// By David Evans, 2008

// Usage:
//	Wave Answer = WaveSolver.solve( data );
//
//	double Amplitude = Answer.getAmplitude( );
//	double YShift = Answer.getShift( );
//	double Omega = Answer.getOmega( );
//	double Phi = Answer.getPhi( );
//	double y = Answer.apply( x );


// data should be in the form:
//		data[ 0 ][ 0 ] = x1
//		data[ 0 ][ 1 ] = y1
//		data[ 1 ][ 0 ] = x2
//		data[ 1 ][ 1 ] = y2
//		data[ 2 ][ 0 ] = x3
//		data[ 2 ][ 1 ] = y3
//		etc


public class WaveSolver {
	public static Wave solve( double[][] data ) {
		// Best amplitude and offset can be found mathematically (solveB) but omega and phase have to be found by trial and error
		
		// Answer tolerance - keep looping until cc and cd are within these values
		double tolc = 0.0001, told = 0.0001;
		
		double c, d;
		double cc = 0.01, cd = Math.PI * 0.02; // Initial test resolution
		double bc = 0.0, bd = 0.0; // Best values so far
		double best = Double.POSITIVE_INFINITY; // Best match so far (lower = better)
		
		Wave tmp, otmp = new Wave( 0.0, 0.0, 0.0, 0.0, Double.POSITIVE_INFINITY ); // Current & last result
		
		// Cached stuff (so solveB is faster)
		int n = data.length;
		BasicMatrix A = new BasicMatrix( n, 2 ), B = new BasicMatrix( n, 1 );
		for( int i = 0; i != n; ++ i ) {
			B.setValue( i, 0, data[ i ][ 1 ] );
			A.setValue( i, 1, 1.0 );
		}
		
		// Initial test (to get a rough idea of best phase - 0 or Pi/2)
		for( d = 0.0; d < Math.PI * 0.6; d += Math.PI * 0.5 ) {
			for( c = 0.0; c < bc + cc * 500; c += cc ) {
				tmp = solveB( data, n, c, d, A, B );
				if( tmp.getDiff( ) < best ) {
					bc = c;
					bd = d;
					best = tmp.getDiff( );
				}
				otmp = tmp;
			}
		}
		cc *= 0.2;
		cd *= 0.5;
		c = bc;
		d = bd;
		
		// Fine tuning loop continues until phi and omega are within the desired tolerance
		while( cc > tolc || cd > told ) {
			// Fine-tune phi
			for( d = Math.max( 0.0, bd - cd * 100 ); d < bd + cd * 100 && d < Math.PI * 2; d += cd ) {
				tmp = solveB( data, n, c, d, A, B );
				if( tmp.getDiff( ) < best ) {
					bd = d;
					best = tmp.getDiff( );
				}
				otmp = tmp;
			}
			cd *= 0.02;
			d = bd;
			
			// Fine-tune omega
			for( c = Math.max( 0.0, bc - cc * 200 ); c < bc + cc * 200; c += cc ) {
				tmp = solveB( data, n, c, d, A, B );
				if( tmp.getDiff( ) < best ) {
					bc = c;
					best = tmp.getDiff( );
				}
				otmp = tmp;
			}
			cc *= 0.02;
			c = bc;
		}
		
		// Answer has been found
		Wave ans = solveB( data, n, c, d, A, B );
		ans.correct( );
		return ans;
	}
	
	private static Wave solveB( double[][] data, int n, double c, double d, BasicMatrix A, BasicMatrix B ) {
		// Mathematically finds the amplitude and y-shift given a set of data, omega and phi
		
		// Populate required arrays
		for( int i = 0; i != n; ++ i )
			A.setValue( i, 0, Math.cos( c * data[ i ][ 0 ] + d ) );
		
		// Maths bit
		try {
			BasicMatrix x = A.leftInverse( ).multiply( B );
			return new Wave( x.getValue( 0, 0 ), x.getValue( 1, 0 ), c, d, A.multiply( x ).squareDiff( B ) );
		} catch( NoInverseException e ) {
			return new Wave( 0.0, 0.0, 0.0, 0.0, Double.POSITIVE_INFINITY );
		}
	}
}