import java.lang.*;
import Vertex3D;

public class Matrix3D {

  // variables

  float[] a = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

  // constructors

  public Matrix3D() {
    // initialize with identity transform
    loadIdentity();
  }

  public Matrix3D(Matrix3D copy) {
    // initialize with copy of source
    for (int i = 0; i < 16; i++) {
      a[i] = copy.gt(i);
    }
  }

  // methods
        
  public void set(int i, int j, float value) {
    // set element [j][i] to value
    a[4 * j + i] = value;
  }

  public void st(int i, float value) {
    // set element in ith index of array
    a[i] = value;
  }

  public float get(int i, int j) {
    // return element [j][i]
    return a[4 * j + i];
  }
  
  public float gt(int i) {
    // return element in ith index of array
    return a[i];
  }

  public Vertex3D applyPt(Vertex3D pt) {
    // applies vertex to this and returns vertex'
    float xx = pt.x * a[0] + pt.y * a[1] + pt.z * a[2] + a[3];
    float yy = pt.x * a[4] + pt.y * a[5] + pt.z * a[6] + a[7];
    float zz = pt.x * a[8] + pt.y * a[9] + pt.z * a[10] + a[11];
    float scale = pt.x * a[12] + pt.y * a[13] + pt.z * a[14] + a[15];
    return new Vertex3D(xx / scale, yy / scale, zz / scale);
  }

  public Vertex3D applyPt2(Vertex3D pt) {
    // applies vertex to this and returns vertex'
    float xx = pt.x * a[0] + pt.y * a[1] + pt.z * a[2] + a[3];
    float yy = pt.x * a[4] + pt.y * a[5] + pt.z * a[6] + a[7];
    float zz = pt.x * a[8] + pt.y * a[9] + pt.z * a[10] + a[11];
    float scale = pt.x * a[12] + pt.y * a[13] + pt.z * a[14] + a[15];
    Vertex3D a = new Vertex3D(xx, yy, zz);
    a.w = scale;
    return a;
  }

  public void transform(Vertex3D in[], Vertex3D out[], int length) {
    // Transform vertices from the in array to the out array
    // using the current matrix. The subset of vertices transformed
    // begins at the start index and has the specified length
    for (int i = 0; i < length; i++) {
      out[i] = applyPt(in[i]);
    }
  }
  
  public Vertex3D transform(Vertex3D v) {
    float x, y, z, w;
    x = a[0] * v.x + a[1] * v.y + a[2] * v.z + a[3] * v.w;
    y = a[4] * v.x + a[5] * v.y + a[6] * v.z + a[7] * v.w;
    z = a[8] * v.x + a[9] * v.y + a[10] * v.z + a[11] * v.w;
    w = a[12] * v.x + a[13] * v.y + a[14] * v.z + a[15] * v.w;
    w = 1 / w;
    Vertex3D result = new Vertex3D(x * w, y * w, z * w);
    return result;
  }

  public void transpose() {
    Matrix3D b = new Matrix3D(this);
    a[1] = b.gt(4);
    a[2] = b.gt(8);
    a[3] = b.gt(12);
    a[4] = b.gt(1);
    a[6] = b.gt(9);
    a[7] = b.gt(13);
    a[8] = b.gt(2);
    a[9] = b.gt(6);
    a[11] = b.gt(14);
    a[12] = b.gt(3);
    a[13] = b.gt(7);
    a[14] = b.gt(11);
    for (int i = 0; i < 16; i++) {
      a[i] = b.gt(i);
    }
  }

  public final void compose(Matrix3D s) {
    // this = this * s
    Matrix3D b = new Matrix3D(this);

    b.st(0, a[0] * s.gt(0) + a[1] * s.gt(4) + a[2] * s.gt(8)
      + a[3] * s.gt(12));
    b.st(1, a[0] * s.gt(1) + a[1] * s.gt(5) + a[2] * s.gt(9)
      + a[3] * s.gt(13));
    b.st(2, a[0] * s.gt(2) + a[1] * s.gt(6) + a[2] * s.gt(10)
      + a[3] * s.gt(14));
    b.st(3, a[0] * s.gt(3) + a[1] * s.gt(7) + a[2] * s.gt(11)
      + a[3] * s.gt(15));

    b.st(4, a[4] * s.gt(0) + a[5] * s.gt(4) + a[6] * s.gt(8)
      + a[7] * s.gt(12));
    b.st(5, a[4] * s.gt(1) + a[5] * s.gt(5) + a[6] * s.gt(9)
      + a[7] * s.gt(13));
    b.st(6, a[4] * s.gt(2) + a[5] * s.gt(6) + a[6] * s.gt(10)
      + a[7] * s.gt(14));
    b.st(7, a[4] * s.gt(3) + a[5] * s.gt(7) + a[6] * s.gt(11)
      + a[7] * s.gt(15));

    b.st(8, a[8] * s.gt(0) + a[9] * s.gt(4) + a[10] * s.gt(8)
      + a[11] * s.gt(12));
    b.st(9, a[8] * s.gt(1) + a[9] * s.gt(5) + a[10] * s.gt(9)
      + a[11] * s.gt(13));
    b.st(10, a[8] * s.gt(2) + a[9] * s.gt(6) + a[10] * s.gt(10)
      + a[11] * s.gt(14));
    b.st(11, a[8] * s.gt(3) + a[9] * s.gt(7) + a[10] * s.gt(11)
      + a[11] * s.gt(15));

    b.st(12, a[12] * s.gt(0) + a[13] * s.gt(4) + a[14] * s.gt(8)
      + a[15] * s.gt(12));
    b.st(13, a[12] * s.gt(1) + a[13] * s.gt(5) + a[14] * s.gt(9)
      + a[15] * s.gt(13));
    b.st(14, a[12] * s.gt(2) + a[13] * s.gt(6) + a[14] * s.gt(10)
      + a[15] * s.gt(14));
    b.st(15, a[12] * s.gt(3) + a[13] * s.gt(7) + a[14] * s.gt(11)
      + a[15] * s.gt(15));
    
    for (int i = 0; i < 16; i++) {
      a[i] = b.gt(i);
    }
  }

  public void loadIdentity() {
    // this = identity
    for (int i = 0; i < 16; i++) {
      if ((i == 0) || (i == 5) || (i == 10) || (i == 15))
	a[i] = 1;
      else a[i] = 0;
    }
  }

  public void translate(float tx, float ty, float tz) {
    // this = this * t
    Matrix3D trn = new Matrix3D();
    trn.st(3, tx);
    trn.st(7, ty);
    trn.st(11, tz);
    compose(trn);
  }

  public void scale(float sx, float sy, float sz) {
    // this = this * scale
    Matrix3D scl = new Matrix3D();
    scl.set(0, 0, sx);
    scl.set(1, 1, sy);
    scl.set(2, 2, sz);
    compose(scl);
  }

  public void skew(float kxy, float kxz, float kyz) {
    // this = this * skew
    Matrix3D skw = new Matrix3D();
    skw.set(1, 0, kxy);
    skw.set(2, 0, kxz);
    skw.set(2, 1, kyz);
    compose(skw);
  }

  public void rotate(float ax, float ay, float az, float angle) {
    // this = this * rotate
    Matrix3D rot = new Matrix3D();
    float sinangle = (float) java.lang.Math.sin((double) angle);
    float cosangle = (float) java.lang.Math.cos((double) angle);
    rot.set(0, 0, ax * ax * (1 - cosangle) + cosangle);
    rot.set(1, 0, ax * ay * (1 - cosangle) - az * sinangle);
    rot.set(2, 0, ax * az * (1 - cosangle) + ay * sinangle);
    rot.set(0, 1, ax * ay * (1 - cosangle) + az * sinangle);
    rot.set(1, 1, ay * ay * (1 - cosangle) + cosangle);
    rot.set(2, 1, ay * az * (1 - cosangle) - ax * sinangle);
    rot.set(0, 2, ax * az * (1 - cosangle) - ay * sinangle);
    rot.set(1, 2, ay * az * (1 - cosangle) + ax * sinangle);
    rot.set(2, 2, az * az * (1 - cosangle) + cosangle);
    compose(rot);
  }

  public void lookAt(float eyex, float eyey, float eyez,
		     float atx,  float aty,  float atz,
		     float upx,  float upy,  float upz) {
    // this = this * lookat
    float nrm;
    nrm = (float) java.lang.Math.sqrt((eyex - atx) * (eyex - atx) +
				      (eyey - aty) * (eyey - aty) +
				      (eyez - atz) * (eyez - atz));
    float nx = (eyex - atx) / nrm;
    float ny = (eyey - aty) / nrm;
    float nz = (eyez - atz) / nrm;
    //    float ux = ny * upz - nz * upy;
    //    float uy = nz * upx - nx * upz;
    //    float uz = nx * upy - ny * upx;
    float ux = nz * upy - ny * upz;
    float uy = nx * upz - nz * upx;
    float uz = ny * upx - nx * upy;
    nrm = (float) java.lang.Math.sqrt(ux * ux + uy * uy + uz * uz);
    ux = ux / nrm;
    uy = uy / nrm;
    uz = uz / nrm;
    float vx = uy * nz - uz * ny;
    float vy = uz * nx - ux * nz;
    float vz = ux * ny - uy * nx;
    Matrix3D look = new Matrix3D();
    look.set(0, 0, ux);
    look.set(1, 0, uy);
    look.set(2, 0, uz);
    look.set(0, 1, vx);
    look.set(1, 1, vy);
    look.set(2, 1, vz);
    look.set(0, 2, nx);
    look.set(1, 2, ny);
    look.set(2, 2, nz);
    compose(look);
    translate(-eyex, -eyey, -eyez);
  }

  // Assume the following projection transformations
  // transform points into the canonical viewing space

  public void perspective(float left, float right,               
			  float bottom, float top,
			  float near, float far) {
    // this = this * pers
    Matrix3D pers = new Matrix3D();
    pers.set(0, 0, (2 * near) / (right - left));
    pers.set(2, 0, (right + left) / (left - right));
    pers.set(1, 1, (2 * near) / (bottom - top));
    pers.set(2, 1, (bottom + top) / (top - bottom));
    pers.set(2, 2, (far + near) / (far - near));
    pers.set(3, 2, (-2 * far * near) / (far - near));
    pers.set(2, 3, 1);
    pers.set(3, 3, 0);
    compose(pers);
  }

  public void orthographic(float left, float right,               
			   float bottom, float top,
			   float near, float far) {
    // this = this * orth
    Matrix3D orth = new Matrix3D();
    orth.set(0, 0, 2 / (right - left));
    orth.set(3, 0, (right + left) / (left - right));
    orth.set(1, 1, 2 / (bottom - top));
    orth.set(3, 1, (bottom + top) / (top - bottom));
    orth.set(2, 2, 2 / (far - near));
    orth.set(3, 2, (far + near) / (near - far));
    compose(orth);
  }

  public String toString() {
    String out = "";
    out = out + "[" + a[0] + " " + a[1] + " " + a[2] + " " + a[3] +
      "\n " + a[4] + " " + a[5] + " " + a[6] + " " + a[7] +
      "\n " + a[8] + " " + a[9] + " " + a[10] + " " + a[11] +
      "\n " + a[12] + " " + a[13] + " " + a[14] + " " + a[15] + "]";
    return out;
  }

  public static void error(String err) {
    // A function to relate an error to the user
    System.err.print("Translate: " + err);
    System.exit(-1); // non-zero means "not good"
  } // end error
  
} // end Matrix3D

