/*-
 *  Copyright 2020 Beuth Hochschule für Technik Berlin, Hochschule für Technik Stuttgart
 * 
 *  This file is part of CityDoctor2.
 *
 *  CityDoctor2 is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  CityDoctor2 is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with CityDoctor2.  If not, see <https://www.gnu.org/licenses/>.
 */
package de.hft.stuttgart.citydoctor2.math;

import java.io.Serializable;

/**
 * A bounded line between two points
 * 
 * @author Matthias Betz
 *
 */
public class Segment3d implements Serializable {

	private static final long serialVersionUID = -506306945169934099L;

	private static final double PRECISION = 0.00000001;

	private static final double EPSILON = 0.01;

	private final Vector3d pointA;
	private final Vector3d pointB;

	public Segment3d(Vector3d pointA, Vector3d pointB) {
		this.pointA = pointA;
		this.pointB = pointB;
	}

	/**
	 * calculates the distance from this segment to the other segment
	 * 
	 * @param otherSeg the other segment
	 * @return the distance
	 */
	public double getDistance(Segment3d otherSeg) {
		return getDistanceResult(otherSeg).getDistance();
	}

	public DistanceResult getDistanceResult(Segment3d other) {
		// calculate direction vectors and normalize them
		Vector3d u = pointB.minus(pointA);
		Vector3d v = other.getPointB().minus(other.getPointA());
		Vector3d w = pointA.minus(other.getPointA());

		double a = u.dot(u);
		double b = u.dot(v);
		double c = v.dot(v);
		double d = u.dot(w);
		double e = v.dot(w);
		double f = a * c - b * b;
		double sc;
		double sN;
		double sD = f;
		double tc;
		double tN;
		double tD = f;

		if (f < PRECISION) {
			sN = 0.0;
			sD = 1.0;
			tN = e;
			tD = c;
		} else {
			sN = b * e - c * d;
			tN = a * e - b * d;
			if (sN < 0.0) {
				sN = 0.0;
				tN = e;
				tD = c;
			} else if (sN > sD) {
				sN = sD;
				tN = e + b;
				tD = c;
			}
		}

		if (tN < 0.0) {
			tN = 0.0;
			if (-d < 0.0) {
				sN = 0.0;
			} else if (-d > a) {
				sN = sD;
			} else {
				sN = -d;
				sD = a;
			}
		} else if (tN > tD) {
			tN = tD;
			if ((-d + b) < 0.0) {
				sN = 0;
			} else if ((-d + e > a)) {
				sN = sD;
			} else {
				sN = (-d + b);
				sD = a;
			}
		}

		sc = getSc(sN, sD);
		tc = getSc(tN, tD);

		Vector3d us = u.mult(sc);
		Vector3d ut = v.mult(tc);
		Vector3d point1 = pointA.plus(us);
		Vector3d point2 = other.pointA.plus(ut);
		Vector3d dP = w.plus(us.minus(ut));
		double length = dP.getLength();
		return new DistanceResult(point1, point2, length);
	}

	private double getSc(double sN, double sD) {
		double sc;
		if (Math.abs(sN) < PRECISION) {
			sc = 0.0;
		} else {
			sc = sN / sD;
		}
		return sc;
	}

	public Vector3d getPointA() {
		return pointA;
	}

	public Vector3d getPointB() {
		return pointB;
	}

	public double getDistance(Vector3d v) {
		Vector3d ab = pointB.minus(pointA);
		Vector3d av = v.minus(pointA);

		if (av.dot(ab) <= 0.0) {
			return av.getLength();
		}

		Vector3d bv = v.minus(pointB);

		if (bv.dot(ab) >= 0.0) {
			return bv.getLength();
		}

		Vector3d cross = ab.cross(av);
		return cross.getLength() / ab.getLength();
	}

	public Vector3d getNormalThroughPoint(Vector3d p) {
		Vector3d dir = p.minus(pointA);
		Vector3d lineDir = pointB.minus(pointA);

		double lineLengthSquare = lineDir.getSquaredLength();

		double scalar = dir.dot(lineDir) / lineLengthSquare;
		Vector3d projectedPoint = (lineDir.mult(scalar)).plus(pointA);
		return p.minus(projectedPoint);
	}

	@Override
	public String toString() {
		return "Segment3d [pointA=" + pointA + ", pointB=" + pointB + "]";
	}

	public Vector3d intersection(Triangle3d triangle) {
		Vector3d v0 = triangle.getP1();
		Vector3d v1 = triangle.getP2();
		Vector3d v2 = triangle.getP3();
		Vector3d edge1 = v1.minus(v0);
		Vector3d edge2 = v2.minus(v0);
		Vector3d direction = pointB.minus(pointA);
		Vector3d p = direction.cross(edge2);
		double det = edge1.dot(p);

		if (det > -EPSILON && det < EPSILON) {
			return null;
		}

		double invDet = 1 / det;

		Vector3d s = pointA.minus(v0);

		double u = invDet * s.dot(p);
		if (u < EPSILON || u > (1.0 - EPSILON)) {
			return null;
		}

		Vector3d q = s.cross(edge1);

		double v = invDet * direction.dot(q);

		if (v < 0.0 || u + v > 1.0) {
			return null;
		}

		double t = edge2.dot(q) * invDet;
		if (t > EPSILON && t < 1 - EPSILON) {
			// t is in segment
			return pointA.plus(direction.mult(t));
		}
		return null;
	}

}
