package algorithmrepository.magnetostatics;

import jafama.FastMath;
import algorithmrepository.Algorithms;
import algorithmrepository.interfaces.VectorND;


/**
 * A class with static methods for magnetostatic calculations.
 * 
 * Author: Jakob Svensson (c) 2010
 *
 */
public final class Magnetostatics {
    static FastMath mth;
    //static Math mth;
	
	/**
	 * Calculates  the magnetic field from an infinitely thin, infinite line current.
	 *  
	 * @param point A point on the line.
	 * @param direction The direction of the line, does not have to be normalised.
	 * @param current The current [A]
	 * @param fieldPos Positions at which the field should be calculated. fieldPos[0][j] is the j:th x-coordinate,
	 * fieldPos[1][j] the j:th y-coordinate, fieldPos[2][j] the j:th z-coordinate.
	 * 
	 * @return The magnetic field at the given field points. ret[i][j] is the i:th component of the field at point j.
	 */
	public static double[][] magneticFieldInfiniteLineFilament(double[] point, double[] direction, double current, double[][] fieldPos) {
		double[][] ret = new double[3][fieldPos[0].length];
		double[] B = new double[3];
		double[] dv = new double[3];
		double[] n = normaliseExpr(new double[3], direction.clone());		
		for(int i=0;i<fieldPos[0].length;++i) {
			
			// dv = r - r0
			dv[0] = fieldPos[0][i]-point[0];
			dv[1] = fieldPos[1][i]-point[1];
			dv[2] = fieldPos[2][i]-point[2];
			
			// Bdir = n x dv (unnormalised)
			// |n.cross(dv)| = |dv|sin(theta)=d (d is distance between point and line)
			B[0] = crossx(n, dv);
			B[1] = crossy(n, dv);
			B[2] = crossz(n, dv);
			
			double d = magnitude(B);						
			mulExpr(B, B, 2e-7*current/(d*d));
			
			ret[0][i] = B[0];
			ret[1][i] = B[1];
			ret[2][i] = B[2];
		}

		return ret;
	}
	
	
   public static double[][] magneticFieldDirectIntegration(double[][] sourcePos, double[][] sourceCurrentDensity, double[] sourceDifferentialVolume, double[][] fieldPos) {
       return magneticFieldDirectIntegration(sourcePos, sourceCurrentDensity, sourceDifferentialVolume, fieldPos, false);
   }
	
   public static double[][] magneticFieldDirectIntegration(double[][] sourcePos, double[][] sourceCurrentDensity, double[] sourceDifferentialVolume, double[][] fieldPos, boolean excludeNearestSourcePoint) {
        double[][] ret = new double[3][fieldPos[0].length];
        
        double[] r = new double[3];
        double[] jr = new double[3];
        for(int i=0;i<fieldPos[0].length;++i) {
            double[] sum = new double[3];
            int indexOfNearestSourcePoint = 0;
            double[] contributionFromNearestSourcePoint = new double[3];
            double minDist = Double.MAX_VALUE;
            for(int j=0;j<sourcePos[0].length;++j) {
                
                r[0] = fieldPos[0][i] - sourcePos[0][j]; 
                r[1] = fieldPos[1][i] - sourcePos[1][j]; 
                r[2] = fieldPos[2][i] - sourcePos[2][j];
                
                jr[0] = sourceCurrentDensity[0][j]; 
                jr[1] = sourceCurrentDensity[1][j]; 
                jr[2] = sourceCurrentDensity[2][j];
                
                double dist = Algorithms.veclength(r);
                double factor = sourceDifferentialVolume[j]/(dist*dist*dist);
                
                double dsumx = (jr[1] * r[2] - jr[2] * r[1])*factor;
                double dsumy = (jr[2] * r[0] - jr[0] * r[2])*factor;
                double dsumz = (jr[0] * r[1] - jr[1] * r[0])*factor;
                sum[0] += dsumx;
                sum[1] += dsumy;
                sum[2] += dsumz;
                
                if (excludeNearestSourcePoint && dist < minDist) {
                    minDist = dist;
                    indexOfNearestSourcePoint = j;
                    contributionFromNearestSourcePoint[0] = dsumx;
                    contributionFromNearestSourcePoint[1] = dsumy;
                    contributionFromNearestSourcePoint[2] = dsumz;
                }
            }
            ret[0][i] = (excludeNearestSourcePoint ? sum[0]-contributionFromNearestSourcePoint[0] : sum[0])*1e-7;
            ret[1][i] = (excludeNearestSourcePoint ? sum[1]-contributionFromNearestSourcePoint[1] : sum[0])*1e-7;
            ret[2][i] = (excludeNearestSourcePoint ? sum[2]-contributionFromNearestSourcePoint[2] : sum[0])*1e-7;
        }
        
        return ret;     
    }
    
    public static double[][] vectorPotentialDirectIntegration(double[][] sourcePos, double[][] sourceCurrentDensity, double[] sourceDifferentialVolume, double[][] fieldPos) {
        double[][] ret = new double[3][fieldPos[0].length];
        
        double[] r = new double[3];
        double[] jr = new double[3];
        for(int i=0;i<fieldPos[0].length;++i) {
            double[] sum = new double[3];
            for(int j=0;j<sourcePos[0].length;++j) {
                
                r[0] = fieldPos[0][i] - sourcePos[0][j]; 
                r[1] = fieldPos[1][i] - sourcePos[1][j]; 
                r[2] = fieldPos[2][i] - sourcePos[2][j];
                
                jr[0] = sourceCurrentDensity[0][j]; 
                jr[1] = sourceCurrentDensity[1][j]; 
                jr[2] = sourceCurrentDensity[2][j];
                
                double dist = Algorithms.veclength(r);
                double factor = sourceDifferentialVolume[j]/dist;
                
                sum[0] += jr[0]*factor;
                sum[1] += jr[1]*factor;
                sum[2] += jr[2]*factor;
            }
            ret[0][i] = sum[0]*1e-7;
            ret[1][i] = sum[1]*1e-7;
            ret[2][i] = sum[2]*1e-7;
        }
        
        return ret;     
    }

	
	/**
     * Returns the magnetic field at the given positions, from a circular filament.
     * 
     * The direction of positive current is defined as the
     * clockwise direction around the normal to the coil (when 
	 * looking in the direction of the normal). This
	 * gives a field direction parallell to the filament normal.
	 * 
	 *  
     * The expression for the magnetic field from a circular loop is given by
     * (derived by J. Geiger):
     * <br>
     * \f[ 
     * B_R = \mu_0 \frac{I}{4\pi} \frac{2 (Z-Z^\prime)}{ R \sqrt{(R+R^\prime)^2 + 
     * (Z+Z^\prime)^2}} \left[ -K(m) + \frac{ {R^\prime}^2+R^2 +(Z-Z^\prime)^2}{(R-R^\prime)^2 +
     * (Z-Z^\prime)^2} E(m) \right]
     * \f]
     * 
     * and
     * 
     * \f[
     * B_Z = \mu_0 \frac{I}{4\pi} \frac{2}{\sqrt{(R+R^\prime)^2 + (Z+Z^\prime)^2}}
     * \left[ K(m) + \frac{ {R^\prime}^2-R^2 -(Z-Z^\prime)^2}{(R-R^\prime)^2 +
     * (Z-Z^\prime)^2} E(m) \right]
     * \f]
     * 
     * where
     * 
     * \f[ m = \frac{4RR^\prime}{(R+R^\prime)^2+(Z-Z^\prime)^2} \f]
     * <br>
     * In these formulas the centre of the loop is at origo, and the conductor is 
     * positioned at R',Z' (cylindrical coordinates). The position at which the
     * magnetic field is evaluated is R,Z. E(m) and K(m) are elliptical integrals. 
     * 
	 * @param centre The centre of the circular filament
	 * @param normal The normal of the circular filament, need not be normalised.
	 * @param radius The radius.
	 * @param current Current flowing in filament.
	 * @param fieldPos Positions at which the field should be calculated. fieldPos[0][j] is the j:th x-coordinate,
	 * fieldPos[1][j] the j:th y-coordinate, fieldPos[2][j] the j:th z-coordinate.
	 * 
	 * @return The magnetic field at the given field points. ret[i][j] is the i:th component of the field at point j.
     */    
	public static double[][] magneticFieldCircularFilament(double[] centre, double[] normal, double radius, double current, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;		
		double[][] ret = new double[3][numPoints];
		double[] n = normaliseExpr(new double[3], normal);
        double Rprim = radius;
        double Zprim = 0;
        
        double[] dR = new double[3];
        double[] Rv = new double[3];
        double[] r = new double[3];
        double[] t = new double[3];
        double[] B = new double[3];
        for(int i=0;i<numPoints;++i) {  
        	copySingleExpr(r, fieldPos, i);
        	minusExpr(dR, r, centre); // Vector from centre to the point	        
	        double Z = dot(dR, n);	        
	        minusExpr(Rv, dR, mulExpr(t, copyExpr(t, n), Z)); // Vector in the plane of the circle, from centre to the points projection	        
	        double R = magnitude(Rv);  // Distance to the centre axis	                                
	        double k2, sr, dz, E, K;
	        double t1, t2, t3;
	       
	        dz = Z - Zprim;
	        sr = R + Rprim;
	        t1 = sr * sr + dz * dz;
	        t2 = 1 / mth.sqrt(t1);
	        k2 = 4 * R * Rprim / t1;
	        t3 = k2 / (1 - k2);
	        E = Algorithms.ellipticE(k2);
	        K = Algorithms.ellipticK(k2);   
	        	                 
	        if (mth.abs(R) < 1e-12) { // Otherwise roundoff errors, strictly only for R == 0     
	        	mulExpr(B, n, 2.0*mth.PI*radius*radius/mth.pow(radius*radius+dz*dz,1.5));	           
	        } else {
	            double Br, Bz;
	            Br = 2 * dz * t2 / R * ((1 + 0.5 * t3) * E - K); 
	            Bz = t2 * (((Rprim - R) / R * t3 - 2) * E + 2 * K);
	            mulExpr(B, Rv, Br/R); // Rv.mul(1.0/R) is eR, unit vector in R-normal	            
	            addExpr(B, B, mulExpr(t, n, Bz));	  	           
	        }  
	        mulExpr(B, B, 1.0e-7*current);
	    
	        ret[0][i] = B[0]; ret[1][i] = B[1]; ret[2][i] = B[2];        
        }
                
        return ret;
	}
	
	public static double[][] vectorPotentialCircularFilament(double[] centre, double[] normal, double radius, double current, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;		
		double[][] ret = new double[3][numPoints];
        double Rprim = radius;
        double Zprim = 0;
        
        double[] dR = new double[3];
        double[] Rv = new double[3];
        double[] r = new double[3];
        double[] t = new double[3];
        double[] A = new double[3];
        double[] ePhi = new double[3];
        for(int i=0;i<numPoints;++i) {   
        	r[0] = fieldPos[0][i]; r[1] = fieldPos[1][i]; r[2] = fieldPos[2][i];        	
        	minusExpr(dR, r, centre); // Vector from centre to the point	        
	        double Z = dot(dR, normal);	        
	        minusExpr(Rv, dR, mulExpr(t, copyExpr(t, normal), Z)); // Vector in the plane of the circle, from centre to the points projection	        
	        double R = magnitude(Rv);  // Distance to the centre axis	                                
	        double k,k2, sr, dz, E, K;
	        double t1;
	  	       
	        dz = Z - Zprim;
	        sr = R + Rprim;
	        t1 = sr * sr + dz * dz;

	        k2 = 4 * R * Rprim / t1;
	        k = mth.sqrt(k2);        
	        E = Algorithms.ellipticE(k2);
	        K = Algorithms.ellipticK(k2);        
       
	        // Infinite on the filament (as it should)
	        // But what about R == 0?
	        double Aphi = 1.0/k*mth.sqrt(Rprim/R)*((1-k2/2)*K - E);                
	        
	        // Unit vector in phi direction
	        crossExpr(ePhi, normal, Rv);
	        normaliseExpr(ePhi, ePhi);
	        mulExpr(A, ePhi, Aphi);
	        
	        mulExpr(A, A, 4.0e-7*current);
	       
	        ret[0][i] = A[0]; ret[1][i] = A[1]; ret[2][i] = A[2];        
        }
                
        return ret;
	}	
	 	
	public static double[][] vectorPotentialInfiniteLineFilament(double[] point, double[] direction, double current, double[][] fieldPos) {
		// TODO: What is the vector potential from a line current?
		throw new RuntimeException("Not implemented: Vector potential from infinite line filament");		
	}
	

	/**
	 * 
	 * Calculates magnetic field from a polygonal filament.
	 * 
	 * The direction of the current for a polgon filament is defined through
	 * the order in which the vertices is given. A positive direction indicates that positive
	 * current flows in the direction the vertices are defined. A negative
	 * direction indicates that positive current flows in the opposite vertices
	 * direction.	 
	 * 
	 * @param vertices vertices[0][i] is the ith x coordinate, vertices[1][i] ith y-coordinate, vertices[2][i] ith z coordinate.
	 * @param current The current [A]
	 * @param fieldPos Positions at which the field should be calculated. fieldPos[0][j] is the j:th x-coordinate,
	 * fieldPos[1][j] the j:th y-coordinate, fieldPos[2][j] the j:th z-coordinate.
	 * 
	 * @return The magnetic field at the given field points. ret[i][j] is the i:th component of the field at point j.
	 */
	public static double[][] magneticFieldPolygonFilament(double[][] vertices, double current, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		int numVertices = vertices[0].length;
		
		if (current == 0) return ret;
	
		double[] bc = new double[3];
		double[] r = new double[3];
		double[] v1 = new double[3];
		double[] v2 = new double[3];
		double[] a = new double[3];
		double[] b = new double[3];
		double[] B = new double[3];
		double[] t = new double[3];
		for(int i=0;i<numPoints;++i) {
			copySingleExpr(r, fieldPos, i);
			constExpr(bc, 0);
			for(int j=0;j<numVertices-1;++j) {
				copySingleExpr(v1, vertices, j);
				copySingleExpr(v2, vertices, j+1);
				minusExpr(a, v1, r);
				minusExpr(b, v2, r);

				double an = magnitude(a);
				double bn = magnitude(b);
				double ab = an*bn;
				double s = (an+bn)/(ab*(ab + dot(a, b)));
				addExpr(bc, bc, mulExpr(t, crossExpr(t, a, b), s));				        
			}
			mulExpr(B, bc, 1.0e-7*current);
			ret[0][i] = B[0]; ret[1][i] = B[1]; ret[2][i] = B[2];
		}        
		
		return ret;		
	}

	public  static double[][] vectorPotentialPolygonFilament(double[][] vertices, double current, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		int numVertices = vertices[0].length;
		
		if (current == 0) return ret;
	
		double[] dr = new double[3], dr0 = new double[3], dr1 = new double[3], v1 = new double[3];
		double[] v2 = new double[3], A = new double[3], r = new double[3], t = new double[3];
		for(int i=0;i<numPoints;++i) {
			copySingleExpr(r, fieldPos, i);
			constExpr(A, 0);
			for(int j=0;j<numVertices-1;++j) {
				copySingleExpr(v1, vertices, j);
				copySingleExpr(v2, vertices, j+1);
				minusExpr(dr, v2, v1);
				minusExpr(dr0, v1, r);
				minusExpr(dr1, v2, r);
				
				double dl = magnitude(dr);
				double log = 1.0/dl*mth.log((dot(dr1, dr)+dl*magnitude(dr1))/(dot(dr0, dr)+dl*magnitude(dr0)));
				addExpr(A, A, mulExpr(t, dr, log));
				
			}
			mulExpr(A, A, 1.0e-7*current);
			ret[0][i] = A[0]; ret[1][i] = A[1]; ret[2][i] = A[2];
		}        
		
		return ret;		      	
	}
	
	public static double[][] magneticFieldDipole(double[] pos, double[] moment, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		double[] rv = new double[3], r = new double[3], B = new double[3], t1 = new double[3], t2 = new double[3];		
		for(int i=0;i<numPoints;++i) {
			copySingleExpr(r, fieldPos, i);
			minusExpr(rv, pos, r);
			double rvmag = magnitude(rv);			
			minusExpr(B, mulExpr(t2, rv, 3.0*dot(moment, rv)), mulExpr(t1, moment, rvmag*rvmag));
			mulExpr(B, B, 1e-7/mth.pow(rvmag, 5.0));
			ret[0][i] = B[0]; ret[1][i] = B[1]; ret[2][i] = B[2];
		}
				
        return ret;		
	}
	
	
	public static double[][] vectorPotentialDipole(double[] pos, double[] moment, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		double[] rv = new double[3], r = new double[3], A = new double[3], t = new double[3];		
		for(int i=0;i<numPoints;++i) {
			copySingleExpr(r, fieldPos, i);
			minusExpr(rv, r, pos);
			mulExpr(A, crossExpr(t, moment, r), 1e-7/magnitude2(rv));
			ret[0][i] = A[0]; ret[1][i] = A[1]; ret[2][i] = A[2];
		}

		return ret; 
	}

	

	
	/**
	 * This method calculates the field from a solid circular arc with rectangular
	 * cross section carrying a uniform current. It is based on
	 * Laxmikant K Urankar:'Vector potential and Magnetic Field of
	 * Current-Carrying Finite Arc Segment in Analytical Form, Part III:
	 * Exact Computation for Rectangular Cross Section', IEEE Trans. on 
	 * Magnetics, Vol MAG-18, No 6, 1982, p.1860-1867.
	 * 
	 * @param centre The centre of the curvature of the arc segment.
	 * @param normal The normal of the midplane of the arc segment.
	 * @param r0 A vector to the start of the arc segment (centre of the rectangular cross section) 
	 * @param width The width of the rectangular cross section
	 * @param height The height of the rectangular cross section.
	 * @param arcAngularExtent The angular extent, in radians, of the arc. Starting at r0.
	 * @param current The current in the segment.
	 * @param gaussLegendrePolesAndWeights Poles (gaussLegendrePolesAndWeights[0] and weights gaussLegendrePolesAndWeights[1])
	 * for the Gauss-Legendre integration. These coefficients must have the integration limits [-1, 1], and can be created
	 * by for example algorithmrepository.Algorithms.createGaussLegendrePolesAndWeights(). By providing these terms in the
	 * method call the user can decide on the accuracy of the solution, and use precalculated coefficients of very high
	 * order if necessary.
	 * @param fieldPos Positions at which the field should be calculated. fieldPos[0][j] is the j:th x-coordinate,
	 * fieldPos[1][j] the j:th y-coordinate, fieldPos[2][j] the j:th z-coordinate.
	 * 
	 * @return The magnetic field at the given field points. ret[i][j] is the i:th component of the field at point j.
	 */
	public static double[][] magneticFieldSolidCircularArc(double[] centre, double[] normal, double[] r0, double width, double height, double arcAngularExtent, double current, double[][] gaussLegendrePolesAndWeights, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		if (current == 0) return ret;
	      
	    // Derived quantities
	    double r1,r2,z1,z2,rm;   
	    double[] x1, x2, x3;
	    double[] t = new double[3];
	    
	    // Coordinate transformation
	    double[][] axes;
	    double[][] trans;
	    double[][] invtrans;
	    
	    // Initial initialisation
        x1 = minusExpr(new double[3], r0, centre);
        x3 = normal.clone();
        x2 = crossExpr(new double[3], x3, x1);
        
        normaliseExpr(x1, x1);
        normaliseExpr(x2, x2);
        normaliseExpr(x3, x3);       
        
        axes = new double[3][];
        axes[0] = x1.clone();
        axes[1] = x2.clone();
        axes[2] = x3.clone();
        
        trans = Algorithms.transformationMatrix(axes);
        invtrans = Algorithms.inv3x3(trans); 
        
        rm = magnitude(minusExpr(t, r0, centre));
        r1 = rm-width/2;
        r2 = rm+width/2;
        z1 = -height/2;
        z2 = height/2;          
        
        double[] r = new double[3];
        double[] polesNorm = gaussLegendrePolesAndWeights[0];
        double[] weightsNorm = gaussLegendrePolesAndWeights[1];
        for(int i=0;i<numPoints;++i) {
        	copySingleExpr(r, fieldPos, i);
	       	        
	    	double[] p = Algorithms.coordinateTransform(trans, centre, r);      
	
	        double zf = p[2];
	        double rf = mth.sqrt(p[0]*p[0] + p[1]*p[1]);
	        double phif = mth.atan2(p[1], p[0]);
	        	        
	        double lowerLimit = 0+phif;
	        double upperLimit = arcAngularExtent + phif;
	        double d = (upperLimit - lowerLimit) / 2.0;
	        
	        double sr = 0, sphi = 0, sz = 0;
	        double br = 0, bphi = 0, bz = 0;       
	        double[] integ = new double[3];
	        for(int j=0;j<polesNorm.length;++j) {           
	            sr = sphi = sz = 0;
	            
	            double phi = d * (polesNorm[j] + 1.0) + lowerLimit;
	            double w = d * weightsNorm[j];
	            
	            solidCircularArc_contribB(r1,z1,phi,rf,zf,phif,integ);
	            sr += integ[0]; sphi += integ[1]; sz += integ[2];
	            
	            solidCircularArc_contribB(r2,z1,phi,rf,zf,phif,integ);
	            sr -= integ[0]; sphi -= integ[1]; sz -= integ[2];
	            
	            solidCircularArc_contribB(r1,z2,phi,rf,zf,phif,integ);
	            sr -= integ[0]; sphi -= integ[1]; sz -= integ[2];
	
	            solidCircularArc_contribB(r2,z2,phi,rf,zf,phif,integ);
	            sr += integ[0]; sphi += integ[1]; sz += integ[2];
	            
	            br += w*sr;
	            bphi += w*sphi;
	            bz += w*sz;
	        }
	        
	        double f = 1.0e-7*current/(width*height); 
	        br   *= f;
	        bphi *= f;
	        bz   *= f;    
	        		       
	        double bx = br*mth.cos(phif)-bphi*mth.sin(phif);
	        double by = br*mth.sin(phif)+bphi*mth.cos(phif);  
		        
	   		// Pure rotation of b-vector to align with global axes
			double[] blocal = new double[] { bx, by, bz };
			double[] bglobal = Algorithms.coordinateTransformInverse(invtrans, new double[] { 0,0,0 }, blocal);	
			
			ret[0][i] = bglobal[0]; ret[1][i] = bglobal[1]; ret[2][i] = bglobal[2];       
        }
	    	    
		return ret;
	}
	
    private static void solidCircularArc_contribB(double rs, double zs, double phis, double rf, double zf, double phif, double[] integ) {               
        double gamma = zs - zf;
        double phi = phis - phif;
        double c = mth.cos(phi);
        double s = mth.sin(phi);
        double B2 = rs*rs + rf*rf - 2*rf*rs*c;
        double G2 = gamma*gamma + rf*rf*s*s;
        double D2 = gamma*gamma + B2;
        double D = mth.sqrt(D2);
        double beta1 = (rs - rf*c)/mth.sqrt(G2);
        double beta2 = gamma/mth.sqrt(B2);
        double beta3 = gamma*(rs - rf*c)/(rf*s*D);
        
        double br = c*(D+rf*c*Algorithms.asinh(beta1));
        double bphi =  s*(D+rf*c*Algorithms.asinh(beta1));
        double bz = gamma*Algorithms.asinh(beta1)-rf*c*Algorithms.asinh(beta2)-rf*s*mth.atan(beta3);
        
        integ[0] = br; integ[1] = bphi; integ[2] = bz;   
    }
    
 	
	public static double[][] vectorPotentialSolidCircularArc(double[] centre, double[] normal, double[] r0, double width, double height, double arcAngularExtent, double current, double[][] gaussLegendrePolesAndWeights, double[][] fieldPos) {
		int numPoints = fieldPos[0].length;
		double[][] ret = new double[3][numPoints];
		if (current == 0) return ret;
	      
	    // Derived quantities
	    double r1,r2,z1,z2,rm;   
	    double[] x1, x2, x3;
	    double[] t = new double[3];
	    
	    // Coordinate transformation
	    double[][] axes;
	    double[][] trans;
	    double[][] invtrans;
	    
	    // Initial initialisation
        x1 = minusExpr(new double[3], r0, centre);
        x3 = normal.clone();
        x2 = crossExpr(new double[3], x3, x1);
        
        normaliseExpr(x1, x1);
        normaliseExpr(x2, x2);
        normaliseExpr(x3, x3);       
        
        axes = new double[3][];
        axes[0] = x1.clone();
        axes[1] = x2.clone();
        axes[2] = x3.clone();
        
        trans = Algorithms.transformationMatrix(axes);
        invtrans = Algorithms.inv3x3(trans); 
        
        rm = magnitude(minusExpr(t, r0, centre));
        r1 = rm-width/2;
        r2 = rm+width/2;
        z1 = -height/2;
        z2 = height/2;          
        
        double[] r = new double[3];
        double[] polesNorm = gaussLegendrePolesAndWeights[0];
        double[] weightsNorm = gaussLegendrePolesAndWeights[1];
        for(int i=0;i<numPoints;++i) {
        	copySingleExpr(r, fieldPos, i);
	       	        
	    	double[] p = Algorithms.coordinateTransform(trans, centre, r);
	    	
	        double zf = p[2];
	        double rf = mth.sqrt(p[0]*p[0] + p[1]*p[1]);
	        double phif = mth.atan2(p[1], p[0]);
	        
	        double lowerLimit = 0+phif;
	        double upperLimit = arcAngularExtent + phif;
	        double d = (upperLimit - lowerLimit) / 2.0;	        	       
	        		      
	        double sr = 0, sphi = 0;
	        double ar = 0, aphi = 0;       
	        double[] integ = new double[2];
	        for(int j=0;j<polesNorm.length;++j) {           
	            sr = sphi = 0;
	            
	            double phi = d * (polesNorm[j] + 1.0) + lowerLimit;
	            double w = d * weightsNorm[j];	            
	            
	            solidCircularArc_contribA(r1,z1,phi,rf,zf,phif,integ);
	            sr += integ[0]; sphi += integ[1]; 
	            
	            solidCircularArc_contribA(r2,z1,phi,rf,zf,phif,integ);
	            sr -= integ[0]; sphi -= integ[1]; 
	            
	            solidCircularArc_contribA(r1,z2,phi,rf,zf,phif,integ);
	            sr -= integ[0]; sphi -= integ[1]; 

	            solidCircularArc_contribA(r2,z2,phi,rf,zf,phif,integ);
	            sr += integ[0]; sphi += integ[1]; 
	            
	            ar += w*sr;
	            aphi += w*sphi;            
	        }
	        
	        double f = 0.5*1.0e-7*current/(width*height);        
	        ar   *= f;
	        aphi *= f;
	                    
	        double ax = ar*mth.cos(phif)-aphi*mth.sin(phif);
	        double ay = ar*mth.sin(phif)+aphi*mth.cos(phif);  
	        
	        // Pure rotation of b-vector to align with global axes
	        double[] alocal = new double[] { ax, ay, 0 };
	        double[] aglobal = Algorithms.coordinateTransformInverse(invtrans, new double[] { 0,0,0 }, alocal);	 
	        ret[0][i] = aglobal[0]; ret[1][i] = aglobal[1]; ret[2][i] = aglobal[2];
        }
		
		return ret;
	}		

    protected static void solidCircularArc_contribA(double rs, double zs, double phis, double rf, double zf, double phif, double[] integ) {               
        double gamma = zs - zf;
        double phi = phis - phif;
        double c = mth.cos(phi);
        double s = mth.sin(phi);
        double B2 = rs*rs + rf*rf - 2*rf*rs*c;
        double G2 = gamma*gamma + rf*rf*s*s;
        double D2 = gamma*gamma + B2;
        double D = mth.sqrt(D2);
        double beta1 = (rs - rf*c)/mth.sqrt(G2);
        double beta2 = gamma/mth.sqrt(B2);
        double beta3 = gamma*(rs - rf*c)/(rf*s*D);
        
        double t = gamma*D+2*gamma*rf*c*Algorithms.asinh(beta1)+
                   (rs*rs-rf*rf*mth.cos(2*phi))*Algorithms.asinh(beta2) -
                   rf*rf*mth.sin(2*phi)*mth.atan(beta3);
        double ar = -t*s;
        double aphi =  t*c;        
        
        integ[0] = ar; integ[1] = aphi; 
    }	
    
    	
	public static VectorND createMagneticFieldInfiniteLineFilament(final double[] point, final double[] direction, final double current) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return  magneticFieldInfiniteLineFilament(point, direction, current, pos);
			}			
		};
	}
	
	public static VectorND createVectorPotentialInfiniteLineFilament(final double[] point, final double[] direction, final double current) {
		throw new RuntimeException("Not implemented");
	}
	
	public static VectorND createMagneticFieldCircularFilament(final double[] centre, final double[] normal, final double radius, final double current) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return magneticFieldCircularFilament(centre, normal, radius, current, pos);
			}			
		};
	}
	
	public static VectorND createVectorPotentialCircularFilament(final double[] centre, final double[] normal, final double radius, final double current) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return vectorPotentialCircularFilament(centre, normal, radius, current, pos);
			}			
		};
	}
	

	public static VectorND createMagneticFieldPolygonFilament(final double[][] vertices, final double current) { 
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return magneticFieldPolygonFilament(vertices, current, pos); 
			}			
		};
	}

	public static VectorND createVectorPotentialPolygonFilament(final double[][] vertices, final double current) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return vectorPotentialPolygonFilament(vertices, current, pos); 
			}			
		};
	}
     

	public static VectorND createMagneticFieldDipole(final double[] centre, final double[] moment) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return magneticFieldDipole(centre, moment, pos); 
			}			
		};
	}

	public static VectorND createVectorPotentialDipole(final double[] centre, final double[] moment) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return vectorPotentialDipole(centre, moment, pos); 
			}			
		};
	}
	
	
	public static VectorND createMagneticFieldSolidCircularArc(final double[] centre, final double[] normal, final double[] r0, final double width, final double height, final double arcAngularExtent, final double current, final double[][] gaussLegendrePolesAndWeights) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return magneticFieldSolidCircularArc(centre, normal, r0, width, height, arcAngularExtent, current, gaussLegendrePolesAndWeights, pos);
			}			
		};
	}

	public static VectorND createVectorPotentialSolidCircularArc(final double[] centre, final double[] normal, final double[] r0, final double width, final double height, final double arcAngularExtent, final double current, final double[][] gaussLegendrePolesAndWeights) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {				
				return vectorPotentialSolidCircularArc(centre, normal, r0, width, height, arcAngularExtent, current, gaussLegendrePolesAndWeights, pos);
			}			
		};
	}
	
	public static VectorND createSuperposedField(final VectorND[] fields) {
		return new VectorND() {
			@Override
			public double[][] eval(double[][] pos) {
				double[][] ret = new double[pos.length][pos[0].length];
				for(VectorND field : fields) {
					double[][] t = field.eval(pos);
					for(int i=0;i<ret.length;++i) {						
					    for(int j=0;j<ret[i].length;++j) ret[i][j] += t[i][j];
					}					
				}
				
				return ret;
			}			
		};		
	}
	
	
	
	   /**
     * Returns a circle made out of a polygon with a given number of vertices. 
     * The normal defines the direction of the loop - the vertices will go
     * clockwise around the circle when looking along the normal.
     * 
     * @param centre Centre of the circle
     * @param normal Normal of the circle
     * @param r0 Starting point of the circle.
     * @param numVertices Number of vertices for polygonal approximation
     * @return ret[i][j] is the jth vertices for coordinate i.
     */
    public static double[][] createPolygonalCircle(double[] centre, double[] normal, double[] r0, int numVertices) {    
        double[][] ret = new double[3][numVertices];
        double[] x1 = minusExpr(new double[3], r0, centre); 
        double[] x3 = normal.clone(); 
        double[] x2 = crossExpr(new double[3], x3, x1);
        normaliseExpr(x1, x1);
        normaliseExpr(x2, x2);
        normaliseExpr(x3, x3);
        double[] t1 = new double[3], t2 = new double[3], v = new double[3];
        
        double r = magnitude(minusExpr(t1, r0, centre));        
        double dphi = 1.0/(double)(numVertices-2)*2*mth.PI;      
        for(int i=0;i<numVertices-1;++i) {       
            double cx1 = r*mth.cos(i*dphi);
            double cx2 = r*mth.sin(i*dphi);         
            
            mulExpr(t1, x1, cx1);
            mulExpr(t2, x2, cx2);
            addExpr(v, centre, addExpr(t1, t1, t2));
            ret[0][i] = v[0]; ret[1][i] = v[1]; ret[2][i] = v[2];
        }
        ret[0][numVertices-1] = ret[0][0]; ret[1][numVertices-1] = ret[1][0]; ret[2][numVertices-1] = ret[2][0];
        
        return ret;     
    }
    
    
    
    /** 
     * The following helper functions are meant to do common vector algebra without allocating any memory for returned vectors etc.
     * They either return a scalar directly, or have the structure of a standard 'expression', with the first argument being
     * the vector to which the results should be assigned. This argument is then always returned by the function.
     */
    public static final double crossx(double[] a, double[] b) { return a[1]*b[2] - a[2]*b[1]; }
    public static final double crossy(double[] a, double[] b) { return a[2]*b[0] - a[0]*b[2]; }
    public static final double crossz(double[] a, double[] b) { return a[0]*b[1] - a[1]*b[0]; }
    public static final double[] crossExpr(double[] ret, double[] a, double[] b) {  ret[0] = crossx(a,b); ret[1] = crossy(a,b); ret[2] = crossz(a,b); return ret;  };    
    public static final double magnitude(double[] a) { return mth.sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]); }
    public static final double magnitude2(double[] a) { return a[0]*a[0] + a[1]*a[1] + a[2]*a[2]; }
    public static final double[] mulExpr(double[] ret, double[] a, double factor) { ret[0] = a[0] * factor; ret[1] = a[1] * factor; ret[2] = a[2] * factor; return ret;  };
    public static final double[] addExpr(double[] ret, double[] a, double[] b) {  ret[0] = a[0] + b[0]; ret[1] = a[1] + b[1]; ret[2] = a[2] + b[2]; return ret;  };
    public static final double[] minusExpr(double[] ret, double[] a, double[] b) {  ret[0] = a[0] - b[0]; ret[1] = a[1] - b[1]; ret[2] = a[2] - b[2]; return ret;  };
    public static final double[] constExpr(double[] ret, double a) {  ret[0] = a; ret[1] = a; ret[2] = a; return ret;  };
    public static final double[] normaliseExpr(double[] ret, double[] a) { return mulExpr(ret, a, 1.0/magnitude(a)); }
    public static final double[] copyExpr(double[] ret, double[] a) { ret[0] = a[0]; ret[1] = a[1]; ret[2] = a[2]; return ret; }        
    public static final double[] copySingleExpr(double[] vector, double[][] vectors, int index) { vector[0] = vectors[0][index]; vector[1] = vectors[1][index]; vector[2] = vectors[2][index]; return vector; }
    public static final double dot(double[] a, double[] b) { return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; }

    
    /**
     * Simple convenience function to calculate the curl of a field at given 
     * (Cartesian) positions using central difference derivatives. 
     *  
     * @param pos The positions at which the curl should be evaluated.
     * @param epsilon The step size for the central difference derivatives.
     * @return The curl of the field at the given positions.
     */    
    public static double[][] curl(VectorND field, double[][] pos, double epsilon) {
        int numPoints = pos[0].length;
        double[][] ret = new double[3][numPoints];
        
        double[][] points = new double[3][6*numPoints];
        for(int i=0;i<numPoints;++i) {
            int base = i*6;
            
            // xl
            points[0][base+0] = pos[0][i] - epsilon;
            points[1][base+0] = pos[1][i];
            points[2][base+0] = pos[2][i];
            
            // xh
            points[0][base+1] = pos[0][i] + epsilon;
            points[1][base+1] = pos[1][i];
            points[2][base+1] = pos[2][i];

            // yl
            points[0][base+2] = pos[0][i];
            points[1][base+2] = pos[1][i] - epsilon;
            points[2][base+2] = pos[2][i];
            
            // yh
            points[0][base+3] = pos[0][i];
            points[1][base+3] = pos[1][i] + epsilon;
            points[2][base+3] = pos[2][i];
            
            // zl
            points[0][base+4] = pos[0][i];
            points[1][base+4] = pos[1][i];
            points[2][base+4] = pos[2][i] - epsilon;
            
            // zh
            points[0][base+5] = pos[0][i];
            points[1][base+5] = pos[1][i];
            points[2][base+5] = pos[2][i] + epsilon;
        }
        
        double[][] b = field.eval(points);
        for(int i=0;i<numPoints;++i) {
            int base = i*6;
            double fx = (b[2][base+3] - b[2][base+2] - b[1][base+5] + b[1][base+4])/(2.0*epsilon);
            double fy = (b[0][base+5] - b[0][base+4] - b[2][base+1] + b[2][base+0])/(2.0*epsilon);
            double fz = (b[1][base+1] - b[1][base+0] - b[0][base+3] + b[0][base+2])/(2.0*epsilon);    
            ret[0][i] = fx; ret[1][i] = fy; ret[2][i] = fz;
        }                                   
                   
        return ret;
    }
    
    
    /**
     * Does a line integral along a path in a 3D field.
     * 
     * @param field The field.
     * @param path A set of vertices in 3D. path[0] are the x-coordinates, path[1] the y-coordinates and path[2] the z-coordinates.
     * @param gaussLegendrePolesAndWeights Precalcualted Gauss-Legendre poles and weights for the integration limits [-1,1].
     * gaussLegendrePolesAndWeights[0] contains the poles and gaussLegendrePolesAndWeights[1] the weights. Can be created
     * with for example algorithmrepository.Algorithms.createGaussLegendrePolesAndWeights().
     * @return The line integral.
     */
    public static double lineIntegral(VectorND field, double[][] path, double[][] gaussLegendrePolesAndWeights) {
        int numPoints = path[0].length;
        int dim = path.length;
        if (dim != 3) {
            throw new RuntimeException("lineIntegral currently only works for 3D fields");
        }
        double[] n = new double[3], v1 = new double[3], v2 = new double[3], r = new double[3];
        double[] t = new double[3], b = new double[3];
        double[] polesNorm = gaussLegendrePolesAndWeights[0];
        double[] weightsNorm = gaussLegendrePolesAndWeights[1];
        double[][] pos = new double[3][polesNorm.length];
        double ret = 0;
        for(int i=0;i<numPoints-1;++i) {
            copySingleExpr(v1, path, i);
            copySingleExpr(v2, path, i+1);
            minusExpr(n, v2, v1);
            double lowerLimit = 0;
            double upperLimit = magnitude(n);
            normaliseExpr(n, n);
            double d = (upperLimit - lowerLimit) / 2.0;
            
            for(int j=0;j<polesNorm.length;++j) {              
                double s = d * (polesNorm[j] + 1.0) + lowerLimit;
                addExpr(r, v1, mulExpr(t, n, s));
                pos[0][j] = r[0]; pos[1][j] = r[1]; pos[2][j] = r[2];
            }
            
            double[][] bres = field.eval(pos);
            
            for(int j=0;j<polesNorm.length;++j) {
                copySingleExpr(b, bres, j);
                double w = d * weightsNorm[j];
                double tt = dot(b, n);
                if (Double.isNaN(tt)) {
                    System.out.println("Is nan");
                }
                ret += w*dot(b, n);
            }
        }
        
        return ret;
    }    
    
    public static double lineIntegral(VectorND field, double[][] path, int gaussLegendreIntegrationOrder) {
        return lineIntegral(field, path, Algorithms.createGaussLegendrePolesAndWeights(gaussLegendreIntegrationOrder));
    }
    
    public static double lineIntegral(VectorND field, double[][] path) {
        return lineIntegral(field, path, 6);
    }


}