/**
 * 
 */
package edu.uprm.walsaip.vte.core.datatypes;

import edu.uprm.walsaip.vte.core.util.GLUtils;

/**
 * @author Ricardo Veguilla (veguilla@ece.uprm.edu)
 * 
 */
public class Vector {

	public static final int X = 0;

	public static final int Y = 1;

	public static final int Z = 2;

	public static final int W = 3;

	protected float array[] = { 0.0f, 0.0f, 0.0f, 0.0f };

	public Vector() {
		super();
	}

	public Vector(float array[]) {
		super();
		this.array[X] = array[X];
		this.array[Y] = array[Y];
		this.array[Z] = array[Z];
		this.array[W] = array[W];
	}

	public Vector(float x, float y, float z) {
		super();
		array[X] = x;
		array[Y] = y;
		array[Z] = z;

	}

	public Vector(float x, float y, float z, float w) {
		super();
		array[X] = x;
		array[Y] = y;
		array[Z] = z;
		array[W] = w;
	}

	
	public void setElements(float x, float y, float z, float w) {
		array[X] = x;
		array[Y] = y;
		array[Z] = z;
		array[W] = w;
	}

	 public float[] getAsArray() {
	 return array.clone();
	 }
	
	 public void setFromArray(float[] points) {
	 int size = (array.length >= points.length? points.length: array.length );
	 for ( int i = 0; i < size; i++)
	 this.array[i] = points[i];
	 }

	 public int getSize() {
	 return array.length;
	 }

	public static Vector subtract(Vector a, Vector b) {
		return new Vector(Vector.subtract(a.array, b.array));

	}

	public static float[] subtract(float[] a, float[] b) {

		return new float[] { a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3] };

	}
	
	public static void subtract(float[] a, float[] b, float[] c) {

		c[0] = a[0] - b[0];
		c[1] = a[1] - b[1];
		c[2] = a[2] - b[2];
		c[3] = a[3] - b[3];

	}
	

	public static Vector add(Vector a, Vector b) {

		return new Vector(Vector.add(a.array, b.array));

	}

	public static float[] add(float[] a, float[] b) {

		return new float[] { a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3] };

	}

	public void substract(Vector v) {

		array[0] -= v.array[0];
		array[1] -= v.array[1];
		array[2] -= v.array[2];
		array[3] -= v.array[3];
	}

	public void add(Vector v) {

		array[0] += v.array[0];
		array[1] += v.array[1];
		array[2] += v.array[2];
		array[3] += v.array[3];
	}

	public static float dotProduct(float a[], float b[]) {
		;

		return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
	}

	public static float dotProduct(Vector a, Vector b) {
		return Vector.dotProduct(a.array, b.array);

	}

	public static Vector crossProduct(Vector a, Vector b) {
		Vector r = new Vector();

		r.array[0] = (a.array[1] * b.array[2]) - (a.array[2] * b.array[1]);
		r.array[1] = (a.array[2] * b.array[0]) - (a.array[0] * b.array[2]);
		r.array[2] = (a.array[0] * b.array[1]) - (a.array[1] * b.array[0]);

		return r;
	}

	public static float[] crossProduct(float[] a, float[] b) {
		float r[] = new float[a.length];

		r[0] = (a[1] * b[2]) - (a[2] * b[1]);
		r[1] = (a[2] * b[0]) - (a[0] * b[2]);
		r[2] = (a[0] * b[1]) - (a[1] * b[0]);

		return r;
	}

	public static void crossProduct(float[] a, float[] b, float[] r) {
		
		r[0] = (a[1] * b[2]) - (a[2] * b[1]);
		r[1] = (a[2] * b[0]) - (a[0] * b[2]);
		r[2] = (a[0] * b[1]) - (a[1] * b[0]);
		
	}
	
	public static Vector inverse(Vector v) {

		Vector r = new Vector();

		r.array[0] *= -v.array[0];
		r.array[1] *= -v.array[1];
		r.array[2] *= -v.array[2];
		r.array[3] *= -v.array[3];
		return r;
	}

	public void invert() {

		array[0] *= -1.0;
		array[1] *= -1.0;
		array[2] *= -1.0;
		array[3] *= -1.0;

	}

	public static Vector scale(Vector v, float scale) {

		Vector r = new Vector();
		r.array[0] = v.array[0] * scale;
		r.array[1] = v.array[1] * scale;
		r.array[2] = v.array[2] * scale;
		r.array[3] = v.array[3] * scale;
		return r;
	}

	public void scale(float scale) {

		array[0] *= scale;
		array[1] *= scale;
		array[2] *= scale;
		array[3] *= scale;
	}

	public Vector scaled(float scale) {
		Vector r = new Vector();

		r.array[0] = array[0] * scale;
		r.array[1] = array[1] * scale;
		r.array[2] = array[2] * scale;
		r.array[3] = array[3] * scale;

		return r;
	}

	public float length() {

		return (float) Math.sqrt((array[0] * array[0]) + (array[1] * array[1])
				+ (array[2] * array[2]) + (array[3] * array[3]));

	}

	public static Vector perpendicular(Vector a, Vector b) {

		Vector r = new Vector();
		r.array[0] = a.array[1] * b.array[2] - a.array[2] * b.array[1];
		r.array[1] = a.array[2] * b.array[0] - a.array[0] * b.array[2];
		r.array[2] = a.array[0] * b.array[1] - a.array[1] * b.array[0];

		return r;
	}

	public void normalize() {

		float sum = 0;
		float length = 0;
		float ilength = 0;

		sum = (array[0] * array[0]) + (array[1] * array[1])
				+ (array[2] * array[2]) + (array[3] * array[3]);

		length = (float) Math.sqrt(sum);

		if (length != 0) {
			ilength = 1.0f / length;

			array[0] *= ilength;
			array[1] *= ilength;
			array[2] *= ilength;
			array[3] *= ilength;
		}

	}

	public Vector normalized() {
		return new Vector(Vector.normalize(this.array));
	}

	public static float[] normalize(float v[]) {
		float r[] = new float[v.length];

		float sum = 0;
		float length = 0;
		float ilength = 0;

		sum = (v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2]) + (v[3] * v[3]);

		length = (float) Math.sqrt(sum);

		if (length != 0) {
			ilength = 1.0f / length;

			r[0] = v[0] * ilength;
			r[1] = v[1] * ilength;
			r[2] = v[2] * ilength;
			r[3] = v[3] * ilength;
		}

		return r;
	}

	public static void normalize(float []v, float []r) {
		

		float sum = 0;
		float length = 0;
		float ilength = 0;

		sum = (v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2]) + (v[3] * v[3]);

		length = (float) Math.sqrt(sum);

		if (length != 0) {
			ilength = 1.0f / length;

			r[0] = v[0] * ilength;
			r[1] = v[1] * ilength;
			r[2] = v[2] * ilength;
			r[3] = v[3] * ilength;
		}

	}
	
	public float getX() {
		return array[Vector.X];
	}

	public void setX(float x) {
		array[Vector.X] = x;
	}

	public float getY() {
		return array[Vector.Y];
	}

	public void setY(float y) {
		array[Vector.Y] = y;
	}

	public float getZ() {
		return array[Vector.Z];
	}

	public void setZ(float z) {
		array[Vector.Z] = z;
	}

	public void incrementX(float delta) {
		array[Vector.X] += delta;
	}

	public void incrementY(float delta) {
		array[Vector.Y] += delta;
	}

	public void incrementZ(float delta) {
		array[Vector.Z] += delta;
	}

	public void increment(int axis, float delta) {
		array[axis] += delta;
	}

	public static Vector multiplyAndAdd(Vector a, float scale, Vector b) {
		Vector c = new Vector();

		c.array[0] = a.array[0] + (scale * b.array[0]);
		c.array[1] = a.array[1] + (scale * b.array[1]);
		c.array[2] = a.array[2] + (scale * b.array[2]);
		c.array[3] = a.array[3] + (scale * b.array[3]);
		return c;
	}

	public static Vector multiplyAndAdd(Vector a, Vector scale, Vector b) {
		Vector c = new Vector();

		c.array[0] = a.array[0] + (scale.array[0] * b.array[0]);
		c.array[1] = a.array[1] + (scale.array[1] * b.array[1]);
		c.array[2] = a.array[2] + (scale.array[2] * b.array[2]);
		c.array[3] = a.array[3] + (scale.array[3] * b.array[3]);
		return c;
	}

	public void multiplyAndAdd(Vector a, float scale) {

		array[0] += (scale * a.array[0]);
		array[1] += (scale * a.array[1]);
		array[2] += (scale * a.array[2]);
		array[3] += (scale * a.array[3]);

	}

	public void multiplyAndAdd(Vector a, Vector scale) {

		array[0] += (scale.array[0] * a.array[0]);
		array[1] += (scale.array[1] * a.array[1]);
		array[2] += (scale.array[2] * a.array[2]);
		array[3] += (scale.array[3] * a.array[3]);

	}

	public boolean equals (Object o) {
		
		if (o == null)
			return false;
		if (o instanceof Vector) {
			Vector v = (Vector) o;
			return (array[0] == v.array[0]) && (array[1] == v.array[1]) && (array[1] == v.array[1]) && (array[3] == v.array[3]); 
		}
			
		return false;
	}
	public int hashCode() {
		
		int result = 17;
		
		result = 37 * result + Float.floatToIntBits(array[0]);
		result = 37 * result + Float.floatToIntBits(array[1]);
		result = 37 * result + Float.floatToIntBits(array[2]);
		result = 37 * result + Float.floatToIntBits(array[3]);
		return result;
	}

	public String toString() {
		return "X[" + array[X] + "] Y[" + array[Y] + "] Z["
				+ array[Z]+"]";
	}
	public static Vector newInstance(Vector v) {
		Vector n = new Vector();
		n.array[0] = v.array[0];
		n.array[1] = v.array[1];
		n.array[2] = v.array[2];
		n.array[3] = v.array[3];
		return n;
	}
	
	public static float[] rotate(float vector[],float angle, float u1, float u2, float u3) {
			
		float rotationMatrix[][] = new float[4][4];
		float result[] = new float[4];
		
		float sin_angle = (float)Math.sin(GLUtils.range360(angle));
		float cos_angle = (float)Math.cos(GLUtils.range360(angle));
		
		rotationMatrix [0][0] = ((1-cos_angle)*u1*u1)+cos_angle; 
		rotationMatrix [0][1] = ((1-cos_angle)*u1*u2)-sin_angle*u3; 
		rotationMatrix [0][2] = ((1-cos_angle)*u1*u3)+sin_angle*u2;
		rotationMatrix [0][3] = 0; 
		
		rotationMatrix [1][0] = ((1-cos_angle)*u1*u2)+sin_angle*u3; 
		rotationMatrix [1][1] = ((1-cos_angle)*u2*u2)+cos_angle;  
		rotationMatrix [1][2] = ((1-cos_angle)*u2*u3)-sin_angle*u1; 
		rotationMatrix [1][3] = 0; 
		
		rotationMatrix [2][0] = ((1-cos_angle)*u1*u3)-sin_angle*u2; 
		rotationMatrix [2][1] = ((1-cos_angle)*u2*u3)+sin_angle*u1;
		rotationMatrix [2][2] = ((1-cos_angle)*u3*u3)+cos_angle;  
		rotationMatrix [2][3] = 0;
		
		rotationMatrix [3][0] = 0; 
		rotationMatrix [3][1] = 0; 
		rotationMatrix [3][2] = 0; 
		rotationMatrix [3][3] = 1;
		
		
		for (int i=0; i<4; ++i)
			for (int j=0; j<4; ++j)
				result[i] += rotationMatrix[i][j] * vector[j];
		return result;
	}
}
