package geometric;

import Jama.Matrix;

import java.io.*;
import java.util.*;

import fragmenter.util.*;

/**
 * This class represents a triangle, specified by three points, which are represented by positions in a given
 * vertex list.
 * 
 */
public class Triangle {

	public static final double EPS = Math.pow(10, -10);

	private Matrix center, normal;
	private double surface;
	private int p0, p1, p2;
	private List<Matrix> vs;
	private Matrix rot, rotV0, rotV1, rotV2;

	public Triangle(int p0, int p1, int p2, ArrayList<Matrix> vs) {
		this.p0 = p0;
		this.p1 = p1;
		this.p2 = p2;

		this.vs = vs;
		recalculate();
	}

	public void recalculate() {
		calcNormal();
		calcSurface();
		calcCenter();
		rot = null;
	}

	private void calcNormal() {
		Matrix v0 = getV0();
		Matrix v1 = getV1();
		Matrix v2 = getV2();

		Matrix u = v1.minus(v0);
		Matrix v = v2.minus(v0);

		normal = cross(u, v);
	}

	public static Matrix cross(Matrix u, Matrix v) {
		double[] cross = new double[3];

		cross[0] = u.get(1, 0) * v.get(2, 0) - u.get(2, 0) * v.get(1, 0);
		cross[1] = u.get(2, 0) * v.get(0, 0) - u.get(0, 0) * v.get(2, 0);
		cross[2] = u.get(0, 0) * v.get(1, 0) - u.get(1, 0) * v.get(0, 0);

		return new Matrix(cross, 3);
	}

	public static double dot(Matrix a, Matrix b) {
		Matrix c = a.arrayTimes(b);
		return c.get(0, 0) + c.get(1, 0) + c.get(2, 0);
	}

	private void calcCenter() {
		Matrix v0 = getV0();
		Matrix v1 = getV1();
		Matrix v2 = getV2();

		center = new Matrix(3, 1);
		center.plusEquals(v0);
		center.plusEquals(v1);
		center.plusEquals(v2);
		center.timesEquals((1.0 / 3));
	}

	private void calcSurface() {
		surface = Math.sqrt(Math.pow(normal.get(0, 0), 2) + Math.pow(normal.get(1, 0), 2)
				+ Math.pow(normal.get(2, 0), 2)) / 2;
	}

	public Matrix getV0() {
		return vs.get(getP0());
	}

	public Matrix getV1() {
		return vs.get(getP1());
	}

	public Matrix getV2() {
		return vs.get(getP2());
	}

	public Matrix getCenter() {
		return center;
	}

	public double getSurface() {
		return surface;
	}

	public int getP0() {
		return p0;
	}

	public int getP1() {
		return p1;
	}

	public int getP2() {
		return p2;
	}

	public boolean isIn(Matrix p) {
		Matrix v0 = getV0();
		Matrix v1 = getV1();
		Matrix v2 = getV2();

		int[] r = { 0, 1, 2 };
		int[] c0 = { 0 };
		int[] c1 = { 1 };

		Matrix A = new Matrix(3, 2);
		Matrix g1 = v1.minus(v0);
		Matrix g2 = v2.minus(v0);
		A.setMatrix(r, c0, g1);
		A.setMatrix(r, c1, g2);

		Matrix x = null;

		try {
			x = A.solve(p.minus(v0));
		} catch (RuntimeException e) {
			// point parallel to triangle
			return false;
		}

		double a = x.get(0, 0);
		double b = x.get(1, 0);

		if (Math.abs(a * g1.get(2, 0) + b * g2.get(2, 0) - p.minus(v0).get(2, 0)) > EPS) {
			// System.out.println(Math.abs((a * g1.get(2, 0) + b * g2.get(2, 0)) - p.minus(v0).get(2, 0)));
			return false;
		}

		if (0 <= a && 0 <= b && a + b <= 1) {
			return true;
		}

		return false;
	}

	public Matrix getPointOnPlane(Matrix m) {
		Matrix v0 = getV0();
		Matrix v1 = getV1();
		Matrix v2 = getV2();

		int[] r = { 0, 1, 2 };
		int[] c0 = { 0 };
		int[] c1 = { 1 };
		int[] c2 = { 2 };

		Matrix A = new Matrix(3, 3);
		Matrix g1 = v0.minus(v1);
		Matrix g2 = v0.minus(v2);
		A.setMatrix(r, c0, m);
		A.setMatrix(r, c1, g1);
		A.setMatrix(r, c2, g2);
		Matrix x = null;

		try {
			x = A.solve(v0);
		} catch (RuntimeException ex) {
			// ray m is parallel to triangle-plane
			// throw new NoInterceptionException("Ray m is parallel to triangle-plane");
			return null;
		}
		return m.times(x.get(0, 0));
	}

	public Node getPointOnPlane(Node n) {
		Matrix m = this.getPointOnPlane(n.getVertex());
		return m == null ? null : new InnerNode(m);
	}

	@Override
	public String toString() {
		StringWriter sw = new StringWriter();
		PrintWriter pw = new PrintWriter(sw);
		for (int i = 0; i < 3; i++) {
			pw.println("" + round(getV0().get(i, 0), 6) + "  " + round(getV1().get(i, 0), 6) + "  "
					+ round(getV2().get(i, 0), 6));
		}

		return sw.toString();
	}

	private static double round(double value, int digits) {
		long factor = Math.round(Math.pow(10, digits));
		double a = value * factor;
		int retVal = (int) Math.round(a);
		return (double) retVal / factor;

	}

	public List<List<Integer>> getSphericalExtend(int resx, int resy) {
		List<Integer> thetas = new LinkedList<Integer>();
		List<Integer> phis = new LinkedList<Integer>();

		Node[] n = new Node[3];
		n[0] = new OuterNode(getV0());
		n[1] = new OuterNode(getV1());
		n[2] = new OuterNode(getV2());

		List<Double> phisTmp = new ArrayList<Double>(3);
		List<Double> thetaTmp = new ArrayList<Double>(3);

		for (Node node : n) {
			phisTmp.add(node.getPhi());
			thetaTmp.add(node.getTheta());
		}

		Collections.sort(phisTmp);
		Collections.sort(thetaTmp);

		double phiMin = phisTmp.get(0);
		double phiMax = phisTmp.get(2);
		double thetaMin = thetaTmp.get(0);
		double thetaMax = thetaTmp.get(2);

		double extreme = getThetaExtreme(getV0(), getV1());
		if (0 < extreme && extreme < 1) {
			Matrix ex = getV0().plus(getV1().minus(getV0()).times(extreme));
			double th = new OuterNode(ex).getTheta();
			thetaMin = Math.min(thetaMin, th);
			thetaMax = Math.max(thetaMax, th);
		}

		extreme = getThetaExtreme(getV0(), getV2());
		if (0 < extreme && extreme < 1) {
			Matrix ex = getV0().plus(getV2().minus(getV0()).times(extreme));
			double th = new OuterNode(ex).getTheta();
			thetaMin = Math.min(thetaMin, th);
			thetaMax = Math.max(thetaMax, th);
		}

		extreme = getThetaExtreme(getV2(), getV1());
		if (0 < extreme && extreme < 1) {
			Matrix ex = getV2().plus(getV1().minus(getV2()).times(extreme));
			double th = new OuterNode(ex).getTheta();
			thetaMin = Math.min(thetaMin, th);
			thetaMax = Math.max(thetaMax, th);
		}

		double[][] zArr = { { 0 }, { 0 }, { 1 } };
		Matrix z = new Matrix(zArr);
		Matrix intercept = this.getPointOnPlane(z);
		if (intercept != null && isIn(intercept)) {
			double th = new OuterNode(intercept).getTheta();
			thetaMax = Math.max(thetaMax, th);
			thetaMin = Math.min(thetaMin, th);

			for (int i = 0; i < resx; i++) {
				phis.add(i);
			}
		} else if (phiMax - phiMin > Math.PI) {
			for (int i = 0; i < resx; i++) {
				phis.add(i);
			}
		} else {
			int minPhi = toIndex(phiMin, resx, 2 * Math.PI);
			int maxPhi = toIndex(phiMax, resx, 2 * Math.PI) % resx;
			minPhi = minPhi == 0 ? 0 : minPhi - 1;
			maxPhi = maxPhi == resx - 1 ? resx - 1 : maxPhi + 1;

			for (int i = minPhi; i <= maxPhi; i++) {
				phis.add(i);
			}
		}

		int minTheta = toIndex(thetaMin, resy, Math.PI);
		int maxTheta = toIndex(thetaMax, resy, Math.PI);
		minTheta = minTheta == 0 ? 0 : minTheta - 1;
		maxTheta = maxTheta >= resy - 1 ? resy - 1 : maxTheta + 1;
		for (int i = minTheta; i <= maxTheta; i++) {
			thetas.add(i);
		}

		List<List<Integer>> phisAndThetas = new LinkedList<List<Integer>>();
		phisAndThetas.add(phis);
		phisAndThetas.add(thetas);
		return phisAndThetas;
	}

	private double getThetaExtreme(Matrix A, Matrix B) {
		Matrix C = B.minus(A);
		double x0 = A.get(0, 0);
		double y0 = A.get(1, 0);
		double z0 = A.get(2, 0);

		double x1 = C.get(0, 0);
		double y1 = C.get(1, 0);
		double z1 = C.get(2, 0);

		double extreme = (-(y0 * y0 + x0 * x0) * z1 - (-y0 * y1 - x0 * x1) * z0)
				/ ((y0 * y1 + x0 * x1) * z1 + (-y1 * y1 - x1 * x1) * z0);

		return extreme;
	}

	private int toIndex(double angle, int res, double all) {
		return (int) (angle * res / all);
	}

	public double distanceToPoint(Matrix point) {
		Matrix rot = getRotationMatrix();
		Matrix v0 = rotV0;
		Matrix v1 = rotV1;
		Matrix v2 = rotV2;

		Matrix projectedPoint = rot.times(point.minus(getV0()));
		double d = projectedPoint.get(2, 0);
		projectedPoint.set(2, 0, 0);

		if (isInAnotherTriangle(v0, v1, v2, projectedPoint)) {
			return Math.abs(d);
		}

		// v0-v1
		double projectedDistance = dot(v1, projectedPoint);
		if (0 < projectedDistance && projectedDistance < dot(v1, v1)) {
			Matrix ortho = get2DOrthogonal(v1);
			if (dot(ortho, v2) > 0) {
				ortho.timesEquals(-1);
			}
			if (dot(ortho, projectedPoint) > 0) {
				double e = projectedPoint.get(0, 0);
				return Math.sqrt(e * e + d * d);
			}

		}

		// v0-v2
		projectedDistance = dot(v2, projectedPoint);
		if (0 < projectedDistance && projectedDistance < dot(v2, v2)) {
			Matrix ortho = get2DOrthogonal(v2);
			if (dot(ortho, v1) > 0) {
				ortho.timesEquals(-1);
			}
			if (dot(ortho, projectedPoint) > 0) {
				ortho.timesEquals(1 / ortho.norm2());
				double e = dot(ortho, projectedPoint);
				return Math.sqrt(e * e + d * d);
			}
		}

		// v1-v2
		Matrix v = v2.minus(v1);
		Matrix trProjPoint = projectedPoint.minus(v1);
		projectedDistance = dot(v, trProjPoint);
		if (0 < projectedDistance && projectedDistance < dot(v, v)) {
			Matrix ortho = get2DOrthogonal(v);
			if (dot(ortho, v1.times(-1)) > 0) {
				ortho.timesEquals(-1);
			}
			if (dot(ortho, trProjPoint) > 0) {
				ortho.timesEquals(1 / ortho.norm2());
				double e = dot(ortho, trProjPoint);
				return Math.sqrt(e * e + d * d);
			}
		}

		// not closest to any side. must be to a vertex
		Matrix dv0 = point.minus(getV0());
		double d0 = dot(dv0, dv0);
		Matrix dv1 = point.minus(getV1());
		double d1 = dot(dv1, dv1);
		Matrix dv2 = point.minus(getV2());
		double d2 = dot(dv2, dv2);
		return Math.sqrt(Math.min(Math.min(d0, d1), d2));
	}

	public boolean isInAnotherTriangle(Matrix v0, Matrix v1, Matrix v2, Matrix p) {
		int[] r = { 0, 1, 2 };
		int[] c0 = { 0 };
		int[] c1 = { 1 };

		Matrix A = new Matrix(3, 2);
		Matrix g1 = v1.minus(v0);
		Matrix g2 = v2.minus(v0);
		A.setMatrix(r, c0, g1);
		A.setMatrix(r, c1, g2);

		Matrix x = null;

		try {
			x = A.solve(p.minus(v0));
		} catch (RuntimeException e) {
			// point parallel to triangle
			return false;
		}

		double a = x.get(0, 0);
		double b = x.get(1, 0);

		if (Math.abs(a * g1.get(2, 0) + b * g2.get(2, 0) - p.minus(v0).get(2, 0)) > EPS) {
			// System.out.println(Math.abs((a * g1.get(2, 0) + b * g2.get(2, 0)) - p.minus(v0).get(2, 0)));
			return false;
		}

		if (0 <= a && 0 <= b && a + b <= 1) {
			return true;
		}

		return false;
	}

	private Matrix get2DOrthogonal(Matrix a) {
		Matrix ortho = new Matrix(3, 1);
		ortho.set(0, 0, -a.get(1, 0));
		ortho.set(1, 0, a.get(0, 0));
		return ortho;
	}

	private Matrix getRotationMatrix() {
		if (rot == null) {
			Matrix v0 = getV0();
			Matrix v1 = getV1().minus(v0);
			Matrix v2 = getV2().minus(v0);

			double v1x = v1.get(0, 0);
			double v1y = v1.get(1, 0);

			double hz = Math.sqrt(v1x * v1x + v1y * v1y);

			Matrix rotz = new Matrix(3, 3);
			rotz.set(0, 0, v1y);
			rotz.set(0, 1, -v1x);
			rotz.set(0, 2, 0);
			rotz.set(1, 0, v1x);
			rotz.set(1, 1, v1y);
			rotz.set(1, 2, 0);
			rotz.set(2, 0, 0);
			rotz.set(2, 1, 0);
			rotz.set(2, 2, hz);
			rotz.timesEquals(1 / hz);

			v1 = rotz.times(v1);
			v2 = rotz.times(v2);

			v1y = v1.get(1, 0);
			double v1z = v1.get(2, 0);

			double hx = Math.sqrt(v1z * v1z + v1y * v1y);

			Matrix rotx = new Matrix(3, 3);
			rotx.set(0, 0, hx);
			rotx.set(0, 1, 0);
			rotx.set(0, 2, 0);
			rotx.set(1, 0, 0);
			rotx.set(1, 1, v1y);
			rotx.set(1, 2, v1z);
			rotx.set(2, 0, 0);
			rotx.set(2, 1, -v1z);
			rotx.set(2, 2, v1y);
			rotx.timesEquals(1 / hx);

			v1 = rotx.times(v1);
			v2 = rotx.times(v2);

			double v2x = v2.get(0, 0);
			double v2z = v2.get(2, 0);
			double hy = Math.sqrt(v2z * v2z + v2x * v2x);

			Matrix roty = new Matrix(3, 3);
			roty.set(0, 0, v2x);
			roty.set(0, 1, 0);
			roty.set(0, 2, v2z);
			roty.set(1, 0, 0);
			roty.set(1, 1, hy);
			roty.set(1, 2, 0);
			roty.set(2, 0, -v2z);
			roty.set(2, 1, 0);
			roty.set(2, 2, v2x);
			roty.timesEquals(1 / hy);

			v1 = roty.times(v1);
			v2 = roty.times(v2);

			rot = roty.times(rotx.times(rotz));
			rotV0 = new Matrix(3, 1);
			rotV1 = v1;
			rotV2 = v2;
		}

		return rot;
	}

	public List<Matrix> getVs() {
		return vs;
	}
}