package ProGAL.geom3d.volumes;

import java.util.List;

import ProGAL.geom3d.Point;
import ProGAL.geom3d.PointList;
import ProGAL.geom3d.Vector;
import ProGAL.math.Matrix.EigenvalueDecomposition;
import ProGAL.math.Matrix3x3;

public class OBB implements Volume{
	protected Point anchor;  // midpoint of the box
	protected Vector[] bases;   // unit vectors
	public double[] extents;

	public OBB(Point anchor, Vector xdir, Vector ydir, Vector zdir) {
		this.anchor = anchor;
		extents = new double[]{xdir.length(), ydir.length(), zdir.length()}; 
		bases = new Vector[]{xdir.scaleToLength(1), ydir.scaleToLength(1), zdir.scaleToLength(1)};
	}
	public OBB(Point anchor, Vector[] bases, double[] extents){
		this.anchor = anchor;
		this.bases = bases;
		this.extents = extents;
	}

	public Point getAnchor() { return anchor; }
	public Vector getXDir() { return bases[0]; }
	public Vector getYDir() { return bases[1]; }
	public Vector getZDir() { return bases[2]; }
	public Vector[] getBases() { return bases; }
	public void setAnchor(Point pos) { anchor = pos; }

	public double cutArealYZ() {
		return 4*extents[1]*extents[2];
	}

//	public static Box3d createBoundingBox(List<Point> points, int i, int j) {
//		List<Point> pointSubset = new List<Point>();
//		for (int k = i; k <= j; k++) pointSubset.insert(points.get(k));
//		return createBoundingBox_Covariance(pointSubset);
//	}

	/* 458HOps */
	public static OBB createBoundingBox_Covariance(PointList points){
		Matrix3x3 m = points.getCovariance();
		EigenvalueDecomposition ed = m.getEigenvalueDecomposition();
		ProGAL.geomNd.Vector r = ed.getV().getColumn(2);
		ProGAL.geomNd.Vector s = ed.getV().getColumn(1);
		ProGAL.geomNd.Vector t = ed.getV().getColumn(0);
		OBB ret = createBoxFromBases(new Vector[]{new Vector(r),new Vector(s),new Vector(t)}, points);
//		double[][] coords = new double[3][3];
//		for(int i=0;i<3;i++) for(int j=0;j<3;j++) coords[i][j] = m.get(i, j);
//		Jama.Matrix A = new Jama.Matrix(coords);
//		EigenvalueDecomposition ed = new EigenvalueDecomposition(A);//84HOps
//		Jama.Matrix eV = ed.getV();

//		Vector r = new Vector(eV.get(0, 2), eV.get(1, 2), eV.get(2, 2));
//		Vector s = new Vector(eV.get(0, 1), eV.get(1, 1), eV.get(2, 1));
//		Vector t = r.cross(s);//6HOps
////		Vector t = new Vector(eV.get(0, 0), eV.get(1, 0), eV.get(2, 0));
//		//		Vector[] eigens = m.getEigenvectors();
//		Box3d ret = createBoxFromBases( new Vector[]{r,s,t}, points);//16*18+71
		
		return ret;
	}

//	public static Box3d createBoundingBox_Covariance(Box3d b1, Box3d b2){
//		List<Point> corners = new List<Point>();
//		corners.append(b1.getVertices());//72HOps
//		corners.append(b2.getVertices());//72HOps
//		return createBoundingBox_Covariance(corners);
//	}


	/**
	 * Creates a bounding box enclosing the points in <code>points</code> using the supplied 
	 * bases.
	 * @hops n*9+15
	 */
	public static OBB createBoxFromBases(Vector[] bases, List<Point> points){
		//Find point farthest back along each base
//		Point[] fPoints = new Point[3];
		double[][] extremeDots = {
				{Double.POSITIVE_INFINITY,Double.NEGATIVE_INFINITY},
				{Double.POSITIVE_INFINITY,Double.NEGATIVE_INFINITY},
				{Double.POSITIVE_INFINITY,Double.NEGATIVE_INFINITY} };
		for(int b=0;b<3;b++){
			for(Point p: points){
				double dot = bases[b].dot(p.toVector());//3HOps
				if(dot<extremeDots[b][0]) 
					extremeDots[b][0] = dot;
				if(dot>extremeDots[b][1])
					extremeDots[b][1] = dot;
			}
		}//n*3*3

		Point center = bases[0].multiply((extremeDots[0][0]+extremeDots[0][1])/2).toPoint();//4HOps
		center.addThis(bases[1].multiply((extremeDots[1][0]+extremeDots[1][1])/2));//4HOps
		center.addThis(bases[2].multiply((extremeDots[2][0]+extremeDots[2][1])/2));//4HOps
		
		double[] extents = new double[3];
		extents[0] = (extremeDots[0][1]-extremeDots[0][0])/2;//1HOps
		extents[1] = (extremeDots[1][1]-extremeDots[1][0])/2;//1HOps
		extents[2] = (extremeDots[2][1]-extremeDots[2][0])/2;//1HOps
		
		return new OBB(center,bases,extents);
		
//		//Solve Ax=b
//		double[][] array = {
//				{bases[0].x,bases[0].y,bases[0].z},
//				{bases[1].x,bases[1].y,bases[1].z},
//				{bases[2].x,bases[2].y,bases[2].z}};
//		Jama.Matrix A = new Jama.Matrix(array);
//		Jama.Matrix b = new Jama.Matrix(new double[][]{
//				{Vector.dotProduct(fPoints[0],bases[0])},
//				{Vector.dotProduct(fPoints[1],bases[1])},
//				{Vector.dotProduct(fPoints[2],bases[2])}
//		});//9HOps
//		Jama.Matrix x = A.solve(b);//Est. 50HOps
//
//		Point corner = new Point(x.get(0, 0),x.get(1, 0),x.get(2, 0));
//
//		//Find extents
//		double[] extents = new double[3];
//		for(int base=0;base<3;base++){
//			double maxDot = -1;
//			for(Point point: points){
//				double dot = corner.vectorTo(point).dotProduct(bases[base]);
//				if(dot>maxDot) maxDot = dot;
//			}
//			extents[base] = Math.max(maxDot,Constants.epsilon);
//
//		}//n*3*3
//		bases[0].scaleToLength(extents[0]/2);//4HOps
//		bases[1].scaleToLength(extents[1]/2);//4HOps
//		bases[2].scaleToLength(extents[2]/2);//Bases now have the right dimension//4HOps
//		corner.addIn(bases[0]);
//		corner.addIn(bases[1]);
//		corner.addIn(bases[2]);//Corner is now center
		
//		return new Box3d(corner, bases[0], bases[1], bases[2]);
	}


	/** Returns true iff this box has an overlap with b. Uses the separating axis implementation 
	 * described in "Realtime collision detection" by Christer Ericsson.  30 -> 27+9+9+18+54=117HOps*/
	public boolean overlaps(OBB b){
		double[][] R = new double[3][3], AbsR = new double[3][3];
		double[] ae = extents, be = b.extents;

		// Compute rotation matrix expressing b in a___s coordinate frame
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
				R[i][j] = bases[i].dot(b.bases[j]);	
		//Total: 3*3*3 = 27HOps

		// Compute translation vector t
		Vector tmp = this.anchor.vectorTo(b.anchor);

		// Bring translation into a___s coordinate frame
		tmp = new Vector(tmp.dot(bases[0]), tmp.dot(bases[1]), tmp.dot(bases[2]));//9HOps
		double[] t = new double[]{tmp.x(), tmp.y(), tmp.z()};

		// Compute common subexpressions. Add in an epsilon term to
		// counteract arithmetic errors when two edges are parallel and
		// their cross product is (near) null (see text for details)
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++)
				AbsR[i][j] = Math.abs(R[i][j])+0.00001;

		// Test axes L = A0, L = A1, L = A2
		for (int i = 0; i < 3; i++) 
			if (Math.abs(t[i]) > ae[i] + be[0]*AbsR[i][0]+be[1]*AbsR[i][1]+be[2]*AbsR[i][2]) return false; //9HOps
		// Test axes L = B0, L = B1, L = B2
		for (int i = 0; i < 3; i++) 
			if (Math.abs(t[0]*R[0][i] + t[1]*R[1][i] + t[2]*R[2][i]) > ae[0]*AbsR[0][i] + ae[1]*AbsR[1][i] + ae[2]*AbsR[2][i] + be[i]) return false;  //18HOps

		//For all the rest: 9*6 = 54HOps
		// Test axis L = A0 x B0
		if (Math.abs(t[2] * R[1][0] - t[1] * R[2][0]) > ae[1] * AbsR[2][0] + ae[2] * AbsR[1][0] + be[1] * AbsR[0][2] + be[2] * AbsR[0][1]) return false;
		// Test axis L = A0 x B1
		if (Math.abs(t[2] * R[1][1] - t[1] * R[2][1]) > ae[1] * AbsR[2][1] + ae[2] * AbsR[1][1] + be[0] * AbsR[0][2] + be[2] * AbsR[0][0]) return false;
		// Test axis L = A0 x B2
		if (Math.abs(t[2] * R[1][2] - t[1] * R[2][2]) > ae[1] * AbsR[2][2] + ae[2] * AbsR[1][2] + be[0] * AbsR[0][1] + be[1] * AbsR[0][0]) return false;
		// Test axis L = A1 x B0
		if (Math.abs(t[0] * R[2][0] - t[2] * R[0][0]) > ae[0] * AbsR[2][0] + ae[2] * AbsR[0][0] + be[1] * AbsR[1][2] + be[2] * AbsR[1][1]) return false;
		// Test axis L = A1 x B1
		if (Math.abs(t[0] * R[2][1] - t[2] * R[0][1]) > ae[0] * AbsR[2][1] + ae[2] * AbsR[0][1] + be[0] * AbsR[1][2] + be[2] * AbsR[1][0]) return false;
		// Test axis L = A1 x B2
		if (Math.abs(t[0] * R[2][2] - t[2] * R[0][2]) > ae[0] * AbsR[2][2] + ae[2] * AbsR[0][2] + be[0] * AbsR[1][1] + be[1] * AbsR[1][0]) return false;
		// Test axis L = A2 x B0
		if (Math.abs(t[1] * R[0][0] - t[0] * R[1][0]) > ae[0] * AbsR[1][0] + ae[1] * AbsR[0][0] + be[1] * AbsR[2][2] + be[2] * AbsR[2][1]) return false;
		// Test axis L = A2 x B1
		if (Math.abs(t[1] * R[0][1] - t[0] * R[1][1]) > ae[0] * AbsR[1][1] + ae[1] * AbsR[0][1] + be[0] * AbsR[2][2] + be[2] * AbsR[2][0]) return false;
		// Test axis L = A2 x B2
		if (Math.abs(t[1] * R[0][2] - t[0] * R[1][2]) > ae[0] * AbsR[1][2] + ae[1] * AbsR[0][2] + be[0] * AbsR[2][1] + be[1] * AbsR[2][0]) return false;
		// Since no separating axis is found, the OBBs must be intersecting
		return true;
	}


	public Point getCenter() {
		return this.getAnchor();
	}


	/* 3*3*8 = 72HOps */
	public Point[] getVertices(){
		Point[] ret = new Point[8];
		ret[0] = anchor.subtract(bases[0].multiply(extents[0])).subtractThis(bases[1].multiply(extents[1])).subtractThis(bases[2].multiply(extents[2]));
		ret[1] = anchor.subtract(bases[0].multiply(extents[0])).subtractThis(bases[1].multiply(extents[1])).addThis(bases[2].multiply(extents[2]));
		ret[2] = anchor.subtract(bases[0].multiply(extents[0])).addThis(bases[1].multiply(extents[1])).subtractThis(bases[2].multiply(extents[2]));
		ret[3] = anchor.subtract(bases[0].multiply(extents[0])).addThis(bases[1].multiply(extents[1])).addThis(bases[2].multiply(extents[2]));
		ret[4] = anchor.add(bases[0].multiply(extents[0])).subtractThis(bases[1].multiply(extents[1])).subtractThis(bases[2].multiply(extents[2]));
		ret[5] = anchor.add(bases[0].multiply(extents[0])).subtractThis(bases[1].multiply(extents[1])).addThis(bases[2].multiply(extents[2]));
		ret[6] = anchor.add(bases[0].multiply(extents[0])).addThis(bases[1].multiply(extents[1])).subtractThis(bases[2].multiply(extents[2]));
		ret[7] = anchor.add(bases[0].multiply(extents[0])).addThis(bases[1].multiply(extents[1])).addThis(bases[2].multiply(extents[2]));
		return ret;
	}


	public boolean overlaps(Volume s) {
		if(s instanceof OBB) return overlaps((OBB)s);
		throw new Error("Unknown volume");
	}

	public double volume() {
		return extents[0]*extents[1]*extents[2]*8;
	}

	public OBB clone(){
		Point aClone = anchor.clone();
		Vector[] bClone = {bases[0].clone(),bases[1].clone(),bases[2].clone()};
		double[] eClone = {extents[0],extents[1],extents[2]};
		return new OBB(aClone,bClone,eClone);

	}

	public String toString(){
		return "Box3d["+getAnchor()+","+bases[0]+","+bases[1]+","+bases[2]+"]";
	}

	@Override
	public double getVolume() {
		// TODO Auto-generated method stub
		return 0;
	}

}