///////////////
//// Lattice.java extends Shape3D.java
//// By Murphy Stein (2006)
//// A full-fledged 3D shape consisting of vertices connected by springs
////
//// Calculates the shape of the lattice by computing the combined forces at each vertex including gravity
//// Note, right now gravity is built-into the object rather than interacting with a "Gravity" object which is probably preferable
//// One or more of the vertices can be made static "corners" so that they are unaffected by forces.
//// We are using the "Eulerian" method of solving the differential equation that represents the motion of the lattice, and we are using
//// are high dampening coefficient to circumvent rapid changes in positions of the vertices which can result in crazy, non-physically accurate results.

import java.util.ArrayList;
import wonderlab.graphics.shape.Shape3D;
import wonderlab.graphics.geometry.Vector3D;
import wonderlab.graphics.geometry.Vertex;


public class Lattice extends Shape3D {

	/////////////////// CONSTANTS /////////////////////////////
	static double gDEFAULT_EDGE_LENGTH = .1;
	static double gSPRINGYNESS = 1;
	static double gDAMPENER = 0.05;								
	Vector3D gravity = new Vector3D(0, 0, 4.0);

	Vertex[] tempVertices;
	ArrayList edges;
	Vector3D[][] forceMatrix;
	boolean[][] adjMatrix;
	int length, width;
	int size;
	protected double edgeLength;
	protected double k;
	
	public Lattice(int l, int w) {
		super();
		length =  	Math.max(l,1);
		width = 	Math.max(w,1);
		size = length*width;
		vertices = new Vertex[l*w];
		transformedVertices = new Vertex[vertices.length];
		faces = new int[(length-1)*(width-1)*2][3];
		adjMatrix = new boolean[length*width][length*width];
		forceMatrix = new Vector3D[length*width][length*width];
		edges = new ArrayList();
		clearAdjMatrix();
		clearForceMatrix();
		edgeLength = gDEFAULT_EDGE_LENGTH;
		k = gSPRINGYNESS;
		
		///////////////// CREATE VERTICES IN LATTICE FORMATION
		for (int i = 0; i < length; i++) {
			for (int j = 0; j < width; j++) {
					double x, y, z;
					x = i-(edgeLength*(float)length)/2;
					y = j-(edgeLength*(float)width)/2;
					vertices[(width)*i + j] = new Vertex(x, y, 0.0,0,1.0,0);
			}
		}
		
		
		///// derive triangular faces
		int lastIndex = 0;
		for (int i = 0; i < length-1; i++) {
			for (int j = 0; j < width-1; j++) {
				faces[lastIndex  ][0] = i*(width) + 	j;
				faces[lastIndex  ][1] = i*(width) + 	j+1;
				faces[lastIndex++][2] = i*(width) + 	j+1 + (width);

				faces[lastIndex  ][0] = i*(width) + 	j;
				faces[lastIndex	 ][1] = i*(width) + 	j+1 + width;
				faces[lastIndex++][2] = i*(width) +   	j + width;				
			}
		}
		computeNormals();
		
		///////////////// DEFINE EDGES
		/// create adjacency matrix
		for (int i = 0; i < length; i++) {
			for (int j = 0; j < width; j++) {
				if (i > 0) 					addEdge((width*i) + j, width*(i-1) + j);
				if (i < (length-1)) 		addEdge((width*i) + j, width*(i+1) + j);
				if (j > 0) 					addEdge((width*i) + j, width*i + j-1);
				if (j < (width-1)) 			addEdge((width*i) + j, width*i + j+1);
				if (((i == 0) || (i == length-1)) && ((j == 0) || (j == width-1))) {
					makeCorner(i*width + j);
				}
			}
		}
		/// create list of edges so that we don't have to do an nxn traversal 
		/// of adjacency matrix to comput forces and find edges
		int[] edge;
		for (int i = 0; i < size; i++) {
			for (int j = i+1; j < size; j++) {
				if (adjMatrix[i][j]) {
					edge = new int[2];
					edge[0] = i;
					edge[1] = j;
					edges.add(edge);
				}
			}
		}		
	}
	
	/// recompute weighted normals for each vertex
	/// do this by summing normals from each face the vertex belongs to and then normalizing
	private void computeNormals() {
		Vector3D U, normal, curPosition;
		double x,y;
		Vector3D V = new Vector3D(3);
		for (int i = 0; i < faces.length; i++) {
			U = vertices[faces[i][0]].pos().copy();
			V.copy(U);
			U.subtract((Vector3D)((Vertex)(vertices[faces[i][1]])).pos());
			V.subtract((Vector3D)((Vertex)(vertices[faces[i][2]])).pos());
			normal = U.cross(V);
			vertices[faces[i][0]].normal().add(normal);
			vertices[faces[i][1]].normal().add(normal);
			vertices[faces[i][2]].normal().add(normal);
		}
		
		/// make vertex normals unit length
		for (int i = 0; i < vertices.length; i++) {
			vertices[i].normal().normalize();
		}
	}
	
	//////////////////// CLEAR ADJACENCY MATRIX
	public void clearAdjMatrix() {
		for (int i = 0; i < size; i++) {
			for (int j = i; j < size; j++) {
				adjMatrix[i][j] = false;
			}
		}
	}

	//////////////////// CLEAR FORCE MATRIX
	public void clearForceMatrix() {
		for (int i = 0; i < size; i++) {
			for (int j = 0; j < size; j++) {
				forceMatrix[i][j] = null;
			}
		}
	}
	
	/// calculate forces on each vertex for all vertices, then update positions 
	/// by adding dx/dt.
	public void update() {
		for (int i = 0; i < size; i++) {
			calculateForceOnVertex(i);
		}
		for (int i = 0; i < size; i++) {
			updateVertexPosition(i);
		}
		clearForceMatrix();
		computeNormals();
	}
	
	/// calculate all forces on a single vertex
	/// use the force adjacency matrix to compute pairwise forces just once
	public void calculateForceOnVertex(int id) {
		Vertex vertex = vertices[id];
		Vector3D combinedForces = new Vector3D(0,0,0);
		if (isCorner(id) == false) {
			for (int i = 0; i < size; i++) {
				if (thereIsAnEdgeBetween(id,i)) {
					combinedForces.add(calculateForceBetween(id,i));
				}
			}
			vertex.vel().add(combinedForces);
			vertex.vel().add(gravity);
			vertex.vel().scaleBy(gDAMPENER);				
		}
	}

	/// calculate force between two vertices
	public Vector3D calculateForceBetween(int i, int j) {
		if (forceMatrix[i][j] != null) {
			return forceMatrix[i][j];
		} else {
			Vector3D force = vertices[j].pos().copy();
			force.subtract(vertices[i].pos());
			force.scaleBy(k * (force.length() - edgeLength));
			forceMatrix[i][j] = force;
			forceMatrix[j][i] = force.copy();
			forceMatrix[j][i].scaleBy(-1);
			return force;
		}
	}
	
	public boolean thereIsAnEdgeBetween(int i, int j) {
		if (i <= j) {
			return adjMatrix[i][j];
		} else {
			return adjMatrix[j][i];
		}
	}
	
	/////////// add edge to adjacency matrix
	/////////// store in top-half of matrix
	public void addEdge(int i, int j) {
		if (i <= j) {
			adjMatrix[i][j] = true;
		} else {
			adjMatrix[j][i] = true;
		}
	}
	public void updateVertexPosition(int i) {
		Vector3D temp = vertices[i].vel().copy();
		temp.scaleBy(0.1);
		vertices[i].pos().add(temp);
	}
	
	public double distanceBetween(int i, int j) {
		Vector3D temp = vertices[i].pos().copy();
		temp.subtract(vertices[j].pos());
		return temp.length();
	}
	
	public void makeCorner(int i) {
		adjMatrix[i][i] = true;
	}
	
	public boolean isCorner(int i) {
		return adjMatrix[i][i];
	}
	
	public Vertex get(int i) {
		return vertices[i];
	}
	
	public int size() {
		return size;
	}
	
	public void setEdgeLength(double l) {
		edgeLength = l;
	}
	
	public double edgeLength() {
		return edgeLength();
	}
	
	public double k() {
		return k;
	}
	
	public void setK(double newK) {
		k = newK;
	}
	
	public void printAdjacencyMatrix() {
		String s = "";		
		for (int i = 0; i < size; i++) {
			s = "";
			for (int j = 0; j < size; j++) {
				if (adjMatrix[i][j]) {
					s += adjMatrix[i][j] + "\t";
				} else {
					s += "---\t";
				}
			}
			System.out.println(s);
		}
		System.out.println();
	}
	
	public Vector3D gravity() {
		return gravity;
	}
	
}