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

class RotMatrix {
  float[] anis;
  float[] angles;
  float[][] R;

  RotMatrix(float angle1, float angle2, float angle3,
            float range1, float range2, float range3) {
    this.anis = new float[] {
      range1, range2, range3
    };
    this.angles = new float[] {
      angle1, angle2, angle3
    };
    
    this.R = getRot(angles[0], angles[1], angles[2],
                    anis[0], anis[1], anis[2]);
  }
  RotMatrix(RotMatrix old) {
    this.anis = old.anis;
    this.angles = old.angles;
    this.R = old.R;
  }
  RotMatrix deepCopy() {
    return new RotMatrix(angles[0], angles[1], angles[2],
                         anis[0], anis[1], anis[2]);
  }

  float[][] getRot(float ang0, float ang1, float ang2,
                   float anis0, float anis1, float anis2) {
    float alpha, beta, theta, a1, a2;
    float sa, sb, st, ca, cb, ct;

    if ( (ang0 >= 0.0) && (ang0 < 270.0) ) {
      alpha = radians(90.0 - ang0);
    }
    else {
      alpha = radians(450.0 - ang0);
    }
    beta = -radians(ang1);
    theta = radians(ang2);

    sa  = sin(alpha);
    sb  = sin(beta);
    st  = sin(theta);
    ca  = cos(alpha);
    cb  = cos(beta);
    ct  = cos(theta);

    a1 = anis0 / max(anis1, EPSILON);
    a2 = anis0 / max(anis2, EPSILON);

    float[][] rotmat = new float[][]
        { {cb*ca,                  cb*sa,                  -sb},
          {a1*(-ct*sa + st*sb*ca), a1*(ct*ca + st*sb*sa),  a1*(st*cb)},
          {a2*(st*sa + ct*sb*ca),  a2*(-st*ca + ct*sb*sa), a2*(ct*cb)} };
    //debug_print_mat(rotmat, 3, 3);
    return rotmat;
  }

  float sqDist(float[] u, float[] v) {
    float dx, dy, dz, result, cont;
    int i;
  
    dx = u[0] - v[0];
    dy = u[1] - v[1];
    dz = u[2] - v[2];
  
    result = 0.0;
    for (i=0; i < 3; ++i) {
      cont = R[i][0] * dx + R[i][1] * dy + R[i][2] * dz;
      result += cont * cont;
    }
    return (result);
  }
  
  float[] apply(float[] u) {
    float[] v = new float[3];
    for (int i=0; i < 3; ++i) {
      v[i] = R[i][0] * u[0] + R[i][1] * u[1] + R[i][2] * u[2];
    }
    return v;
  }
  
  float dist(float[] u, float[] v) {
    return sqrt(sqDist(u, v));
  }

}

class Variogram {
  float nugget;
  float variance;

  VariogramStructure[] structures=null;
  
  Variogram() {
    nugget = 0.0;
    variance = 0.0;
  }

  Variogram(float nugget) {
    this.nugget = nugget;
    variance = 0.0;
  }

  void demo() {
    nugget = 0.1;
    addStructure(new SphericalStructure(0.7, 100, 50, 10, 30, 10, 0));
    addStructure(new ExponentialStructure(0.9, 80, 40, 20, 50, 20, 10));
  }
  
  Variogram(Variogram old) {
    this.nugget = old.nugget;
    this.variance = old.variance;
    if (old.structures != null) {
      for (int i=0; i < old.structures.length; ++i) {
        addStructure(old.structures[i].duplicate());
      }
    }
  }
  
  void addStructure(VariogramStructure vs) {
    
    if (structures == null) {
      structures = new VariogramStructure[1];
      structures[0] = vs;
    }
    else {
      VariogramStructure[] temp;

      temp = new VariogramStructure[structures.length + 1];
      for (int i=0; i < structures.length; ++i) {
        temp[i] = structures[i];
      }
      temp[structures.length] = vs;
      structures = temp;
    }
  }
  
  void addStructure() {
    if (this.structures.length > 0) {
      this.addStructure(this.structures[this.structures.length-1]);
    }
    else {
      this.addStructure(new SphericalStructure());
    }
  }

  void deleteStructure(int n) {
    if ( (structures.length < 2) || (structures.length < (n+1)) ) {
      return;
    }

    VariogramStructure[] newstruct = new VariogramStructure[structures.length-1];
    for (int i=0; i < structures.length-1; ++i) {
      if (i<n) {
        newstruct[i] = structures[i];
      }
      else {
        newstruct[i] = structures[i+1];
      }
    }
    structures = newstruct;
    getVariance();
  }
  
  float getVariance() {
    variance = nugget;
    for (int i=0; i < structures.length; ++i) {
      variance = variance + structures[i].cc;
    }
    return variance;
  }

  float getCovariance(float[] u, float[] v) {
    float cov = 0;
    
    if ( (u[0] == v[0]) &&  (u[1] == v[1]) && (u[2] == v[2]) ) {
      return variance;
    }
    for (int i=0; i < structures.length; ++i) {
      cov = cov + structures[i].getCovariance(u, v, variance);
    }
    return cov;
  }
  
  Variogram duplicate() {
    Variogram dest;
    dest = new Variogram(nugget);
    for (int i=0; i < structures.length; ++i) {
      VariogramStructure dup;
      dup = structures[i].duplicate();
      dest.addStructure(dup);
    }
    return dest;
  }
}

abstract class VariogramStructure {
  float   cc;
  RotMatrix rot;
  
  VariogramStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    this.cc = cc;
    this.rot = new RotMatrix(angle1, angle2, angle3,
                             range1, range2, range3);
    // debug_print_mat(this.R, 3, 3);
  }
  
  VariogramStructure() {
    this(1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0);
  }

  VariogramStructure(VariogramStructure old) {
    this.cc = old.cc;
    this.rot = old.rot.deepCopy();
  }

  abstract float getCovariance(float[] u, float[] v, float cmax);
  abstract VariogramStructure duplicate();
  
  VariogramStructure switchType(int code) {
    switch (code) {
      case 0: return new SphericalStructure(this);
      case 1: return new ExponentialStructure(this);
      case 2: return new GaussianStructure(this);
      case 3: return new PowerStructure(this);
      case 4: return new HoleEffectStructure(this);
    }
    return null;
  }
  
  int getCode() {
    if (this instanceof SphericalStructure) return 0;
    if (this instanceof ExponentialStructure) return 1;
    if (this instanceof GaussianStructure) return 2;
    if (this instanceof PowerStructure) return 3;
    if (this instanceof HoleEffectStructure) return 4;
    return -1; // hope we don't get here
  }
}

class SphericalStructure extends VariogramStructure {
  SphericalStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    super(cc, range1, range2, range3, angle1, angle2, angle3);
  }
  SphericalStructure(VariogramStructure old) {
    super(old);
  }
  SphericalStructure() {
    super();
  }

  float getCovariance(float[] u, float[] v, float cmax) {
    float h;
    
    h = rot.dist(u,v);
    h /= rot.anis[0];
    if (h>0.0) {
      if (h < 1.0) {
        return (cc*(1.0 - h * (1.5 - 0.5*h*h)));
      }
      return 0;
    }
    return cc;
  }
  
  VariogramStructure duplicate() {
    return(new SphericalStructure(this));
  }
}

class ExponentialStructure extends VariogramStructure {
  ExponentialStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    super(cc, range1, range2, range3, angle1, angle2, angle3);
  }
  ExponentialStructure(VariogramStructure old) {
    super(old);
  }
  float getCovariance(float[] u, float[] v, float cmax) {
    float h;
    
    h = rot.dist(u,v);
    h /= rot.anis[0];

    return(cc*exp(-3.0*h));
  }
  VariogramStructure duplicate() {
    return(new ExponentialStructure(this));
  }

}

class GaussianStructure extends VariogramStructure {
  GaussianStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    super(cc, range1, range2, range3, angle1, angle2, angle3);
  }
  GaussianStructure(VariogramStructure old) {
    super(old);
  }

  float getCovariance(float[] u, float[] v, float cmax) {
    float h;
    
    h = rot.dist(u,v);
    h /= rot.anis[0];

    return(cc*exp(-3.0*h*h));
  }
  VariogramStructure duplicate() {
    return(new GaussianStructure(this));
  }

}

class PowerStructure extends VariogramStructure {
  PowerStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    super(cc, range1, range2, range3, angle1, angle2, angle3);
  }
  PowerStructure(VariogramStructure old) {
    super(old);
  }

  float getCovariance(float[] u, float[] v, float cmax) {
    float h;
    
    h = rot.dist(u,v);
    
    return(cmax - cc*pow(h, max(0.0, min(2.0, rot.anis[0]))));
  }
  
  VariogramStructure duplicate() {
    return(new PowerStructure(this));
  }

}

class HoleEffectStructure extends VariogramStructure {
  HoleEffectStructure(float cc, float range1, float range2, float range3, 
                     float angle1, float angle2, float angle3) {
    super(cc, range1, range2, range3, angle1, angle2, angle3);
  }
  HoleEffectStructure(VariogramStructure old) {
    super(old);
  }

  float getCovariance(float[] u, float[] v, float cmax) {
    float h;
    
    h = rot.dist(u,v);
    h /= rot.anis[0];

    return(cc*cos(h*PI));
  }
  VariogramStructure duplicate() {
    return(new HoleEffectStructure(this));
  }

}

/* 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)
*/