package framework.model3D;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.List;

import javax.media.j3d.BoundingPolytope;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Transform3D;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector4d;

public class OBB implements Cloneable {
	private ArrayList<Vector3d> vertexList = new ArrayList<Vector3d>();
	private Vector4d[] plane;
	private BoundingPolytope bp = null;
	private static int edges[][] = { { 0, 1 }, { 0, 2 }, { 1, 3 }, { 2, 3 },
									{ 0, 4 }, { 1, 5 }, { 3, 7 }, { 2, 6 }, 
									{ 4, 5 }, { 4, 6 }, { 5, 7 }, { 6, 7 } };
	private static Integer planes[][] = { {0, 2, 3, 1}, {0, 1, 5, 4},
									 {1, 3, 7, 5}, {4, 5, 7, 6}, 
									 {0, 4, 6, 2}, {2, 6, 7, 3}};
	private static boolean[][] inside = new boolean[8][6];
	
	public OBB() {
	}
	
	public OBB(Vector3d v1, Vector3d v2, Vector3d v3, Vector3d v4, 
			Vector3d v5, Vector3d v6, Vector3d v7, Vector3d v8) {
		vertexList.add(v1);
		vertexList.add(v2);
		vertexList.add(v3);
		vertexList.add(v4);
		vertexList.add(v5);
		vertexList.add(v6);
		vertexList.add(v7);
		vertexList.add(v8);
		createPlanes();
	}

	public void addVertex(Vector3d v) {
		vertexList.add(v);
	}
	
	public Vector3d getVertex(int i) {
		return vertexList.get(i);
	}
	
	public BoundingPolytope getBoundingPolytope() {
		return bp;
	}

	public BoundingSphere getBoundingSphere() {
		double radius = 0.0;
		Point3d p = new Point3d();
		Vector3d cv = new Vector3d();
		for (int i = 0; i < vertexList.size() - 1; i++) {
			for (int j = i + 1; j < vertexList.size(); j++) {
				Vector3d v = new Vector3d();
				v.sub(vertexList.get(i), vertexList.get(j));
				if (radius < v.length()) {
					radius = v.length();
					cv.add(vertexList.get(i), vertexList.get(j));
					cv.scale(0.5);
					p.x = cv.x;
					p.y = cv.y;
					p.z = cv.z;
				}
			}
		}
		BoundingSphere s = new BoundingSphere(p, radius / 2);
		return s;
	}
	
	/**
	 * 頂点情報を元に面および境界多面体を生成する
	 */
	public void createPlanes() {
		// 面の作成
		Vector3d v1 = new Vector3d();
		Vector3d v2 = new Vector3d();
		Vector3d v3 = new Vector3d();
		Vector4d[] plane = new Vector4d[6];
	
		// 0231
		v1 = getVertex(0);
		v2.sub(getVertex(2), getVertex(0));
		v3.sub(getVertex(1), getVertex(0));
		Vector3d n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[0] = new Vector4d();
		plane[0].set(n.x, n.y, n.z, -n.dot(v1));
		
	
		// 0154
		v1 = getVertex(0);
		v2.sub(getVertex(1), getVertex(0));
		v3.sub(getVertex(4), getVertex(0));
		n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[1] = new Vector4d();
		plane[1].set(n.x, n.y, n.z, -n.dot(v1));
	
		// 1375
		v1 = getVertex(1);
		v2.sub(getVertex(3), getVertex(1));
		v3.sub(getVertex(5), getVertex(1));
		n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[2] = new Vector4d();
		plane[2].set(n.x, n.y, n.z, -n.dot(v1));
	
		// 4576
		v1 = getVertex(6);
		v2.sub(getVertex(4), getVertex(6));
		v3.sub(getVertex(7), getVertex(6));
		n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[3] = new Vector4d();
		plane[3].set(n.x, n.y, n.z, -n.dot(v1));
	
		// 0462
		v1 = getVertex(6);
		v2.sub(getVertex(2), getVertex(6));
		v3.sub(getVertex(4), getVertex(6));
		n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[4] = new Vector4d();
		plane[4].set(n.x, n.y, n.z, -n.dot(v1));
	
		// 2673
		v1 = getVertex(6);
		v2.sub(getVertex(7), getVertex(6));
		v3.sub(getVertex(2), getVertex(6));
		n = new Vector3d();
		n.cross(v2, v3);
		n.normalize();
		plane[5] = new Vector4d();
		plane[5].set(n.x, n.y, n.z, -n.dot(v1));
		
		this.plane = plane;
		
		bp = new BoundingPolytope();
		bp.setPlanes(plane);
	}

	/**
	 * 平面との衝突判定
	 * @param plane 衝突判定の対象となる平面
	 * @return 衝突判定の結果（衝突していない場合はnull）
	 */
	public CollisionResult intersect(Vector4d plane) {
		int i = 0;
		boolean inside = false;
		boolean outside = false;
		int count = 0;
		Vector3d center = new Vector3d(0, 0, 0);
		ArrayList<Vector3d> collisionPoints = new ArrayList<Vector3d>();
		double l = Math.sqrt(plane.x * plane.x + plane.y * plane.y + plane.z * plane.z);
		double length;
		double deepest = 0.0;
		Vector3d v;
		for (i = 0; i < vertexList.size(); i++) {
			v = vertexList.get(i);
			if (GeometryUtility.inside(v, plane)) {
				inside = true;
				length = -(v.x * plane.x + v.y * plane.y + v.z * plane.z + plane.w) / l;
				if (length > deepest + GeometryUtility.TOLERANCE) {
					center.set(v);
					collisionPoints.clear();
					collisionPoints.add(v);
					count = 1;
					deepest = length;
				} else if (length >= deepest - GeometryUtility.TOLERANCE) {
					center.add(v);
					collisionPoints.add(v);
					count++;			
				}
			} else {
				outside = true;				
			}
		}

		if (!inside || !outside) {
			// 全頂点が外側か全頂点が内側の場合
			return null;
		}

		center.scale(1.0 / count);

		CollisionResult cr = new CollisionResult();
		cr.length = deepest;
		cr.collisionPoint.setVector3d(center);
		cr.collisionPoints = collisionPoints;
		cr.normal.setX(plane.x / l);
		cr.normal.setY(plane.y / l);
		cr.normal.setZ(plane.z / l);

		return cr;
	}

	/**
	 * OBBとの衝突判定
	 * @param another 衝突判定の対象となるOBB
	 * @return 衝突判定の結果（衝突していない場合はnull）
	 */
	public CollisionResult intersect(OBB another) {
		// anotherの各頂点がthisの各面の内側にあるかを調べる
		// i：anotherの頂点, j：thisの面
		OBB o1 = this;
		OBB o2 = another;
		for (int i = 0; i < o2.vertexList.size(); i++) {
			for (int j = 0; j < o1.plane.length; j++) {
				inside[i][j] = GeometryUtility.inside(o2.vertexList.get(i), o1.plane[j]);
			}
		}

		int v = 0;
		boolean bCollides = false;

		// anotherの各頂点がthisに包含されているか？
		Vector3d contactCenter = new Vector3d();
		int count = 0;
		for (int i = 0; i < o2.vertexList.size(); i++) {
			boolean f1 = false;
			for (int p = 0; p < o1.plane.length; p++) {
				f1 = inside[i][p];
				if (!f1) break;		// 包含されていない
			}
			if (f1) {
				// thisのすべての面の内側 ＝ thisに包含されている
				if (!bCollides) {
					bCollides = true;
					v = i;
				}
				contactCenter.add(o2.vertexList.get(i));
				count++;
			}
		}
		if (!bCollides) {
			// thisの各頂点がanotherの各面の内側にあるかを調べる
			o1 = another;
			o2 = this;
			for (int i = 0; i < o2.vertexList.size(); i++) {
				for (int j = 0; j < o1.plane.length; j++) {
					inside[i][j] = GeometryUtility.inside(o2.vertexList.get(i), o1.plane[j]);
				}
			}
			// thisの各頂点がanotherに包含されているか？
			for (int i = 0; i < o2.vertexList.size(); i++) {
				boolean f1 = false;
				for (int p = 0; p < o1.plane.length; p++) {
					f1 = inside[i][p];
					if (!f1) break;		// 包含されていない
				}
				if (f1) {
					// anotherのすべての面の内側 ＝ anotherに包含されている
					if (!bCollides) {
						bCollides = true;
						v = i;
					}
					contactCenter.add(o2.vertexList.get(i));
					count++;
				}
			}			
		}
		
		CollisionResult cr = new CollisionResult();
		ArrayList<Vector3d> collisionPoints = new ArrayList<Vector3d>();
		if (bCollides) {
			// いずれかのOBBの頂点vが他方のOBBに包含されていた場合(vを持つ側のOBBをo2、他方をo1とする)
			// o1のどの面と衝突したかを判定する
			double lMin = 0;
			int p1 = -1;
			Vector3d normal = null;
			contactCenter.scale(1.0 / (double)count);
			for (int p = 0; p < o1.plane.length; p++) {
				Vector3d n = new Vector3d(o1.plane[p].x, o1.plane[p].y, o1.plane[p].z);
				double l = -(contactCenter.x * o1.plane[p].x + contactCenter.y * o1.plane[p].y + 
						contactCenter.z * o1.plane[p].z + o1.plane[p].w) / n.length();
				if (lMin > l || normal == null) {
					lMin = l;
					p1 = p;
					normal = n;
				}
			}
			
			// o2上のvを含む辺のうち衝突面の法線と逆方向に向かっている辺を探す(そのような辺の数によって接触状況が変わる)
			Vector3d collisionPoint = o2.vertexList.get(v);
			Vector3d d1;
			ArrayList<Integer> contactEdges = new ArrayList<Integer>();
			for (int e = 0; e < edges.length; e++) {
				if (v == edges[e][0]) {
					d1 = new Vector3d(o2.vertexList.get(edges[e][1]));
					d1.sub(collisionPoint);
					if (normal.dot(d1) < GeometryUtility.TOLERANCE) {
						// 衝突面に接している辺
						contactEdges.add(edges[e][1]);		// vの反対側の端点
					}
				} else if (v == edges[e][1]) {
					d1 = new Vector3d(o2.vertexList.get(edges[e][0]));					
					d1.sub(collisionPoint);
					if (normal.dot(d1) < GeometryUtility.TOLERANCE) {
						// 衝突面に接している辺
						contactEdges.add(edges[e][0]);		// vの反対側の端点
					}
				}
			}
			
			// 接触状況に応じた処理
			if (contactEdges.size() == 0) {
				// a) 頂点で接触
				collisionPoints.add(collisionPoint);
				cr.collisionPoint.setVector3d(collisionPoint);
			} else if (contactEdges.size() == 1) {
				// b) 辺で接触
				boolean f1 = true;
				Vector3d otherPoint = o2.vertexList.get(contactEdges.get(0));
				Vector3d intersectionPoint = null;
				double nearest = 0.0;
				for (int p = 0; p < o1.plane.length; p++) {
					if (!inside[contactEdges.get(0)][p]) {
						// collisionPoint と　otherPoint の間を通る平面
						Vector3d ip = GeometryUtility.intersect(o1.plane[p], collisionPoint, otherPoint);
						if (ip != null) {
							f1 = false;		// 包含されていない
							Vector3d ip2 = (Vector3d)ip.clone();
							ip2.sub(collisionPoint);
							double d = ip2.length();
							if (intersectionPoint == null || d < nearest) {
								intersectionPoint = ip;
								nearest = d;
							}
						}
					}
				}
				if (f1) {
					// b-1) 辺全体が接触している
					collisionPoints.add(collisionPoint);
					collisionPoints.add(otherPoint);
					Vector3d center = new Vector3d(collisionPoint);
					center.add(otherPoint);
					center.scale(0.5);
					cr.collisionPoint.setVector3d(center);
				} else {
					// b-2) 辺の一部が接触している
					collisionPoints.add(collisionPoint);
					collisionPoints.add(intersectionPoint);
					Vector3d center = new Vector3d(collisionPoint);
					center.add(intersectionPoint);
					center.scale(0.5);
					cr.collisionPoint.setVector3d(center);
				}
			} else {
				// c) 面で接触
				Vector3d center = new Vector3d();
				int v2 = contactEdges.get(0);
				int v3 = contactEdges.get(1);
				int p2;
				for (p2 = 0; p2 < o2.plane.length; p2++) {
					if (v == planes[p2][0] || v == planes[p2][1] || v == planes[p2][2] || v == planes[p2][3]) {
						if (v2 == planes[p2][0] || v2 == planes[p2][1] || v2 == planes[p2][2] || v2 == planes[p2][3]) {
							if (v3 == planes[p2][0] || v3 == planes[p2][1] || v3 == planes[p2][2] || v3 == planes[p2][3]) {
								break;
							}							
						}						
					}
				}
				// o1の面p1とo2の面p2が接している
				// 面と面の共通領域を求める
				for (v2 = 0; v2 < 4; v2++) {
					if (planes[p2][v2] == v) break;
				}
				Vector3d d2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + 1) % 4]).clone();
				d2.sub(collisionPoint);
				d2.cross(normal, d2);
				int sign = -1;
				int v1;
				for (v1 = 4; v1 >= 0; v1--) {
					d1 = (Vector3d)o1.vertexList.get(planes[p1][v1 % 4]).clone();
					d1.sub(o1.vertexList.get(planes[p1][(v1 + 1) % 4]));
					if (d2.dot(d1) > -GeometryUtility.TOLERANCE) {
						sign = 1;
					} else if (sign == 1) {
						// 内積の符号が正から負に変わったとき
						v1 = (v1 + 1) % 4;
						break;
					}
				}
				Vector3d vp1, vp2, dst1, dst2, src1, src2, d3;
				ArrayList<Vector3d[]> contours = new ArrayList<Vector3d[]>();
				for (int i = 0; i < 4; i++) {
					dst1 = vp1 = (Vector3d)o1.vertexList.get(planes[p1][(v1 - i + 4) % 4]).clone();
					src2 = vp2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + i) % 4]).clone();
					src1 = (Vector3d)o1.vertexList.get(planes[p1][(v1 - i + 4 + 1) % 4]).clone();
					dst2 = (Vector3d)o2.vertexList.get(planes[p2][(v2 + i + 1) % 4]).clone();
					d1 = (Vector3d)dst1.clone();
					d1.sub(src1);
					d2 = (Vector3d)dst2.clone();
					d2.sub(src2);
					d3 = (Vector3d)vp2.clone();
					d3.sub(vp1);
					d1.cross(d1, normal);
					if (d1.dot(d3) < GeometryUtility.TOLERANCE) {
						// o1の辺<src1,dst1>は、頂点vp2の内側にあるので、共通領域の輪郭を構成する
						contours.add(new Vector3d[]{src1, dst1});
					}
					d3.negate();
					d2.cross(d2, normal);
					if (d2.dot(d3) < GeometryUtility.TOLERANCE) {
						// o2の辺<src2,dst2>は、頂点vp1の内側にあるので、共通領域の輪郭を構成する
						contours.add(new Vector3d[]{src2, dst2});
					}
				}
				Vector3d[] edge1, edge2;
				Vector3d d;
				Vector4d p;
				for (int i = 0; i < contours.size(); i++) {
					edge1 = contours.get(i);
					edge2 = contours.get((i + 1) % contours.size());
					if (edge1[1].equals(edge2[0])) {
						// edge1とedge2は始点と終点を共有している
						if (!collisionPoints.contains(edge1[1])) {
							collisionPoints.add(edge1[1]);
							center.add(edge1[1]);
						}
					} else if (!edge1[0].equals(edge2[0]) && !edge1[1].equals(edge2[1])) {
						// edge1とedge2の交点を求める
						d = (Vector3d)edge2[1].clone();
						d.add(normal);
						p = GeometryUtility.createPlane(edge2[0], edge2[1], d);
						if (p != null) {
							d = GeometryUtility.intersect(p, edge1[0], edge1[1]);
							if (d != null && !collisionPoints.contains(d)) {
								collisionPoints.add(d);
								center.add(d);
							}
						}
					}
				}
				if (collisionPoints.size() <= 2) return null;			// 面で接触しているはずが2点以下である場合、衝突していない
				center.scale(1.0 / (double)collisionPoints.size());
				cr.collisionPoint.setVector3d(center);
			}
			cr.length = lMin;
			cr.collisionPoints = collisionPoints;
			if (o2 == this) normal.negate();
			normal.normalize();
			cr.normal = normal;
			return cr;
		} else {
			// anotherの辺がthisの面と交わっているか？
			Vector3d center = new Vector3d();
			Vector3d normal = new Vector3d();
			ArrayList<Vector3d> intersections = new ArrayList<Vector3d>();
			ArrayList<Integer> contactPlanes = new ArrayList<Integer>();
			for (int e = 0; e < edges.length; e++) {
				// anotherの各辺eに対して処理
				Vector3d intersection = null;
				intersections.clear();
				contactPlanes.clear();
				for (int p = 0; p < this.plane.length; p++) {
					if (inside[edges[e][0]][p] == !inside[edges[e][1]][p]) {
						// anotherの辺eが、thisの面pを貫いている
						Vector3d ip = GeometryUtility.intersect(
								this.plane[p],
								another.vertexList.get(edges[e][0]), 
								another.vertexList.get(edges[e][1]));
						// eとpの交点がthisに包含されているか?
						if (ip != null) {
							intersection = ip;
							boolean bInside = true;
							for (int p2 = 0; p2 < 6; p2++) {
								if (p != p2
									&& !GeometryUtility.inside(intersection, this.plane[p2])) {
									bInside = false;
									break;
								}
							}
							if (bInside) {
								// eがpと交わっている
								contactPlanes.add(p);
								intersections.add(intersection);
								bCollides = true;
							}
						}
					}
				}
				if (contactPlanes.size() == 1) {
					// eと交わっているthisの面が1つ
					collisionPoints.add(intersection);
					center.add(intersection);
				} else if (contactPlanes.size() == 2) {
					// eと交わっているthisの面が2つ
					int p1 = contactPlanes.get(0);
					int p2 = contactPlanes.get(1);
					ArrayList<Integer> p1vs = new ArrayList<Integer>(Arrays.asList(planes[p1]));
					ArrayList<Integer> p2vs = new ArrayList<Integer>(Arrays.asList(planes[p2]));
					p1vs.retainAll(p2vs);
					if (p1vs.size() == 2) {
						// p1とp2は隣り合う面(anotherの辺eはthisのある辺と接触している)
						Vector3d v1 = this.vertexList.get(p1vs.get(0));		// p1とp2の間の辺の始点
						Vector3d v2 = this.vertexList.get(p1vs.get(1));		// p1とp2の間の辺の終点
						Vector3d d1 = (Vector3d)v1.clone();
						d1.sub(v2);
						Vector3d d2 = (Vector3d)another.vertexList.get(edges[e][0]).clone();
						d2.sub(another.vertexList.get(edges[e][1]));
						normal.cross(d1, d2);		// 辺eと、p1とp2の間の辺の両方に垂直なベクトルを法線方向とする
						normal.normalize();
						Vector3d n1 = new Vector3d(this.plane[p1].x, this.plane[p1].y, this.plane[p1].z);
						Vector3d n2 = new Vector3d(this.plane[p2].x, this.plane[p2].y, this.plane[p2].z);
						n1.normalize();
						n2.normalize();
						n1.add(n2);
						n1.scale(0.5);
						if (n1.dot(normal) < -GeometryUtility.TOLERANCE) {
							normal.negate();
						}
						intersection = intersections.get(0);
						intersection.add(intersections.get(1));
						intersection.scale(0.5);
						center.add(intersection);				// 衝突点は1つ
						collisionPoints.add(intersection);
						Vector3d c = GeometryUtility.nearest(v1, v2, center);
						c.sub(center);
						cr.length = c.length();
						cr.normal = normal;
					} else {
						// p1とp2は向かい合う面(anotherの辺eはthisのある面と接触している)
						center.add(intersections.get(0));		// 衝突点は2つ
						center.add(intersections.get(1));
						collisionPoints.add(intersections.get(0));
						collisionPoints.add(intersections.get(1));
						// thisのどの面と衝突したかを判定する
						double lMin = 0;
						normal = null;
						Vector4d plane;
						for (int p = 0; p < this.plane.length; p++) {
							plane = this.plane[p];
							Vector3d n = new Vector3d(plane.x, plane.y, plane.z);
							double l = -(center.x * plane.x + center.y * plane.y + center.z * plane.z + plane.w) / n.length();
							if (lMin > l || normal == null) {
								lMin = l;
								normal = n;
							}
						}
						normal.normalize();
						cr.length = lMin;
						cr.normal = normal;
					}
				}
			}
			if (!bCollides) return null;
			center.scale(1.0 / (double)collisionPoints.size());
			cr.collisionPoint.setVector3d(center);
			cr.collisionPoints = collisionPoints;
			return cr;
		}
	}

	public Object clone() {
		OBB obb = new OBB();
		obb.plane = new Vector4d[6]; 
		for (int i = 0; i < plane.length; i++) {
			obb.plane[i] = (Vector4d) plane[i].clone();

		}
		for (int i = 0; i < vertexList.size(); i++) {
			obb.vertexList.add((Vector3d) vertexList.get(i).clone());
		}
		obb.bp = new BoundingPolytope(obb.plane);

		// System.out.println("veretexListSIZE:"+vertexList.size());
		return obb;
	}

	public void transform(Transform3D t) {
		// TODO Auto-generated method stub
		bp.transform(t);
		bp.getPlanes(plane);
		// for(int i = 0; i<plane.length;i++){
		// System.out.println("plane");
		// System.out.print(plane[i].x + "," + plane[i].y + "," + plane[i].z +
		// "," + plane[i].w);
		// t.transform(plane[i]);
		// System.out.println("-->" + plane[i].x + "," + plane[i].y + "," +
		// plane[i].z + "," + plane[i].w);
		// }
		for (int i = 0; i < vertexList.size(); i++) {
			// System.out.println("vertex");
			// System.out.print(vertexList.get(i).x + "," + vertexList.get(i).y
			// + "," + vertexList.get(i).z);
			Matrix4d mat4d = new Matrix4d();
			t.get(mat4d);
			double x = mat4d.m00 * vertexList.get(i).x + mat4d.m01
					* vertexList.get(i).y + mat4d.m02 * vertexList.get(i).z
					+ mat4d.m03;
			double y = mat4d.m10 * vertexList.get(i).x + mat4d.m11
					* vertexList.get(i).y + mat4d.m12 * vertexList.get(i).z
					+ mat4d.m13;
			double z = mat4d.m20 * vertexList.get(i).x + mat4d.m21
					* vertexList.get(i).y + mat4d.m22 * vertexList.get(i).z
					+ mat4d.m23;
			vertexList.get(i).x = x;
			vertexList.get(i).y = y;
			vertexList.get(i).z = z;
			// t.transform(vertexList.get(i));
			// System.out.println("-->" + vertexList.get(i).x + "," +
			// vertexList.get(i).y + "," + vertexList.get(i).z);
		}
	}
}
