/* Copyright (c) 2010, Chris Want 
 Please see BSD style licence at the end of this file */

static final int EST = 0;
static final int ESTVAR = 1;

class KrigData {
  int ndata;

  float[] x=null;
  float[] y=null;
  float[] z=null;
  float[] value=null;

  KrigData(float[] x, float[] y, float[] z, float[] value, int ndata) {
    this.x = x;
    this.y = y;
    this.z = z;
    this.value = value;
    this.ndata = ndata;
  }
  
  KrigData() {
    ndata = 0;
  }

  KrigData(File myFile) {
    String line;
    String[] strData;
    int buffersize = 1000;

    ndata = 0;
    if (myFile != null) {
      try {
        BufferedReader reader = new BufferedReader(new FileReader(myFile));
        x = new float[buffersize];
        y = new float[buffersize];
        z = new float[buffersize];
        value = new float[buffersize];

        while ( (line=reader.readLine()) != null) {
          strData = line.split("\\s++");
          if (strData.length != 4) {
            println(line);
            throw new IOException("Bad data line!\n");
          }
          if (ndata >= buffersize) {
            buffersize += 1000;
            reallocData(ndata, buffersize);
          }
          x[ndata] = Float.valueOf(strData[0].trim()).floatValue();
          y[ndata] = Float.valueOf(strData[1].trim()).floatValue();
          z[ndata] = Float.valueOf(strData[2].trim()).floatValue();
          value[ndata] = Float.valueOf(strData[3].trim()).floatValue();
          ndata++;
        }
        reallocData(ndata, ndata);
        reader.close();
      }
      catch (Exception e) {
        e.printStackTrace();
      }
    }
  }

  void demo() {
    ndata = 10;

    x = new float[]
    {
      43.64076261, 83.11554943, 36.5905571, 64.71543249,
      30.64150353, 63.8194525, 55.22933554, 86.71114627,
      48.16396179, 79.28754081
    };
    y = new float[]
    {
      36.41480527, 27.95389152, 27.66767246, 69.94672187,
      23.53553704, 24.52644163, 59.83823328, 1.293301527,
      89.86652171, 18.29903293
    };
    z = new float[]
    {
      2.535860728, 2.556437515, 3.224434115, 2.610613387,
      4.857879322, 6.857443679, 4.402435616, 2.103779292,
      6.775785359, 8.552147555
    };
    value = new float[]
    {
      0.583647025, 0.669384392, 0.937976676, 0.915608021,
      0.084571097, 0.988651978, 0.631691759, 0.145555323,
      0.096880128, 0.634291538
    };

  }
  
  void reallocData(int oldSize, int newSize) {
    float[] xOld, yOld, zOld, valueOld;
    int arrSize;

    if (newSize == oldSize) return;

    xOld=x; 
    yOld=y; 
    zOld=z; 
    valueOld=value;
    x = new float[newSize];
    y = new float[newSize];
    z = new float[newSize];
    value = new float[newSize];
    
    if (oldSize == 0) return;
    
    if (newSize > oldSize) {
      arrSize = oldSize;
    }
    else {
      arrSize = newSize;
    }
    arraycopy(xOld, 0, x, 0, arrSize);
    arraycopy(yOld, 0, y, 0, arrSize);
    arraycopy(zOld, 0, z, 0, arrSize);
    arraycopy(valueOld, 0, value, 0, arrSize);
  }

  float computeMean() {
    float m = 0.0;
    for (int i=0; i < ndata; ++i) {
      m += value[i];
    }
    return (m / ndata);
  }
  

  KrigData deepCopy(int len) {
    KrigData out = new KrigData();
    out.ndata = len;
    out.x = new float[len];
    out.y = new float[len];
    out.z = new float[len];
    out.value = new float[len];
    arraycopy(x, 0, out.x, 0, len);
    arraycopy(y, 0, out.y, 0, len);
    arraycopy(z, 0, out.z, 0, len);
    arraycopy(value, 0, out.value, 0, len);
    
    return out;
  }

  KrigData deepCopy() {
    return deepCopy(ndata);
  }
  
  void deletePoint(int n) {
    float[] xOld, yOld, zOld, valueOld;

    if ( (n<0) || (n>=ndata) ) return;
    
    xOld=x; 
    yOld=y; 
    zOld=z; 
    valueOld=value;

    x = new float[ndata-1];
    y = new float[ndata-1];
    z = new float[ndata-1];
    value = new float[ndata-1];

    arraycopy(xOld, 0, x, 0, n-1);
    arraycopy(yOld, 0, y, 0, n-1);
    arraycopy(zOld, 0, z, 0, n-1);
    arraycopy(valueOld, 0, value, 0, n-1);

    arraycopy(xOld, n+1, x, n, ndata - n);
    arraycopy(yOld, n+1, y, n, ndata - n);
    arraycopy(zOld, n+1, z, n, ndata - n);
    arraycopy(valueOld, n+1, value, n, ndata - n);

    --ndata;
  }

  void addPoint(float x0, float y0, float z0, float value0) {
    reallocData(ndata, ndata+1);
    x[ndata] = x0;
    y[ndata] = y0;
    z[ndata] = z0;
    value[ndata] = value0;
    ++ndata;
  }
  void swapArray(float[] arr, int i, int j) {
    float tmp;
    tmp = arr[i];
    arr[i] = arr[j];
    arr[j] = tmp;
  }
  
  void swapPoints(int i, int j) {
    swapArray(x, i, j);
    swapArray(y, i, j);
    swapArray(z, i, j);
    swapArray(value, i, j);
  }

}

/////////////////////////////// Kriging ////////////////////////////////

abstract class Kriging {
  KrigData kd = null;
  Variogram vario = null;
  Search search = null;
  ScalarField out = null;

  // Intermediate calculations, once per data set
  float variance;
  float mean;
  float[][] invlhs = null;

  Kriging() {
  }
  
  void demo() {
    kd = new KrigData();
    kd.demo();
    out = new ScalarField(2, 50, 50, 5, 1.0, 99.0, 1.0, 99.0, 1.0, 9.0);
    out.setInvalidValue(-1.0);
    vario = new Variogram();
    vario.demo();
    search = new EllipsoidSearch(vario, 0, 5);
    search.disable();
    
    // initialize mean
    mean = kd.computeMean();
  }

  Kriging(KrigData dat, Variogram vg, Search srch, ScalarField ko) {
    kd = dat;
    search = srch;
    vario = vg;
    out = ko;
  }

  abstract float getMean(KrigData dat);
  abstract float[][] getLHS(KrigData dat);
  abstract float[][] getInvLHS(KrigData dat);
  abstract float[] getRHS(KrigData dat, float x0, float y0, float z0);
  abstract float getEstimateVariance(KrigData dat, float[] lambda, float[] rhs, float variance);
  abstract float[] getLambda(KrigData dat, float[][] invlhs, float[] rhs);
  abstract Kriging duplicate();
  abstract void setMean(float newmean);
  abstract float[] transformValues(KrigData dat);
  abstract float getEstimate(KrigData dat, float[] lambda);

  void interpolate() {
    int i, j, k, n;
    float[] u, v, value0;
    float d, xEST, yEST, zEST;
    KrigData ktmp;

    mean = getMean(kd); // may be user set
    variance = vario.getVariance();

    // working copy of data (possibly subtract out mean)
    value0 = transformValues(kd);
    ktmp = new KrigData(kd.x, kd.y, kd.z, value0, kd.ndata);

    //search.initialize(this);

    if (search == null || !search.isEnabled()) {
      invlhs = getInvLHS(ktmp);
    }

    out.allocateData();
    for (k=0; k<out.zRes; ++k) {
      zEST = out.getZ(k);
      for (j=0; j<out.yRes; ++j) {
        yEST = out.getY(j);
        for (i=0; i<out.xRes; ++i) {

          xEST = out.getX(i);

          float rhs[];
          float lambda[];
          KrigData sd=null;
          
          if (search != null && search.isEnabled()) {

            sd = search.search(ktmp, xEST, yEST, zEST);
            if (sd != null) {
              invlhs = getInvLHS(sd);
            }
          }
          else {
            sd = ktmp;
          }
          
          if (sd != null) {
            rhs = getRHS(sd, xEST, yEST, zEST);
            lambda = getLambda(sd, invlhs, rhs);
            out.setData(EST, i, j, k, getEstimate(sd, lambda));
            out.setData(ESTVAR, i, j, k, getEstimateVariance(sd, lambda, rhs, variance));
          }
          else {
            out.invalidate(i, j, k);
          }
        }
      }
    }
  }

  void computeCovarianceMatrix(KrigData dat, float[][] lhs) {
    float[] u, v;

    u = new float[3];
    v = new float[3];
    for (int i=0; i<dat.ndata; ++i) {
      for (int j=0; j<dat.ndata; ++j) {
        u[0] = dat.x[j]; 
        u[1] = dat.y[j]; 
        u[2] = dat.z[j];
        v[0] = dat.x[i]; 
        v[1] = dat.y[i]; 
        v[2] = dat.z[i];
        lhs[i][j] = vario.getCovariance(u, v);
      }
    }
  }

  float[] computeLambda(KrigData dat, float x0, float y0, float z0) {
    if (dat == null) return null;
    if (search != null && search.isEnabled()) {
      invlhs = getInvLHS(dat);
    }
    // else, invlhs is constant for all points, already computed
    return getLambda(dat, invlhs, getRHS(dat, x0, y0, z0));
  }

  Kriging switchType(int code) {
    switch(code) {
      case 0: return new OrdinaryKriging(this);
      case 1: return new SimpleKriging(this);
    }
    return null;
  }
  
  int getCode() {
    if (this instanceof OrdinaryKriging) return 0;
    if (this instanceof SimpleKriging) return 1;
    return -1; // hope we don't get here
  }

  void readData(File myFile) {
    kd = new KrigData(myFile);
  }

  /*
  void writeData(File myFile) {
    out.writeData(myFile);
  }
  */
  
  void writeData(File myFile) {
    if (myFile != null) {
      try {
        BufferedWriter writer = new BufferedWriter(new FileWriter(myFile));
        for (int k=0; k< out.zRes; ++k) {
          for (int j=0; j< out.yRes; ++j) {
            for (int i=0; i< out.xRes; ++i) {
              writer.write(out.getData(EST, i, j, k) + "\t" + out.getData(ESTVAR, i, j, k) + "\n");
            }
          }
        }
        writer.flush();
        writer.close();
      }
      catch (Exception e) {
        e.printStackTrace();
      }
    }
  }

}

float[] matvectmult(float[][] mat, float[] vect, int dim0, int dim1) {
  int i, j;
  float[] result = new float[dim1];
  for (j = 0; j < dim1; ++j) {
    result[j] = 0.0;
    for (i = 0; i < dim0; ++i) {
      result[j] += mat[i][j] * vect[i];
    }
  }
  return result;
}


class OrdinaryKriging extends Kriging {
  OrdinaryKriging() {
    super();
  }
  
  OrdinaryKriging(KrigData dat, Variogram vg, Search srch, ScalarField ko) {
    kd = dat;
    search = srch;
    vario = vg;
    out = ko;
  }

  OrdinaryKriging(Kriging old) {
    this(old.kd, old.vario, old.search, old.out);
  }

  float[][] getLHS(KrigData dat) {
    float[][] lhs;

    lhs = new float[dat.ndata+1][dat.ndata+1];

    computeCovarianceMatrix(dat, lhs);

    for (int i=0; i<dat.ndata; ++i) {
      lhs[dat.ndata][i] = lhs[i][dat.ndata] = variance;
    }
    lhs[dat.ndata][dat.ndata] = 0.0;
    return lhs;
  }

  float[][] getInvLHS(KrigData dat) {
    return invmat(getLHS(dat), dat.ndata+1);
  }
  
  float[] getRHS(KrigData dat, float x0, float y0, float z0) {
    float[] rhs = new float[dat.ndata+1];
    float[] u = new float[] {
      x0, y0, z0
    };
    float[] v = new float[3];
    int n;
    for (n=0; n<dat.ndata; ++n) {
      v[0] = dat.x[n]; 
      v[1] = dat.y[n]; 
      v[2] = dat.z[n];
      rhs[n] = vario.getCovariance(u, v);
    }
    rhs[dat.ndata] = variance;

    return rhs;
  }


  float[] getLambda(KrigData dat, float[][] invlhs, float[] rhs) {
    return matvectmult(invlhs, rhs, dat.ndata+1, dat.ndata+1);
  }

  float getMean(KrigData dat) {
    return dat.computeMean();
  }

  float getEstimateVariance(KrigData dat, float[] lambda, float[] rhs, float variance) {
    return (variance - vectvectmult(lambda, rhs, dat.ndata) - lambda[dat.ndata]*rhs[dat.ndata]);
  }
  
  Kriging duplicate() {
    OrdinaryKriging krig;
    krig = new OrdinaryKriging(kd, vario, search, out);

    mean = kd.computeMean();
    
    return krig;
  }

  void setMean(float newmean) {
    // assume derived from data, do nothing
  }

  float[] transformValues(KrigData dat) {
    float[] value0 = new float[dat.ndata];
    for (int i=0; i < dat.ndata; ++i) {
      value0[i] = dat.value[i];
    }
    return value0;
  }
  
  float getEstimate(KrigData dat, float[] lambda) {
    return vectvectmult(lambda, dat.value, dat.ndata);
  }

}

class SimpleKriging extends Kriging {
  SimpleKriging() {
    super();
  }

  SimpleKriging(KrigData dat, Variogram vg, Search srch, ScalarField ko) {
    kd = dat;
    search = srch;
    vario = vg;
    out = ko;
  }
  
  SimpleKriging(Kriging old) {
    this(old.kd, old.vario, old.search, old.out);
  }

  float[][] getLHS(KrigData dat) {
    float[][] lhs;

    lhs = new float[dat.ndata][dat.ndata];

    computeCovarianceMatrix(dat, lhs);

    return lhs;
  }

  float[][] getInvLHS(KrigData dat) {
    return invmat(getLHS(dat), dat.ndata);
  }

  float[] getRHS(KrigData dat, float x0, float y0, float z0) {
    float[] rhs = new float[dat.ndata];
    float[] u = new float[] {
      x0, y0, z0
    };
    float[] v = new float[3];
    int n;

    for (n=0; n<dat.ndata; ++n) {
      v[0] = dat.x[n]; 
      v[1] = dat.y[n]; 
      v[2] = dat.z[n];
      rhs[n] = vario.getCovariance(u, v);
    }
    return rhs;
  }

  float[] getLambda(KrigData dat, float[][] invlhs, float[] rhs) {
    return matvectmult(invlhs, rhs, dat.ndata, dat.ndata);
  }

  float getMean(KrigData dat) {
    // assume to be set by user, do nothing
    return mean;
  }

  float getEstimateVariance(KrigData dat, float[] lambda, float[] rhs, float variance) {
    return (variance - vectvectmult(lambda, rhs, dat.ndata));
  }

  void setMean(float newmean) {
    mean = newmean;
  }

  Kriging duplicate() {
    SimpleKriging krig;
    krig = new SimpleKriging(kd, vario, search, out);

    krig.setMean(mean);
    return krig;
  }
  
  float[] transformValues(KrigData dat) {
    float[] value0 = new float[dat.ndata];
    for (int i=0; i < dat.ndata; ++i) {
      value0[i] = dat.value[i] - mean;
    }
    return value0;
  }

  float getEstimate(KrigData dat, float[] lambda) {
    return vectvectmult(lambda, dat.value, dat.ndata)+mean;
  }

}

/////////////////////// Utilities ////////////////////////////

float vectvectmult(float[] vect1, float[] vect2, int dim) {
  int i;
  float result = 0.0;
  for (i=0; i < dim; ++i) {
    result += vect1[i] * vect2[i];
  }
  return result;
}


float[][] invmat(float[][] mat, int dim)
{
  int i, j, k, pivot;
  float[][] inv = new float[dim][dim];
  float[][] b = new float[dim][dim];
  float temp, mag, mag2;

  // initialize
  for (i=0; i < dim; ++i) {
    for (j=0; j < dim; ++j) {
      inv[i][j] = ((i==j)? 1.0: 0.0);
      b[i][j] = mat[i][j];
    }
  }
  if (dim == 1) {
    inv[0][0] = 1.0 / mat[0][0];
    return inv;
  }

  for (i = 0; i < dim; ++i) {
    // find pivot
    mag = 0;
    pivot = -1;

    for (j = i; j < dim; ++j) {
      mag2 = abs(b[j][i]);
      if (mag2 > mag) {
        mag = mag2;
        pivot = j;
      }
    }
    // no pivot (error)
    if (pivot == -1 || mag == 0) {
      return null;
    }
    // move pivot row into position
    if (pivot != i) {
      for (j = i; j < dim; ++j) {
        temp = b[i][j];
        b[i][j] = b[pivot][j];
        b[pivot][j] = temp;
      }
      for (j = 0; j < dim; ++j) {
        temp = inv[i][j];
        inv[i][j] = inv[pivot][j];
        inv[pivot][j] = temp;
      }
    }

    // normalize pivot row
    mag = b[i][i];
    for (j = i; j < dim; ++j) {
      b[i][j] /= mag;
    }
    for (j = 0; j < dim; ++j) {
      inv[i][j] /= mag;
    }

    // eliminate pivot row component from other rows
    for (k = 0; k < dim; ++k) {
      if (k == i) continue;
      mag2 = b[k][i];

      for (j = i; j < dim; ++j) {
        b[k][j] -= mag2 * b[i][j];
      }
      for (j = 0; j < dim; ++j) {
        inv[k][j] -= mag2 * inv[i][j];
      }
    }
  }
  return inv;
}

void debug_print_data(float[][][] dat, int l, int m, int n) {
  int i, j, k;
  for (i = 0; i < l; ++i) {
    for (j = 0; j < m; ++j) {
      for (k = 0; k < n; ++k) {
        println(str(dat[k][j][i]));
      }
    }
  }
}


void debug_print_mat(float[][] mat, int m, int n) {
  int i, j;

  for (i = 0; i < m; ++i) {
    for (j = 0; j < n; ++j) {
      print(str(mat[i][j]));
      if (j < (n-1)) {
        print("\t");
      }
      else {
        print("\n");
      }
    }
  }
}

void debug_print_vec(float[] vec, int n) {
  int i;

  for (i = 0; i < n; ++i) {
    print(str(vec[i]));
    if (i < (n-1)) {
      print("\t");
    }
    else {
      print("\n");
    }
  }
}

/* Copyright (c) 2010, Chris Want, Research Support Group,
 AICT, University of Alberta. All rights reserved.
 
 Redistribution and use in source and binary forms, with or without 
 modification, are permitted provided that the following conditions are met:
 
 1) Redistributions of source code must retain the above copyright 
 notice, this list of conditions and the following disclaimer.
 2) Redistributions in binary form must reproduce the above copyright 
 notice, this list of conditions and the following disclaimer in the 
 documentation and/or other materials provided with the distribution.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF  
 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF 
 THE POSSIBILITY OF SUCH DAMAGE.
 
 Contributors: Chris Want (University of Alberta),
 Jeff Boisvert (University of Alberta)
 
 */