package algorithmrepository;

import jafama.FastMath;

/**
 * Class for doing linear interpolation on uniform, nonuniform,
 * monotonically increasing and non-monotonically increasing 1D grids.
 * 
 *****************************************************************
 * FIXME: This needs attention and especially some heavy testing!
 * 
 * The apparent support for non uniform grids didn't work
 * and slightly messed up the tolerance handling for
 * the uniform grid.
 * 
 * Also, the tolerance specification appears to absolute. 
 * It probably should be fractional.
 * 
 * oliford <codes[at]oliford.co.uk>
 * 
 *****************************************************************
 * 
 * 
 * Usage:
 * 
 * LinearInterpolation1D ip = new LinearInterpolation1D(xp,fp);
 * ip.eval(3.2)
 * ip.eval(new double[] { 1,2,3 });
 *
 * See: http://www.algorithmrepository.org
 *
 */
public class LinearInterpolation1D implements Interpolation1D {
    
    double[] xp;
    double[] fp;
    int nump;
    double minx, maxx, dx;
    boolean equallySpaced;
    private double extrapValue = Double.NaN;
    private int extrapolationMode = 0;
    
    

    /**
     * Initialises the linear interpolator with two arrays
     * containing x-values and function values at the points.
     * The x-values don't have to be monotonically increasing
     * or uniformly spaced, but interpolation faster if 
     * uniformly spaced.
     * 
     * Linear extrapolation is done for values outside the given range.
     *
     * @param xp Array of x-coordinates.
     * @param fp The corresponding function values.
     */
    public LinearInterpolation1D(double[] xp, double[] fp) {
       this(xp, fp, EXTRAPOLATE_LINEAR, Double.NaN);
    }
    
    /** As other constructor but extrapolates with given constant value */ 
    public LinearInterpolation1D(double[] xp, double[] fp, double extrapValue) {
        this(xp, fp, EXTRAPOLATE_CONSTANT_VALUE, extrapValue);
    }
          
    public int getExtrapolationMode() { return extrapolationMode; }
    public double getExtrapolationValue() { return extrapValue; }
    public void setExtrapolation(int extrapolationMode, double extrapolationValue) {
        this.extrapolationMode = extrapolationMode;
        this.extrapValue = extrapolationValue;
    }

    /**
     * Initialises the linear interpolator with two arrays
     * containing x-values and function values at the points.
     * The x-values don't have to be monotonically increasing
     * or uniformly spaced, but interpolation faster if 
     * uniformly spaced.
     * 
     * Values outside the given domain will be replaced by the given
     * extrapolation value.
     *
     * @param xp Array of x-coordinates.
     * @param fp The corresponding function values.
     * @param extrapValue The value to be returned for extrapolation, for example Double.NaN
     */
    public LinearInterpolation1D(double[] xp, double[] fp, int extrapolationMode, double extrapValue) {
         
        boolean monotonicallyIncreasing = true;
        for(int i=1;i<xp.length;++i) {
            if (xp[i] < xp[i-1]) {
                monotonicallyIncreasing = false;
                break;
            }
        }
        if (!monotonicallyIncreasing) {
            xp = xp.clone();
            fp = fp.clone();
            int[] index = new int[xp.length];
            Algorithms.quicksort(xp, index);
            Algorithms.order(fp, index);
        }
        
        this.xp = xp;
        this.fp = fp;
        this.nump = xp.length;
        this.minx = xp[0];
        this.maxx = xp[nump-1];
        this.dx = xp[1]-xp[0];
        
        double d2, dist2 = (xp[1]-xp[0])*(xp[1]-xp[0]);
        equallySpaced = true;        
        for(int i=2;i<xp.length;++i) {
            d2 = (xp[i]-xp[i-1])*(xp[i]-xp[i-1]);
            //if ( (dist2-d2)*(dist2-d2) > (tolFrac2*d2)) {
            if ( FastMath.abs(dist2-d2) > (tolFrac2*d2)) {
                equallySpaced = false;
                break;
            }                
        }
        
        this.extrapValue = extrapValue;
        this.extrapolationMode = extrapolationMode;
    }
    
    public void setF(double[] fp) {
        this.fp = fp;
    }
    
    /**
     * Returns true if the points are regarded as equally spaced,
     * false otherwise.
     * 
     * If the points are equally spaced a faster lookup is used.
     * 
     * @return True if the points at which the function is given are
     * equally spaced, false otherwise. 
     */
    public boolean isEquallySpaced() {
        return equallySpaced;
    }

    /**
     * Returns the x coordinates.
     * 
     * @return The x coordinates.
     */
    public double[] getX() {
        return xp;
    }
    
    /**
     * Returns the vector of function values.
     * 
     * @return The vector of function values.
     */
    public double[] getF() {
        return fp;
    }
    
    /**
     * Does linear interpolation from the set of points
     * given in the constructor.
     *
     * Extrapolation: If a given point is outside the grid,
     * a linear extrapolation is done.
     *
     * @param x The x-values where f should be evaluated.
     * @return The function values at the given points.
     */
    public double[] eval(double[] x) {
        return eval(x,null);
    }
    
    public double[] eval(double[] x, double[] der) {
        double[] ret = new double[x.length];
        int left;
        for(int i=0;i<x.length;++i) {
            if (equallySpaced) {
                left = (int) ((nump-1)*(x[i]-minx)/(maxx-minx));
                // we actually should have floor() here because e.g. ((int)-5.5) = 5, not 6
                // However, this is picked up by the anti-jitter loop anyway
            }
            else {
                left = binarySearch(xp,x[i]);
            }
            
            do{
                //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 here makes this very difficult to achieve
                //  without breaking the very important anti-jitter loop
                
                if (left >= nump-1) {
                    switch(extrapolationMode){
                        case EXTRAPOLATE_LINEAR:    
                            double lastdx = xp[nump-1] - xp[nump-2];
                            ret[i] = fp[nump-1] + (fp[nump-1] - fp[nump-2])/lastdx*(x[i] - xp[nump-1]);
                            if (der != null) 
                                der[i] = (fp[nump-1] - fp[nump-2])/lastdx;
                            break;
                        case EXTRAPOLATE_CONSTANT_VALUE:                 
                            ret[i] = extrapValue;
                            if (der != null) 
                                der[i] = 0;
                            break;
                        case EXTRAPOLATE_CONSTANT_END_KNOT:
                            ret[i] = fp[nump-1];
                            if (der != null) 
                                der[i] = 0;
                            break;
                        case EXTRAPOLATE_EXCEPTION:
                        default:
                            throw new IllegalArgumentException("Extrapolation with invalid or no extrapolation mode ("+extrapolationMode+").");
                    }                
                }
                else if (left < 0) {
                    switch(extrapolationMode){
                        case EXTRAPOLATE_LINEAR:    
                            double firstdx = xp[1] - xp[0];
                            ret[i] = fp[0] + (fp[1] - fp[0])/firstdx*(x[i] - xp[0]);
                            if (der != null) 
                                der[i] = (fp[1] - fp[0])/firstdx;
                            break;
                        case EXTRAPOLATE_CONSTANT_VALUE:                 
                           ret[i] = extrapValue;
                           if (der != null) 
                               der[i] = 0;
                           break;
                        case EXTRAPOLATE_CONSTANT_END_KNOT:
                            ret[i] = fp[0];
                            if (der != null) 
                                der[i] = 0;
                            break;
                        case EXTRAPOLATE_EXCEPTION:
                        default:
                            throw new IllegalArgumentException("Extrapolation with invalid or no extrapolation mode ("+extrapolationMode+".");
                    }                
                }
                else {
                    if(equallySpaced){
                        //the points might not be equally spaced to within tol2
                        // so if we've got it slightly wrong we have to correct.
                        if(x[i] < xp[left]){ left--; continue; }
                        if(x[i] > xp[left+1]){ left++; continue; }
                        
                    }
                    if(left < 0 || left >= (xp.length-1))
                        throw new RuntimeException("Internal error: binary search failed, are the Xs in order?");
                     if(x[i] < xp[left] || x[i] > xp[left+1])
                         throw new RuntimeException("Internal error: binary search failed, are the Xs in order?");
                    ret[i] = fp[left]+(fp[left+1] - fp[left])*(x[i]-xp[left])/(xp[left+1]-xp[left]);
                    if (der != null) 
                        der[i] = (fp[left+1] - fp[left])/(xp[left+1]-xp[left]);
                }
                break;
                
            }while(true); // anti-jitter loop only for tolerance failures
        }
        return ret;
    }

    
    /**
     * Does a single evaluation based on the uniform grid
     * passed to the constructor.
     *
     * Extrapolation: If a given point is outside the grid,
     * a linear extrapolation is done.
     *
     * @param x The point at which the function should be
     * interpolated.
     *
     * @return The interpolated value at the given point.
     */
    public double eval(double x) {
        
        return eval(new double[]{ x }, null)[0];
    }
    
    
    /**
     * Static method exposing the functionality of linear interpolation.
     * 
     * Linearly interpolates between two given points.
     *
     * See http://www.algorithmrepository.org
     *
     * @param xa x-value for left point (xa < xb)
     * @param fa Function value at xa
     * @param xb x-value for right point
     * @param fb Function value at xb
     * @param x  The value at which the function should be evaluated
     * @return The value of the function at x.
     */
    public static double eval(double xa, double fa, double xb, double fb, double x) {
        return fa+(fb-fa)/(xb-xa)*(x-xa);
    }
    
    /**
     * Does a binary search (bisection) to find the index of the element in the
     * array x for which x[index] <= x < x[index+1], where x is monotonically
     * increasing.
     *
     * Adapted from numerical recipes ch. 3.4.
     *
     * @param x An array of monotonically increasing values.
     * @param xv The value whose nearest lower index is sought in the array.
     * @return The index that corresponds to x[index] <= x < x[index+1] or
     * -1 if x < x[0], x.length if x > x[x.length-1]
     */
    public static int binarySearch(double[] x, double xv) {
        int num = x.length;
        if (xv > x[num-1]) return x.length;
        if (xv < x[0]) return -1;
        
        int ju=num,jm,jl=-1;

        while (ju-jl > 1) {
            jm = (ju+jl) >> 1;
            if (xv >= x[jm]) {
                jl = jm;
            }
            else {
                ju = jm;
            }
        }
        if (xv > x[num-1]) return num;
        if (xv == x[0]) return 0;
        if (xv == x[num-1]) return num-2;

        return jl;
    }

}