package algorithmrepository;


import jafama.FastMath;
import java.io.PrintWriter;
import java.util.Arrays;
import java.util.Random;
import oneLinersForAlgoTests.algoBinaryMatrixFile;
import oneLinersForAlgoTests.algoOneLiners;
import oneLinersForAlgoTests.algoAsciiMatrixFile;
import algorithmrepository.Algorithms.ODEFunction;

import junit.framework.AssertionFailedError;
import junit.framework.TestCase;
import static algorithmrepository.Algorithms.*;


/**
 * 
 * Test routines for algorithms. The source code for the algorithm
 * implementation is included directly in the test class so that
 * namespace conventions and naming and instantiation of classes containing the
 * implementations can be avoided.
 */
public class AlgorithmRepositoryTest extends TestCase {
	
	
    /**
     * Runs the tests from the command prompt.
     *
     * @param args Not used.
     */
    public static void main(String[] args) {
        junit.textui.TestRunner.run(AlgorithmRepositoryTest.class);
    }
    
    
    // --------------------- TESTS -----------------------------
    
    /**
     * Tests gssearch. Assumes the gssearch method is included
     * directly in the test class.
     */
    public void testgssearch() {
        
        IFunction f = new IFunction() {
            public double eval(double x) {
                return 1+(x-5)*(x-5);
            };
        };
        
        double[] xt = new double[] {-10, 0, 11 };
        double[] ft = new double[] { f.eval(xt[0]), f.eval(xt[1]), f.eval(xt[2]) };
        gssearch(xt,ft,f);
        assertEquals(0, xt[0], 1e-12);
        assertEquals(0 + GSPROPORTION*(11 - 0), xt[1], 1e-12);
        assertEquals(11, xt[2], 1e-12);
        assertEquals(f.eval(xt[0]), ft[0], 1e-12);
        assertEquals(f.eval(xt[1]), ft[1], 1e-12);
        assertEquals(f.eval(xt[2]), ft[2], 1e-12);

        xt = new double[] {-20, 0, 11 };
        ft = new double[] { f.eval(xt[0]), f.eval(xt[1]), f.eval(xt[2]) };
        gssearch(xt,ft,f);
        assertEquals(0 - GSPROPORTION*(20 - 0), xt[0], 1e-12);
        assertEquals(0, xt[1], 1e-12);
        assertEquals(11, xt[2], 1e-12);
        assertEquals(f.eval(xt[0]), ft[0], 1e-12);
        assertEquals(f.eval(xt[1]), ft[1], 1e-12);
        assertEquals(f.eval(xt[2]), ft[2], 1e-12);
                
        // Check convergence to golden ratio.
        for(int i=0;i<20;++i) gssearch(xt,ft,f);
        double GOLDENRATIO = (1+Math.sqrt(5))/2;
        assertEquals(GOLDENRATIO, xt[1] - xt[0] > xt[2] - xt[1] ? (xt[1] - xt[0])/(xt[2] - xt[1]) : (xt[2] - xt[1])/(xt[1] - xt[0]), 1e-2);
        
        
    }
    
    
    
    public void testparabolicsearch() {
        Algorithms.IFunction f = new Algorithms.IFunction() {
            public double eval(double x) {
                return 1+(x-5)*(x-5);
            };
        };
        
        double[] xt = new double[] {-10, 0, 11 };
        double[] ft = new double[] { f.eval(xt[0]), f.eval(xt[1]), f.eval(xt[2]) };
        
        parabolicsearch(xt,ft,f);
        assertEquals(-10, xt[0], 1e-12);
        assertEquals(5, xt[1], 1e-12);
        assertEquals(11, xt[2], 1e-12);
    }
    
    public void testTrapz() {
        double[] f = new double[10];
        
        double a = -2;
        double b = 4;
        double dx = 1.0/9*(b-a);
        for(int i=0;i<f.length;++i) {
            double x = a+i*dx;
            f[i] = 3*x+2;
        }
        
        assertEquals( 3.0/2*Math.pow(b, 2)+2*b - (3.0/2*Math.pow(a, 2)+2*a), trapz(f,dx), 1e-12);
        
    }
    
    public void testBisect() {
        Algorithms.IFunction f = new Algorithms.IFunction() {
            public double eval(double x) {
                return x-1;
            }
        };
        
        double[] xv = { -10, 0 , 10 };
        double[] fv = { f.eval(xv[0]), f.eval(xv[1]), f.eval(xv[2]) };
        
        bisect(xv,fv,f);
        assertEquals(0,xv[0],1e-12);
        assertEquals(10,xv[2],1e-12);
        assertEquals(5,xv[1],1e-12);
        assertEquals(-1,fv[0],1e-12);
        assertEquals(9,fv[2],1e-12);
        assertEquals(4,fv[1],1e-12);
        
        bisect(xv,fv,f);
        assertEquals(0,xv[0],1e-12);
        assertEquals(5,xv[2],1e-12);
        assertEquals(2.5,xv[1],1e-12);
        assertEquals(f.eval(xv[0]),fv[0],1e-12);
        assertEquals(f.eval(xv[2]),fv[2],1e-12);
        assertEquals(f.eval(xv[1]),fv[1],1e-12);
        
        bisect(xv,fv,f);
        assertEquals(0,xv[0],1e-12);
        assertEquals(2.5,xv[2],1e-12);
        assertEquals(1.25,xv[1],1e-12);
        assertEquals(f.eval(xv[0]),fv[0],1e-12);
        assertEquals(f.eval(xv[2]),fv[2],1e-12);
        assertEquals(f.eval(xv[1]),fv[1],1e-12);
    }
    
    
    public void testRotationMatrixVectorAngle() {
        double[] v = new double[] { 0,0,1 };
        double theta = Math.PI/2;
        double[][] rot = rotationMatrix(v,theta);
        double[] r = rotateVector(rot,v);
        assertEquals(v[0],r[0],1e-12);
        assertEquals(v[1],r[1],1e-12);
        assertEquals(v[2],r[2],1e-12);
        
        r = rotateVector(rot,new double[] {1,0,0});
        assertEquals(0,r[0],1e-12);
        assertEquals(1,r[1],1e-12);
        assertEquals(0,r[2],1e-12);
        
        r = rotateVector(rot,r);
        assertEquals(-1,r[0],1e-12);
        assertEquals(0,r[1],1e-12);
        assertEquals(0,r[2],1e-12);
        
        r = rotateVector(rot,r);
        assertEquals(0,r[0],1e-12);
        assertEquals(-1,r[1],1e-12);
        assertEquals(0,r[2],1e-12);
        
        r = rotateVector(rot,r);
        assertEquals(1,r[0],1e-12);
        assertEquals(0,r[1],1e-12);
        assertEquals(0,r[2],1e-12);
                
        v = new double[] { 0,1,0 };
        theta = Math.PI/2;
        rot = rotationMatrix(v,theta);
        r = rotateVector(rot,v);
        assertEquals(v[0],r[0],1e-12);
        assertEquals(v[1],r[1],1e-12);
        assertEquals(v[2],r[2],1e-12);
        
        double l = Math.sqrt(3*3+4*4+5*5);
        v = new double[] { 3/l,4/l,5/l };
        theta = 0.745;
        rot = rotationMatrix(v,theta);
        r = rotateVector(rot,v);
        assertEquals(v[0],r[0],1e-12);
        assertEquals(v[1],r[1],1e-12);
        assertEquals(v[2],r[2],1e-12);
        
        rot = rotationMatrix(v,-theta);
        r = rotateVector(rot,r);
        assertEquals(v[0],r[0],1e-12);
        assertEquals(v[1],r[1],1e-12);
        assertEquals(v[2],r[2],1e-12);
    }
    
    public void testRotationMatrixAngles() {
        double[] v = new double[] { 1,0,0 };
        double theta = Math.PI/2;
        double[][] rot1 = rotationMatrix(v,theta);
        double[][] rot2 = rotationMatrix(theta,0,0);
        //double[] r = new double[] { 3,4.4,-1.9 };
        double[] r = new double[] { 0,1,0 };
        double[] r1 = rotateVector(rot1,r);
        double[] r2 = rotateVector(rot2,r);
        assertEquals(r1[0],r2[0],1e-12);
        assertEquals(r1[1],r2[1],1e-12);
        assertEquals(r1[2],r2[2],1e-12);

        v = new double[] { 0,1,0 };
        theta = -0.4545;
        rot1 = rotationMatrix(v,theta);
        rot2 = rotationMatrix(0,theta,0);
        r = new double[] { 3,4.4,-1.9 };
        r1 = rotateVector(rot1,r);
        r2 = rotateVector(rot2,r);
        assertEquals(r1[0],r2[0],1e-12);
        assertEquals(r1[1],r2[1],1e-12);
        assertEquals(r1[2],r2[2],1e-12);


        v = new double[] { 0,0,1 };
        theta = 4.545;
        rot1 = rotationMatrix(v,theta);
        rot2 = rotationMatrix(0,0,theta);
        r = new double[] { -3, 4.4, 2.2 };
        r1 = rotateVector(rot1,r);
        r2 = rotateVector(rot2,r);
        assertEquals(r1[0],r2[0],1e-12);
        assertEquals(r1[1],r2[1],1e-12);
        assertEquals(r1[2],r2[2],1e-12);
        
    }

    
    public void testRotationMatrixAxes() {
        double[][] axes = new double[][] { {0,1,0 },
                                           {-1,0,0 },
                                           {0,0,1 }};
        double[] v = new double[] { 0,0,1 };
        double theta = Math.PI/2;
        double[][] rot1 = rotationMatrix(v,theta);
        double[][] rot2 = rotationMatrix(axes);
        double[] r = new double[] { 0,1,0 };
        double[] r1 = rotateVector(rot1,r);
        double[] r2 = rotateVector(rot2,r);
        assertEquals(r1[0],r2[0],1e-12);
        assertEquals(r1[1],r2[1],1e-12);
        assertEquals(r1[2],r2[2],1e-12);
        
        r = new double[] { 7,-8, 3.3 };
        r1 = rotateVector(rot1,r);
        r2 = rotateVector(rot2,r);
        assertEquals(r1[0],r2[0],1e-12);
        assertEquals(r1[1],r2[1],1e-12);
        assertEquals(r1[2],r2[2],1e-12);
    }
 
    
    public void testCoordinateTransform() {
        double[][] axes = new double[][] { {0,1,0 },
                        {-1,0,0 },
                        {0,0,1 }};
        double[][] trans = transformationMatrix(axes);
        double[] origo = new double[] { 1,1,1 };
        double[] r = new double[] { 4,5,7 };
        double[] rl = coordinateTransform(trans, origo, r);
        assertEquals(4,rl[0],1e-12);
        assertEquals(-3,rl[1],1e-12);
        assertEquals(6,rl[2],1e-12);
        
        axes = new double[][] { {1,0,0 },
                                {0,1,0 },
                                {0,0,1 }};
        trans = transformationMatrix(axes);
        origo = new double[] { 1,1,1 };
        r = new double[] { 4,5,7 };
        rl = coordinateTransform(trans, origo, r);
        assertEquals(3,rl[0],1e-12);
        assertEquals(4,rl[1],1e-12);
        assertEquals(6,rl[2],1e-12);
        
    }
    
  
    public void testInv3x3() {
        double[][] m1 = new double[][] { { 1,2,3 },
                                        { 4,-2,7},
                                        { -0.4, 3, 2}, };
        
        double[][] m2 = inv3x3(inv3x3(m1));
        
        for(int i=0;i<3;++i) {
            for(int j=0;j<3;++j) {
                assertEquals(m1[i][j], m2[i][j], 1e-12);
            }
        }
                        
    }
    
    public void testDet3x3() {
        double[][] m = new double[][] { { 1,2,3 },
                { 4,-2,7},
                { -0.4, 3, 2}, };
        double d1 = det3x3_direct(m);
        double d2 = det3x3(m);
        assertEquals(d1,d2,1e-12);
        
    }
    /**
     * Calculates the determinant of a 3x3 matrix directly from
     * the definition.
     *
     * @param m The 3x3 matrix whose determinant is sought.
     * @return The determinant of the given 3x3 matrix.
     */
    protected double det3x3_direct(double[][] m) {
        return  m[0][0]*m[1][1]*m[2][2] +
                m[0][1]*m[1][2]*m[2][0] +
                m[0][2]*m[1][0]*m[2][1] -
                m[0][0]*m[1][2]*m[2][1] -
                m[0][1]*m[1][0]*m[2][2] -
                m[0][2]*m[1][1]*m[2][0];
    }
    
    public void testLinearInterpolation1DNonMonotonical() {
        int nump = 10;
        double[] xp = new double[nump];
        double[] yp = new double[nump];
        
        double a = 4;
        double k = 3;
        for(int i=0;i<nump;++i) {
            xp[i] = i;
            yp[i] = a + k*i;
        }
        
        int num = 100;
        double[] x = new double[num];
        for(int i=0;i<num;++i) {
            x[i] = i/10.0;
        }
        
        // Shuffle
        double v = xp[0]; xp[0] = xp[9]; xp[9] = v;
        v = yp[0]; yp[0] = yp[9]; yp[9] = v;
        v = xp[3]; xp[3] = xp[5]; xp[5] = v;
        v = yp[3]; yp[3] = yp[5]; yp[5] = v;
        LinearInterpolation1D ip = new LinearInterpolation1D(xp,yp);    
        assertTrue(ip.isEquallySpaced());
        double[] y = ip.eval(x);
        for(int i=0;i<num;++i) {
            assertEquals(a+k*x[i], y[i], 1e-12);
        }
        
    }
    
    
    /**
     * Tests linearInterpolation1D().
     * 
     * FIXME: Needs non-equally spaced tests.
     */
    public void testLinearInterpolation1D() {
    	int nump = 10;
    	double[] xp = new double[nump];
    	double[] yp = new double[nump];
    	
    	double a = 4;
    	double k = 3;
    	for(int i=0;i<nump;++i) {
    		xp[i] = i;
    		yp[i] = a + k*i;
    	}
    	
    	int num = 100;
    	double[] x = new double[num];
    	for(int i=0;i<num;++i) {
    		x[i] = i/10.0;
    	}
    	
    	LinearInterpolation1D ip = new LinearInterpolation1D(xp,yp);
        assertTrue(ip.isEquallySpaced());
    	double[] y = ip.eval(x);
    	for(int i=0;i<num;++i) {
    		assertEquals(a+k*x[i], y[i], 1e-12);
    	}
    	
    	a = -7;;
    	k = -2.8;
    	for(int i=0;i<nump;++i) {
    		xp[i] = i;
    		yp[i] = a + k*i;
    	}
    	
    	ip = new LinearInterpolation1D(xp,yp);
        assertTrue(ip.isEquallySpaced());
    	y = ip.eval(x);
    	for(int i=0;i<num;++i) {
    		assertEquals(a+k*x[i], y[i], 1e-12);
    	}
    	
    	// Extrapolation
       	x = new double[num];
    	for(int i=0;i<num;++i) {
    		x[i] = -nump+i*(3.0*nump)/num;
    	}

       	y = ip.eval(x);
    	for(int i=0;i<num;++i) {
    		assertEquals(a+k*x[i], y[i], 1e-12);
    	}
    	
    	// On the points
    	y = ip.eval(xp);
       	for(int i=0;i<nump;++i) {
    		assertEquals(yp[i], y[i], 1e-12);
    	}
       	
       	// Extrapolation on nonlinearly related points
        xp = new double[] { 1,2,3 };
        yp = new double[] { 1,2,8 };
        ip = new LinearInterpolation1D(xp,yp);
        assertEquals(8.0+6.0, ip.eval(4));
        
        // Extrapolation on non-uniform nonlinearly related points
        xp = new double[] { 1,2,5 };
        yp = new double[] { 1,2,8 };
        ip = new LinearInterpolation1D(xp,yp);
        assertEquals(8.0+2.0, ip.eval(6));
       	
    }
    
    /** Really aggressive flooring and jitter tests for 'regular spaced'
     * 1D interpolators */
    public void testInterpolatorFlooringSpeed(){
        int nTests = 3000000;
        int nSets = 1;
        Random randGen = new Random();

        //for Math.floor(), FastMath.floor() and (int) tests with speed check
        double x0 = -3.2, x1 = 6.7;
        double tst0 = -10, tst1 = 10;
        double x[] = new double[10];
        double y[] = new double[10];
        for(int i=0; i < x.length; i++){
            x[i] = x0 + i * (x1 - x0) / (x.length-1);
            y[i] = (-1.0 + 2.0 * randGen.nextDouble()) * Math.pow(10, -3.0 + 6*randGen.nextDouble());

        }

        double xTest[] = new double[nTests];
        for(int i=0; i < nTests; i++){
            double f = randGen.nextDouble() * 1.2;
            if(f > 1.1){ //some of the time, hit a knot exactly
                xTest[i] = x[(int)((f-1.1)/0.1 * x.length)];

            }else if(f > 1.0){ //some of the time, hit a knot nearly exactly
                xTest[i] = x[(int)((f-1)/0.1 * x.length)] + Math.pow(10, -randGen.nextInt(20));
            }else{
                xTest[i] = tst0 + f * (tst1 - tst0);
            }
        }
        
        LinearInterpolation1D interpA = new LinearInterpolation1D(x, y, Interpolation1D.EXTRAPOLATE_LINEAR, 0);
        LinearInterpolation1D interpB = new LinearInterpolation1D(x, y, Interpolation1D.EXTRAPOLATE_CONSTANT_VALUE, 5.555);
        LinearInterpolation1D interpC = new LinearInterpolation1D(x, y, Interpolation1D.EXTRAPOLATE_CONSTANT_END_KNOT, 0);

        double yTestA[] = null;
        double yTestB[] = null;
        double yTestC[] = null;
        long t0 = System.currentTimeMillis();
        for(int i=0; i < nSets; i++){
            yTestA = interpA.eval(xTest);
            yTestB = interpB.eval(xTest);
            yTestC = interpC.eval(xTest);
        }
        double t = System.currentTimeMillis() - t0;

        //algoBinaryMatrixFile.mustWrite("/tmp/a.bin", new double[][]{ x, y }, true);
        //algoBinaryMatrixFile.mustWrite("/tmp/b.bin", new double[][]{ xTest, yTest }, true);

        for(int i=0; i < nTests; i++){
            int idx = algoOneLiners.getNearestLowerIndex(x, xTest[i]);
            if(idx >= (x.length-1))
                idx = x.length-2;

            double yGoodA = y[idx] + (xTest[i] - x[idx]) / (x[idx+1] - x[idx]) * (y[idx+1] - y[idx]);
            double yGoodB, yGoodC;
            if(xTest[i] < x[0]){
                yGoodB = 5.555;
                yGoodC = y[0];
            }else if(xTest[i] >= x[x.length-1]){
                //The behaviour here is not /quite/ what I wanted but will have to do:
                // Really, you would want x = x[n-1] to give y[n-1] even in the non-extrapolating modes
                // However, the layout of the code in LinearInterpolation1D makes this very difficult to achieve
                
                yGoodB = 5.555;
                yGoodC = y[x.length-1];
            }else{
                yGoodB = yGoodA;
                yGoodC = yGoodA;
            }
            try{
            assertEquals(yGoodA, yTestA[i], 1e-12);
            assertEquals(yGoodB, yTestB[i], 1e-12);
            assertEquals(yGoodC, yTestC[i], 1e-12);
            }catch(AssertionFailedError e){
                e.printStackTrace();
                interpA.eval(xTest[i]);
                interpB.eval(xTest[i]);
                interpC.eval(xTest[i]);
            }
        }
        System.out.println("Linear interp time = " + t + " ms = " + ((double)t / (nTests*nSets)) + " ms per point");

        // on NervousEnergy Jul 2011:
        // nTests = 3000000
        // Math.floor(): Linear interp time = 299.0 ms = 9.966666666666667E-5 ms per point
        // FastMath.floor(): Linear interp time = 470.0 ms = 1.5666666666666666E-4 ms per point
        // (int): Linear interp time = 214.0 ms = 7.133333333333334E-5 ms per point

    }
    
    public void testCubicInterpolation1DNonMonotonical() {
        int nump = 10;
        double[] xp = new double[nump];
        double[] yp = new double[nump];
                        
        double k = 3;
        double a = (double)nump/2;
        for(int i=0;i<nump;++i) {
            xp[i] = i;
            yp[i] = k*(i-a)*(i-a);
        }
    
        // Shuffle
        double v = xp[0]; xp[0] = xp[9]; xp[9] = v;
        v = yp[0]; yp[0] = yp[9]; yp[9] = v;
        v = xp[3]; xp[3] = xp[5]; xp[5] = v;
        v = yp[3]; yp[3] = yp[5]; yp[5] = v;
        
        // Test values on the points
        CubicInterpolation1D ip = new CubicInterpolation1D(xp,yp);
        assertTrue(ip.isEquallySpaced());
        double[] y = ip.eval(xp);
        for(int i=0;i<nump;++i) {
            assertEquals(yp[i],y[i],1e-23);
        }
                
    }
    

    /**
     * Tests cubicInterpolation1D()
     *
     */
    public void testCubicInterpolation1D() {
    	int nump = 10;
    	double[] xp = new double[nump];
    	double[] yp = new double[nump];
    	    	    	
    	double k = 3;
    	double a = (double)nump/2;
    	for(int i=0;i<nump;++i) {
    		xp[i] = i;
    		yp[i] = k*(i-a)*(i-a);
    	}
    	
    	// Test values on the points
    	CubicInterpolation1D ip = new CubicInterpolation1D(xp,yp);
        assertTrue(ip.isEquallySpaced());
    	double[] y = ip.eval(xp);
    	for(int i=0;i<nump;++i) {
    		assertEquals(yp[i],y[i],1e-23);
    	}
    	
    	// Test derivatives on the points
    	double[] der = new double[y.length];
    	ip.eval(xp,der);
    	for(int i=1;i<nump-1;++i) {
    		assertEquals(2*k*(i-a), der[i],1e-23);
    	}
    	assertEquals(k*(1-a)*(1-a)-k*a*a, der[0],1e-23);
    	assertEquals(k*((nump-1)-a)*((nump-1)-a) - k*((nump-2)-a)*((nump-2)-a), der[nump-1],1e-23);
    	
    	
    	// Test derivatives on the points
    	double[] df = new double[nump-2];
    	double[] xright = new double[nump-2];
    	double[] xleft = new double[nump-2];
    	double h = 1e-6;
    	for(int i=0;i<df.length;++i) {
    		df[i] = 2*k*(i+1-a);
    		xleft[i] = xp[i+1]-h;
    		xright[i] = xp[i+1]+h;
    	}
    	double[] fleft = ip.eval(xleft);
    	double[] fright = ip.eval(xright);
    	
    	for(int i=0;i<df.length;++i) {
    		assertEquals(df[i], (fright[i]-fleft[i])/(2*h),1e-5);
    	}
    	
    	// Test continuity of derivative
    	for(int i=0;i<df.length;++i) {
    		df[i] = 2*k*(i+1-a);
    		xleft[i] = xp[i+1]-3*h;
    		xright[i] = xp[i+1]-h;
    	}
    	fleft = ip.eval(xleft);
    	fright = ip.eval(xright);
    	
    	double[] dfl = new double[nump-2];
    	for(int i=0;i<dfl.length;++i) {
    		dfl[i] = (fright[i]-fleft[i])/(2*h);
    	}
    	
    	
    	for(int i=0;i<df.length;++i) {
    		df[i] = 2*k*(i+1-a);
    		xleft[i] = xp[i+1]+h;
    		xright[i] = xp[i+1]+3*h;
    	}
    	fleft = ip.eval(xleft);
    	fright = ip.eval(xright);
    	
    	double[] dfh = new double[nump-2];
    	for(int i=0;i<dfl.length;++i) {
    		dfh[i] = (fright[i]-fleft[i])/(2*h);
    	}
    	    	
    	for(int i=0;i<dfl.length;++i) {
    		assertEquals(dfl[i],dfh[i],1e-4);
    	}
    	
    }

    public void testCubicInterpolation1DIrregular() {
        String outPath = System.getProperty("java.io.tmpdir") + "/algoTests/cubicInterp1D";
        
        double x[] = algoOneLiners.linSpace(0, 1, 0.02);
        int n = x.length;
        double y[] = new double[n];
        
        double waveLen = 0.2;
        
        for(int i=0; i < n; i++){
            x[i] = Math.sqrt(x[i]);
            y[i] = Math.sin(2*Math.PI*x[i]/waveLen);
            
        }
        
        algoAsciiMatrixFile.mustWrite(outPath + "/sineKnots.txt", new double[][]{ x, y }, true);
        
        
        CubicInterpolation1D interp = new CubicInterpolation1D(x, y);
        
        double evalX[] = algoOneLiners.linSpace(0, 1, 0.001);
        
        algoAsciiMatrixFile.mustWrite(outPath + "/sineEval.txt", new double[][]{ evalX, interp.eval(evalX) }, true);
        
        
    }
    
    
    /**
     * Tests the linearInterpolation2D() methods.
     *
     */
    public void testLinearInterpolation2D() {
    	double minx = -1;
    	double maxx = 2;
    	double miny = 0;
    	double maxy = 4;
    	int numx = 10;
    	int numy = 10;
    	double dx = (maxx-minx)/(numx-1);
    	double dy = (maxy-miny)/(numy-1);
    	double[][] f = new double[numx][numy];
    	double[][] dfdx = new double[numx][numy];
    	double[][] dfdy = new double[numx][numy];
    	double[][] d2fdxdy = new double[numx][numy];
    	double x,y;
    	double[] xp = new double[numx], yp = new double[numy];
    	
    	for(int i=0;i<numx;++i) {
    		x = minx + i*dx;
    		xp[i] = x;
    		for(int j=0;j<numy;++j) {
    			y = miny + j*dy;
    			yp[j] = y;
    			f[i][j] = Math.exp(-x*x-y*y)+(x-1)*(x-1)+(y-2)*(y-2);
    			dfdx[i][j] = -2*x*Math.exp(-x*x-y*y)+2*(x-1);
    			dfdy[i][j] = -2*y*Math.exp(-x*x-y*y)+2*(y-2);
    			d2fdxdy[i][j] = 4*x*y*Math.exp(-x*x-y*y);
    		}
    	}
    	
    	double[] vret = null;
    	double[] fp = new double[] { f[0][0], f[1][0], f[1][1], f[0][1] };
    	double[] dfdxp = new double[] { dfdx[0][0], dfdx[1][0], dfdx[1][1], dfdx[0][1] };
    	double[] dfdyp = new double[] { dfdy[0][0], dfdy[1][0], dfdy[1][1], dfdy[0][1] };
    	double[] d2fdxdyp = new double[] { d2fdxdy[0][0], d2fdxdy[1][0], d2fdxdy[1][1], d2fdxdy[0][1] };
    	
    	x = minx; y = miny;
    	double ret = LinearInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, x, y);
    	assertEquals(f[0][0], ret, 1e-12);
    	
    	x = minx+dx; y = miny;
    	ret = LinearInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, x, y);
    	assertEquals(f[1][0], ret, 1e-12);
    	
    	x = minx+dx; y = miny+dy;
    	ret = LinearInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, x, y);
    	assertEquals(f[1][1], ret, 1e-12);
    	
    	x = minx; y = miny+dy;
    	ret = LinearInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, x, y);
    	assertEquals(f[0][1], ret, 1e-12);
    	
    	
    	LinearInterpolation2D ip = new LinearInterpolation2D(xp,yp,f);
    	double[] xv = new double[numx*numy];
    	double[] yv = new double[numx*numy];
    	int l = 0;
    	for(int i=0;i<numx;++i) {
    		for(int j=0;j<numy;++j,++l) {
    			xv[l] = xp[i];
    			yv[l] = yp[j];
    		}
    	}
    	double[] fv = ip.eval(xv, yv);
    	l = 0;
    	for(int i=0;i<numx;++i) {
    		for(int j=0;j<numy;++j,++l) {
    			assertEquals(f[i][j], fv[l],1e-12);
    		}
    	}
    	
    }
    
    /**
     * Tests the gridDerivatives() method.
     *
     */
    public void testGridDerivatives() {
    	double[] der = new double[3];
    	double minx = -1;
    	double maxx = 2;
    	double miny = 2;
    	double maxy = 4;
    	int numx = 250;
    	int numy = 253;
    	double dx = (maxx-minx)/(numx-1);
    	double dy = (maxy-miny)/(numy-1);
    	double[][] f = new double[numx][numy];
    	double[][] dfdx = new double[numx][numy];
    	double[][] dfdy = new double[numx][numy];
    	double[][] d2fdxdy = new double[numx][numy];
    	double x,y;
    	double[] xp = new double[numx], yp = new double[numy];
    	
    	for(int i=0;i<numx;++i) {
    		x = minx + i*dx;
    		xp[i] = x;
    		for(int j=0;j<numy;++j) {
    			y = miny + j*dy;
    			yp[j] = y;
    			f[i][j] = Math.exp(-x*x-y*y)+(x-1)*(x-1)*y+(y-5)*(y-5)*(y-5);
    			dfdx[i][j] = -2*x*Math.exp(-x*x-y*y)+2*(x-1)*y;
    			dfdy[i][j] = -2*y*Math.exp(-x*x-y*y)+(x-1)*(x-1)+3*(y-5)*(y-5);
    			d2fdxdy[i][j] = 4*x*y*Math.exp(-x*x-y*y)+2*(x-1);
    		}
    	}

    	for(int i=0;i<numx;++i) {
    		for(int j=0;j<numy;++j) {
    			CubicInterpolation2D.gridPointDerivatives(xp, yp, f, i, j, der);
    			
    			if (i == 0 || i == numx-1 || j == 0 || j == numy-1) {
    	       		//System.out.println(i+", "+j+" "+d2fdxdy[i][j]+":"+der[2]);
        	    	assertEquals(1, der[0]/dfdx[i][j], 0.01);
        	    	assertEquals(1, der[1]/dfdy[i][j], 0.01);
        	    	assertTrue(Math.abs(der[2]-d2fdxdy[i][j]) < 0.02);
        	    	    			    	       		
    	       	}
    	       	else {
        	    	assertEquals(1, der[0]/dfdx[i][j], 0.0001);
        	    	assertEquals(1, der[1]/dfdy[i][j], 0.0001);
        	    	assertEquals(1, der[2]/d2fdxdy[i][j], 0.02);
    	       	}
    		}
    	}
    	
    }
    
    
    /**
     * Tests the cubicInterpolation2D() methods.
     *
     */
    public void testCubicInterpolation2D() {
    	double minx = -1;
    	double maxx = 2;
    	double miny = 2;
    	double maxy = 4;
    	int numx = 250;
    	int numy = 253;
    	double dx = (maxx-minx)/(numx-1);
    	double dy = (maxy-miny)/(numy-1);
    	double[][] f = new double[numx][numy];
    	double[][] dfdx = new double[numx][numy];
    	double[][] dfdy = new double[numx][numy];
    	double[][] d2fdxdy = new double[numx][numy];
    	double x,y;
    	double[] xp = new double[numx], yp = new double[numy];
    	
    	for(int i=0;i<numx;++i) {
    		x = minx + i*dx;
    		xp[i] = x;
    		for(int j=0;j<numy;++j) {
    			y = miny + j*dy;
    			yp[j] = y;
    			f[i][j] = Math.exp(-x*x-y*y)+(x-1)*(x-1)*y+(y-5)*(y-5)*(y-5);
    			dfdx[i][j] = -2*x*Math.exp(-x*x-y*y)+2*(x-1)*y;
    			dfdy[i][j] = -2*y*Math.exp(-x*x-y*y)+(x-1)*(x-1)+3*(y-5)*(y-5);
    			d2fdxdy[i][j] = 4*x*y*Math.exp(-x*x-y*y)+2*(x-1);
    		}
    	}
    	
    	double[] der = new double[3];
    	double[] fp = new double[] { f[0][0], f[1][0], f[1][1], f[0][1] };
    	double[] dfdxp = new double[] { dfdx[0][0], dfdx[1][0], dfdx[1][1], dfdx[0][1] };
    	double[] dfdyp = new double[] { dfdy[0][0], dfdy[1][0], dfdy[1][1], dfdy[0][1] };
    	double[] d2fdxdyp = new double[] { d2fdxdy[0][0], d2fdxdy[1][0], d2fdxdy[1][1], d2fdxdy[0][1] };
    	
    	x = minx; y = miny;
    	double ret = CubicInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, dfdxp, dfdyp, d2fdxdyp, x, y, der);
    	assertEquals(f[0][0], ret, 1e-12);
    	assertEquals(dfdx[0][0], der[0], 1e-12);
    	assertEquals(dfdy[0][0], der[1], 1e-12);
    	assertEquals(d2fdxdy[0][0], der[2], 1e-12);
    	
    	x = minx+dx; y = miny;
    	ret = CubicInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, dfdxp, dfdyp, d2fdxdyp, x, y, der);
    	assertEquals(f[1][0], ret, 1e-12);
    	assertEquals(dfdx[1][0], der[0], 1e-10);
    	assertEquals(dfdy[1][0], der[1], 1e-10);
    	assertEquals(d2fdxdy[1][0], der[2], 1e-10);
    	
    	x = minx+dx; y = miny+dy;
    	ret = CubicInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, dfdxp, dfdyp, d2fdxdyp, x, y, der);
    	assertEquals(f[1][1], ret, 1e-12);
    	assertEquals(dfdx[1][1], der[0], 1e-10);
    	assertEquals(dfdy[1][1], der[1], 1e-10);
    	assertEquals(d2fdxdy[1][1], der[2], 1e-8);
    	
    	x = minx; y = miny+dy;
    	ret = CubicInterpolation2D.eval(minx, minx+dx, miny, miny+dy, fp, dfdxp, dfdyp, d2fdxdyp, x, y, der);
    	assertEquals(f[0][1], ret, 1e-12);
    	assertEquals(dfdx[0][1], der[0], 1e-12);
    	assertEquals(dfdy[0][1], der[1], 1e-10);
    	assertEquals(d2fdxdy[0][1], der[2], 1e-10);

    	CubicInterpolation2D ip = new CubicInterpolation2D(xp,yp,f);
    	ret = ip.eval(minx, miny, der);
       	assertEquals(f[0][0], ret, 1e-12);
    	assertEquals(1, der[0]/dfdx[0][0], 0.05);
    	assertEquals(1, der[1]/dfdy[0][0], 0.05);
    	assertEquals(1, der[2]/d2fdxdy[0][0], 0.05);
    	
    	ret = ip.eval(minx, miny+2*dy, der);
       	assertEquals(f[0][2], ret, 1e-12);
    	assertEquals(1, der[0]/dfdx[0][2], 0.05);
    	assertEquals(1, der[1]/dfdy[0][2], 0.05);
    	assertEquals(1, der[2]/d2fdxdy[0][2], 0.05);
    	
    	ret = ip.eval(minx, miny+2*dy, null); //test with no deriv alloced
        assertEquals(f[0][2], ret, 1e-12);
        
    	for(int i=0;i<numx;++i) {
    		for(int j=0;j<numy;++j) {
    			ret = ip.eval(xp[i],yp[j], der);
    	       	assertEquals(f[i][j], ret, 1e-12);
    			if (i == 0 || i == numx-1 || j == 0 || j == numy-1) {
        	    	assertEquals(1, der[0]/dfdx[i][j], 0.01);
        	    	assertEquals(1, der[1]/dfdy[i][j], 0.01);
        	    	assertTrue(Math.abs(der[2]-d2fdxdy[i][j]) < 0.02);
        	    	    			    	       		
    	       	}
    	       	else {
        	    	assertEquals(1, der[0]/dfdx[i][j], 0.0001);
        	    	assertEquals(1, der[1]/dfdy[i][j], 0.0001);
        	    	assertEquals(1, der[2]/d2fdxdy[i][j], 0.02);
    	       	}
    		}
    	}
    	
    	
    	// Test continuity of function and derivative
    	double retll, retlh, rethl, rethh;
    	double[] derll = new double[3], derlh = new double[3];
    	double[] derhl = new double[3], derhh = new double[3];
    	double d = 1e-12;
    	for(int i=1;i<numx-1;++i) {
    		for(int j=1;j<numy-1;++j) {
    			retll = ip.eval(xp[i]-2*d,yp[j], derll);
    			retlh = ip.eval(xp[i]-d,yp[j], derlh);
    			rethl = ip.eval(xp[i]+d,yp[j], derhl);
    			rethh = ip.eval(xp[i]+2*d,yp[j], derhh);
    			
    			assertEquals(1.0, retll/retlh,  0.001);
    			assertEquals(1.0, rethl/rethh,  0.001);
    			assertEquals(1.0, retlh/rethl,  0.001);
    			
    			assertEquals(1.0, derll[0]/derlh[0],  0.001);
    			assertEquals(1.0, derhl[0]/derhh[0],  0.001);
    			assertEquals(1.0, derlh[0]/derhl[0],  0.001);
    			
    			assertEquals(1.0, derll[1]/derlh[1],  0.001);
    			assertEquals(1.0, derhl[1]/derhh[1],  0.001);
    			assertEquals(1.0, derlh[1]/derhl[1],  0.001);
    		}
    	}
    	
    	
    	// Test interpolation with given derivatives
    	/* <oliford  16/5/11> This doesn't seem to exist
    	ip = new CubicInterpolation2D(xp,yp,f,dfdx,dfdy,d2fdxdy);
    	
    	for(int i=0;i<numx;++i) {
    		for(int j=0;j<numy;++j) {
    			ret = ip.eval(xp[i],yp[j], der);
    	       	assertEquals(f[i][j], ret, 1e-12);
    			if (i == 0 || i == numx-1 || j == 0 || j == numy-1) {
        	    	assertEquals(1, der[0]/dfdx[i][j], 1e-10);
        	    	assertEquals(1, der[1]/dfdy[i][j], 1e-10);
        	    	assertEquals(1, der[2]/d2fdxdy[i][j], 1e-8);
    	       	}
    	       	else {
        	    	assertEquals(1, der[0]/dfdx[i][j], 0.0001);
        	    	assertEquals(1, der[1]/dfdy[i][j], 0.0001);
        	    	assertEquals(1, der[2]/d2fdxdy[i][j], 0.02);
    	       	}
    		}
    	}
    	*/
    	
    	
    }
    
    
    
    /**
     * Tests the binarySearch() method.
     *
     */
    public void testBinarySearch() {
    	int num = 10;
    	double[] x = new double[num];
    	for(int i=0;i<num;++i) {
    		x[i] = i;
    	}
    	
    	for(int i=0;i<num-1;++i) {
    		assertEquals(i, binarySearch(x, i));
    	}
    	assertEquals(num-2, binarySearch(x, num-1));
    	
    	for(int i=0;i<9*num;++i) {
    		assertEquals(i/10, binarySearch(x, i/10.0));
    	}
    	
    	assertEquals(num, binarySearch(x, 100000));
    	assertEquals(-1, binarySearch(x, -100000));

    	for(int i=0;i<num;++i) {
    		x[i] = i*i;
    	}

    	for(int i=0;i<num-1;++i) {
    		assertEquals(i, binarySearch(x, i*i));
    	}
    	assertEquals(num-2, binarySearch(x, (num-1)*(num-1)));
    	assertEquals(num, binarySearch(x, 100000));
    	assertEquals(-1, binarySearch(x, -100000));    
    }
    
    /**
     * Tests the contour() and contourSegment() metods.
     *
     */
    public void testContourMarchingSquares() {
        int numx = 50;
        int numy = 50;
        double minx = 0, maxx = 1;
        double miny = 0, maxy = 2;
        double dx = (maxx-minx)/(numx-1);
        double dy = (maxy-miny)/(numy-1);
        double[] xp = new double[numx];
        double[] yp = new double[numy];
        double[][] fp = new double[numx][numy];
        
        for(int i=0;i<numx;++i) {
            xp[i] = minx+dx*i;
            for(int j=0;j<numy;++j) {
                yp[j] = miny+dy*j;
                fp[i][j] = (xp[i]-0.5)*(xp[i]-0.5)+(yp[j]-1)*(yp[j]-1);
            }
        }        
        
        double[][][] contour1 = contour(xp,yp,fp,fp[20][30]+0.01);
        double[][][] contour2 = contour(xp,yp,fp,0.1);
        double[][][] contour3 = contour(xp,yp,fp,0.05);
        
        
        try {
            
            /*
            PrintWriter out = new PrintWriter("q:/matlab/contoFull.txt");
            
            for(int i=0;i<contour1.length;++i) {
                out.print(String.format("%g %g\n", contour1[i][0][0], contour1[i][0][1]));
                out.print(String.format("%g %g\n", contour1[i][1][0], contour1[i][1][1]));
            }
            */
/*
            for(int i=0;i<contour2.length;++i) {
                out.print(String.format("%g %g\n", contour2[i][0][0], contour2[i][0][1]));
                out.print(String.format("%g %g\n", contour2[i][1][0], contour2[i][1][1]));
            }

            for(int i=0;i<contour3.length;++i) {
                out.print(String.format("%g %g\n", contour3[i][0][0], contour3[i][0][1]));
                out.print(String.format("%g %g\n", contour3[i][1][0], contour3[i][1][1]));
            }
*/
            /*
            out.close();
            */
        }
        catch (Exception ex){
            ex.printStackTrace();
        }
        
    }
    
    
    /**
     * Tests the contourTrace() method.
     */
    public void testContourTrace() {
        int numx = 50;
        int numy = 50;
        double minx = 0, maxx = 1;
        double miny = 0, maxy = 2;
        double dx = (maxx-minx)/(numx-1);
        double dy = (maxy-miny)/(numy-1);
        double[] xp = new double[numx];
        double[] yp = new double[numy];
        double[][] fp = new double[numx][numy];
        
        for(int i=0;i<numx;++i) {
            xp[i] = minx+dx*i;
            for(int j=0;j<numy;++j) {
                yp[j] = miny+dy*j;
                fp[i][j] = (xp[i]-0.5)*(xp[i]-0.5)+(yp[j]-1)*(yp[j]-1);
            }
        }        

        double cval = fp[20][30]+0.01;
        double[][] trace = contourTrace(xp,yp,fp,20,30,cval);        
               
        /*
        try {            
            PrintWriter out = new PrintWriter("h:/matlab/conto.txt");
            
            for(int i=0;i<trace[0].length;++i) {
                out.print(String.format("%g %g\n", trace[0][i], trace[1][i]));
            }
            
            out.close();
        }
        catch (Exception ex){
            ex.printStackTrace();
        } 
        */       
    }
    
    
    /**
     * Tests quicksort()
     *
     */
    public void testQuicksort() {
        double[] a = {9,8,7,6,5,4,3,2,1,0,-1};
        
        quicksort(a,null);
        for(int i=0;i<a.length-1;++i) {
            assertEquals(-1.0+i,a[i],1e-23);            
        }
        int[] ia = new int[a.length];
        quicksort(a,ia);
        for(int i=0;i<a.length-1;++i) {
            assertEquals(-1.0+i,a[i],1e-23);
            assertEquals(i,ia[i]);
        }
        
        a = new double[] {9,8,7,6,5,4,3,2,1,0,-1};
        quicksort(a,ia);
        for(int i=0;i<a.length-1;++i) {
            assertEquals(-1.0+i,a[i],1e-23);
            assertEquals(a.length-i-1,ia[i]);
        }
        a = new double[] {9,8,7,6,5,4,3,2,1,0,-1};
        order(a,ia);
        for(int i=0;i<a.length-1;++i) {
            assertEquals(-1.0+i,a[i],1e-23);            
        }                
    }
    
    public void testPolygonArea() {
        double[] x = new double[] { 0,1,1,0 };
        double[] y = new double[] { 0,0,1,1 };    
        assertEquals(1.0,polygonArea(x,y), 1e-24);
        
        x = new double[] { 0,1,1,0,0 };
        y = new double[] { 0,0,1,1,0 };
        assertEquals(1.0,polygonArea(x,y), 1e-24);
        
        x = new double[] { 0,2,2,0,0 };
        y = new double[] { 0,0,1.5,1.5,0 };
        assertEquals(3.0,polygonArea(x,y), 1e-24);            
    }
    
    public void testPolygonCentroid() {
        double[] x = new double[] { 0,1,1,0 };
        double[] y = new double[] { 0,0,1,1 };
        double[] ret = polygonCentroid(x,y);
        assertEquals(0.5,ret[0], 1e-24);
        assertEquals(0.5,ret[1], 1e-24);
        assertEquals(1.0,ret[2], 1e-24);
                
        x = new double[] { 0,1,1,0,0 };
        y = new double[] { 0,0,1,1,0 };
        ret = polygonCentroid(x,y);
        assertEquals(0.5,ret[0], 1e-24);
        assertEquals(0.5,ret[1], 1e-24);
        assertEquals(1.0,ret[2], 1e-24);
        
        x = new double[] { 0,2,2,0,0 };
        y = new double[] { 0,0,1.5,1.5,0 };
        ret = polygonCentroid(x,y);
        assertEquals(1,ret[0], 1e-24);
        assertEquals(0.75,ret[1], 1e-24);
        assertEquals(3.0,ret[2], 1e-24);
    }
    
    public void testPolygonLength() {
        double[] x = new double[] { 0,1,1,0 };
        double[] y = new double[] { 0,0,1.1,1.1 };        
        assertEquals(4.2,Algorithms.polygonLength(x, y), 1e-24);
        
        x = new double[] { 0,1,1,0,0 };
        y = new double[] { 0,0,1.1,1.1,0 };        
        assertEquals(4.2,Algorithms.polygonLength(x, y), 1e-24);        

    }
    
        
    public void testRungeKuttaStep() {
        double h = 0.001;
        int numPoints = 1000;
        double[] xcur = new double[] { 1.0 };
        double[] tcur = new double[] { 0.0 };
        double[][] storage = new double[5][1];
        double[] t = new double[numPoints];
        double[] x = new double[numPoints];
         
        ODEFunction f = new ODEFunction() {          
             public void value(double[] x, double t, double[] f) {
                 f[0] = x[0];
             }
        };
        
        t[0] = tcur[0]; x[0] = xcur[0];
        for(int i=1;i<numPoints;++i) {
              // step size h could be changed dynamically here              
              tcur[0] = h*(i-1); // Alternatively, to reduce cumulative round-off errors on t.
              rungeKuttaStep(f,h,xcur,tcur,storage);             
              
              // Save results
              t[i] = tcur[0]; x[i] = xcur[0];
        }

        assertEquals(0,t[0],1e-12);
        assertEquals(1,x[0],1e-12);
        assertEquals(t[numPoints-1],(numPoints-1)*h,1e-12);
        assertEquals(1.0,x[numPoints-1]/Math.exp(1),1e-3);               
    }
    
    
    public void testRungeKutta() {
       // dx/dt=x, gives x(t)=exp(t) for x(0)=1
       ODEFunction f = new ODEFunction() {          
            public void value(double[] x, double t, double[] f) {
                f[0] = x[0];
            }
       };
       
       int numPoints = 1000;
       double h = 0.001;
       double[][] ret = rungeKutta(f, new double[] { 1.0 }, 0.0, h, numPoints);
       
       assertEquals(0,ret[0][0],1e-12);
       assertEquals(1,ret[1][0],1e-12);
       assertEquals((numPoints-1)*h,ret[0][numPoints-1],1e-12);
       assertEquals(1.0,ret[1][numPoints-1]/Math.exp(1),1e-3);        
       
       // d2x/dt2-x=0, gives x(t)=x(t) = sin(t) for x(0)=0, dx/dt(0)=1 
       // which gives a ODE system with two equations of first order
       // using x1 = dx/dt and x0=x, giving dx0/dt=x1, dx1/dt=x0
       f = new ODEFunction() {          
           public void value(double[] x, double t, double[] f) {
               f[0] = x[1];
               f[1] = -x[0];
           }
       };
       
       numPoints = 10000;
       h = 0.001;
       ret = rungeKutta(f, new double[] { 0, 1 }, 0, h, numPoints);
       
       assertEquals(3,ret.length);
       assertEquals(numPoints,ret[2].length);
       assertEquals(0,ret[0][0],1e-12);
       assertEquals(0,ret[1][0],1e-12);
       assertEquals(1,ret[2][0],1e-12);
       assertEquals((numPoints-1)*h,ret[0][numPoints-1],1e-12);
       assertEquals(1.0,ret[1][numPoints-1]/Math.sin((numPoints-1)*h),1e-3);        
                  
    }
    
    
    public void testMeshgrid() {
        double[] x = new double[] { 1, 2, 3 };
        double[] y = new double[] { 4, 5 };
        
        double[][][] ret = Algorithms.meshgrid(x, y);
        assertEquals(2,ret.length);
        assertEquals(y.length,ret[0].length);
        assertEquals(x.length,ret[0][0].length);
        assertEquals(y.length,ret[1].length);
        assertEquals(x.length,ret[1][0].length);
                
        assertTrue(Arrays.deepEquals(ret[0], new double[][] { {1,2,3}, {1,2,3} } ));
        assertTrue(Arrays.deepEquals(ret[1], new double[][] { {4,4,4}, {5,5,5} } ));
         
    }

    
    public void testConeLineIntersection(){
        //only one simple one
        
        double t[] = Algorithms.coneLineIntersection(
            new double[]{ 1, 0, 1 }, // P point on line
            new double[]{ 0, 1, 0 }, // D vector of line
            new double[]{ 0, 0, 0 }, // V point on cone
            new double[]{ 1, 0, 0 }, // vector on cone axis 
            46 * Math.PI / 180); // theta
        
        assertEquals(t[0], -0.26892941597972275, 1e-8); //should just clip the top of the cone
        assertEquals(t[1], 0.26892941597972275, 1e-8); //should just clip the top of the cone
        
        System.out.println(t.length + " points: " + ((t.length>0)?t[0]:"") + ((t.length>1)?(", "+t[1]):"") );
        
        //the silly case, line // cone axis and up the center, should hit the cone origin (10 points along line and it records twice)
        t = Algorithms.coneLineIntersection(
            new double[]{ -10, 0, 0 }, // P point on line
            new double[]{ 1, 0, 0 }, // D vector of line
            new double[]{ 0, 0, 0 }, // V point on cone
            new double[]{ 1, 0, 0 }, // vector on cone axis 
            1 * Math.PI / 180); // theta
        
        assertEquals(t[0], 10, 1e-8); 
        assertEquals(t[1], 10, 1e-8); 
        
      //the perp. silly case, line perp to cone axis and crossing it, should hit the cone origin (10 points along line and it records twice)
        t = Algorithms.coneLineIntersection(
            new double[]{ -10, 0, 0 }, // P point on line
            new double[]{ 1, 0, 0 }, // D vector of line
            new double[]{ 0, 0, -1e-6 }, // V point on cone, very marginally below axis
            new double[]{ 0, 0, 1 }, // vector on cone axis (vertical) 
            10 * Math.PI / 180); // theta
        
        //hmm, actually this should hit, kind of
        assertEquals(t[0], 10, 1e-4); 
        assertEquals(t[1], 10, 1e-4); 
       
        System.out.println(t.length + " points: " + ((t.length>0)?t[0]:"") + ((t.length>1)?(", "+t[1]):"") );
        
    }
    
    public void testCylinderLineIntersection(){
        //only one simple one
        
        double t[] = Algorithms.cylinderLineIntersection(
            new double[]{ 1, 0, 1 }, // L point on line
            new double[]{ 0, 1, 0 }, // V vector of line
            new double[]{ 0, 0, 0 }, // C point on cylinder
            new double[]{ 1, 0, 0 }, // u vector on cylinder axis 
            1.1); // radius squared
        
        System.out.println(t.length + " points: " + ((t.length>0)?t[0]:"") + ((t.length>1)?(", "+t[1]):"") );
        
        assertEquals(t[0], 0.31622776601683805, 1e-8); //should just clip the top of the cone
        assertEquals(t[1], -0.31622776601683805, 1e-8); //should just clip the top of the cone
         

        t = Algorithms.cylinderLineIntersection(
            new double[]{ 0, 0.1, 0.1 }, // L point on line
            new double[]{ 1, 0, 1e-10 }, // V vector of line, very slightly inclined, will hit cylinder a long way out
            new double[]{ 0, 0, 0 }, // C point on cylinder
            new double[]{ 1, 0, 0 }, // u vector on cylinder axis 
            1.1); // radius squared
        
        assertEquals(t[0], 9.44030650891055E9, 1);
        assertEquals(t[1], -1.1440306508910551E10, 1);
        
        System.out.println(t.length + " points: " + ((t.length>0)?t[0]:"") + ((t.length>1)?(", "+t[1]):"") );
        
        Random rnd = new Random();

        double u[] = new double[]{ 0.5-rnd.nextDouble(), 0.5-rnd.nextDouble(), 0.5-rnd.nextDouble() };
        double l = Math.sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
        u[0] /= l; u[1] /= l; u[2] /= l;

        double v[] = new double[]{ 0.5-rnd.nextDouble(), 0.5-rnd.nextDouble(), 0.5-rnd.nextDouble() };
        l = Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
        v[0] /= l; v[1] /= l; v[2] /= l;

        double C[] = new double[]{ 5-10*rnd.nextDouble(), 5-10*rnd.nextDouble(), 5-10*rnd.nextDouble() };           
        double L[] = new double[]{ 5-10*rnd.nextDouble(), 5-10*rnd.nextDouble(), 5-10*rnd.nextDouble() };
        
        double radius = 30;
        
        t = Algorithms.cylinderLineIntersection(L, v, C, u, radius*radius);
            
        System.out.println(t.length + " points: " + ((t.length>0)?t[0]:"") + ((t.length>1)?(", "+t[1]):"") );
        
        //now get radius 
        
        double p0[] = new double[]{ L[0] + t[0]*v[0], L[1] + t[0]*v[1], L[2] + t[0]*v[2] };
        double p1[] = new double[]{ L[0] + t[1]*v[0], L[1] + t[1]*v[1], L[2] + t[1]*v[2] };
        
        //those points should be exactly the radius from the cylinder axis...
        // (we can test distLinePoint here aswell)
        
        double cu[] = new double[]{ C[0]+u[0], C[1]+u[1], C[2]+u[2] };
        double r0 = distLinePoint(C, cu, p0);
        double r1 = distLinePoint(C, cu, p1);
        
        System.out.println("r0 = " + r0 + ", r1 = "+r1);
        
        assertEquals(r0, radius, 1e-8);
        assertEquals(r1, radius, 1e-8);
        
                
        
    }
    
    private void checkEigenVecsAndVals2x2(double A[][], double v1x, double v1y, double l1, double v2x, double v2y, double l2){
        double tol = 1e-10;
        double ev[][] = Algorithms.eigenVecsAndVals2x2(A);
        
        System.out.println(v1x + "," + v1y + "\t" + v2x + "," + v2y +"\t" + l1 + "," + l2);
        System.out.print(ev[0][0] + "," + ev[0][1] + "\t" + ev[1][0] + "," + ev[1][1] +"\t" + ev[2][0] + "," + ev[2][1]);
         
        
        boolean V1IsV1 = (Math.abs(ev[0][0] - v1x)<tol && Math.abs(ev[0][1] - v1y)<tol) || (Math.abs(ev[0][0] - -v1x)<tol && Math.abs(ev[0][1] - -v1y)<tol);
        boolean V2IsV2 = (Math.abs(ev[1][0] - v2x)<tol && Math.abs(ev[1][1] - v2y)<tol) || (Math.abs(ev[1][0] - -v2x)<tol && Math.abs(ev[1][1] - -v2y)<tol);
        
        boolean V1IsV2 = (Math.abs(ev[1][0] - v1x)<tol && Math.abs(ev[1][1] - v1y)<tol) || (Math.abs(ev[1][0] - -v1x)<tol && Math.abs(ev[1][1] - -v1y)<tol);
        boolean V2IsV1 = (Math.abs(ev[0][0] - v2x)<tol && Math.abs(ev[0][1] - v2y)<tol) || (Math.abs(ev[0][0] - -v2x)<tol && Math.abs(ev[0][1] - -v2y)<tol);
     
        boolean matched = V1IsV1 && V2IsV2 && Math.abs(ev[2][0] - l1)<tol && Math.abs(ev[2][1] - l2)<tol;
        boolean flipped = V1IsV2 && V2IsV1 && Math.abs(ev[2][0] - l2)<tol && Math.abs(ev[2][1] - l1)<tol;
        
        System.out.println("\t" + (matched ? "matched" : (flipped ? "flipped" : "FAIL")));
        assertTrue(matched || flipped);
    }
    
    public void testEigenVecsAndVals2x2(){
        Random randGen = new Random();
        
        double v1x, v1y, l1, v2x, v2y, l2;
        
        checkEigenVecsAndVals2x2(new double[][]{{1,0},{0,1}}, 1,0,1, 0,1,1);
       
        checkEigenVecsAndVals2x2(new double[][]{{0,1},{1,0}}, -Math.sqrt(0.5),Math.sqrt(0.5),-1, Math.sqrt(0.5),Math.sqrt(0.5),1);
        
        // non orthogonal with A01 = 0, answer from scilab spec()
        checkEigenVecsAndVals2x2(new double[][]{{4,0},{3,3}}, 
            0.31622776601683788566532, 0.94868329805051376801828, 4,
            0, 1, 3 );
        
        // non orthogonal with A10 = 0, answer from scilab spec()
        checkEigenVecsAndVals2x2(new double[][]{{1,9},{0,7}}, 
            0.83205029433784372105976, 0.55470019622522914737317, 7,
            1, 0, 1 );
        
        for(int i=0; i < 10; i++){
            
            v1x = randGen.nextDouble() * 10 - 5;
            v1y = randGen.nextDouble() * 10 - 5;
            l1 = FastMath.sqrt(v1x*v1x + v1y*v1y);
            v1x /= l1;
            v1y /= l1;
            
            v2x = -v1y;
            v2y = v1x;
            l2 = randGen.nextDouble() * 10 - 5;

            double A[][] = new double[][]{
                { v1x*l1*v1x + v2x*l2*v2x,  v1x*l1*v1y + v2x*l2*v2y },
                { v1y*l1*v1x + v2y*l2*v2x,  v1y*l1*v1y + v2y*l2*v2y }
            };
            
            checkEigenVecsAndVals2x2(A, v1x,v1y,l1, v2x,v2y,l2);
            
            
        }
        
    }
    
    /*public void testPointOnCircleNearestThings3D() {
        double cc[] = new double[]{ 4.2, 3.4, 4.4 };
        double cn[] = new double[]{ 0.0785, 0.6442, 0.7608 };
        double cr = 4.5;
        
        double l0[] = new double[]{ -5, -5, 2 };
        double l[] = new double[]{ 1/Math.sqrt(2), -1/Math.sqrt(2), 0 };
        
        double p0[] = new double[]{ 3, 3, 3 };
        double p[] = p0.clone();
        double dist2 = pointOnCircleNearestPoint3D(p, cc, cn, cr);
        
        System.out.println("p = " + p[0] + ", "+ p[1] + ", "+ p[2] + ", dist = " + FastMath.sqrt(dist2));
    }*/
}