using UnityEngine;
using System.Collections;

[ExecuteInEditMode]
public class LineCross : MonoBehaviour {
	public LineSegment L1;
	public LineSegment L2;
	// Use this for initialization
	void Start () {
	
	}
	
	// Update is called once per frame
	void Update () {
	
	}

	float SegmentToSegment( LineSegment S1, LineSegment S2)
	{
		float SMALL_NUM = 0.0001f;
		Vector3   u = S1.P2 - S1.P1;
		Vector3   v = S2.P2 - S2.P1;
		Vector3   w = S1.P1 - S2.P1;
		float    a = Vector3.Dot(u,u);         // always >= 0
		float    b = Vector3.Dot(u,v);
		float    c = Vector3.Dot(v,v);         // always >= 0
		float    d = Vector3.Dot(u,w);
		float    e = Vector3.Dot(v,w);
		float    D = a*c - b*b;        // always >= 0
		float    sc, sN, sD = D;       // sc = sN / sD, default sD = D >= 0
		float    tc, tN, tD = D;       // tc = tN / tD, default tD = D >= 0


		
		// compute the line parameters of the two closest points
		if (D < SMALL_NUM) { // the lines are almost parallel
			sN = 0.0f;         // force using point P0 on segment S1
			sD = 1.0f;         // to prevent possible division by 0.0 later
			tN = e;
			tD = c;
		}
		else {                 // get the closest points on the infinite lines
			sN = (b*e - c*d);
			tN = (a*e - b*d);
	
			if (sN < 0.0f) {        // sc < 0 => the s=0 edge is visible
				sN = 0.0f;

				tN = e;
				tD = c;
			}
			else if (sN > sD) {  // sc > 1  => the s=1 edge is visible
				sN = sD;
				tN = e + b;
				tD = c;
			}
		}
		
		if (tN < 0.0f) {            // tc < 0 => the t=0 edge is visible
			tN = 0.0f;
			// recompute sc for this edge
			if (-d < 0.0f)
				sN = 0.0f;
			else if (-d > a)
				sN = sD;
			else {
				sN = -d;
				sD = a;
			}
		}
		else if (tN > tD) {      // tc > 1  => the t=1 edge is visible
			tN = tD;
			// recompute sc for this edge
			if ((-d + b) < 0.0f)
				sN = 0;
			else if ((-d + b) > a)
				sN = sD;
			else {
				sN = (-d +  b);
				sD = a;
			}
		}

		Debug.Log ("SN = " + sN);
		Debug.Log ("SD = " + sD);
		Debug.Log ("TN = " + tN);
		Debug.Log ("TD = " + tD);

		// finally do the division to get sc and tc
		sc = (Mathf.Abs(sN) < SMALL_NUM ? 0.0f : sN / sD);
		tc = (Mathf.Abs(tN) < SMALL_NUM ? 0.0f : tN / tD);

		Vector3 t1 = S1.P1 + u * sc;
		Vector3 t2 = S2.P1 + v * tc;

		Gizmos.DrawWireSphere (t1, 0.02f);
		Gizmos.DrawWireSphere (t2, 0.02f);

		// get the difference of the two closest points
		Vector3   dP = w + (sc * u) - (tc * v);  // =  S1(sc) - S2(tc)
		
		return dP.magnitude;   // return the closest distance
	}

	void OnDrawGizmos(){
		float sphereSize = 0.1f;

		Gizmos.DrawWireSphere (L1.P1, sphereSize);
		Gizmos.DrawWireSphere (L1.P2, sphereSize);

		Gizmos.DrawWireSphere (L2.P1, sphereSize);
		Gizmos.DrawWireSphere (L2.P2, sphereSize);



		Debug.DrawLine (L1.P1, L1.P2, Color.red);
		Debug.DrawLine (L2.P1, L2.P2, Color.green);
		Debug.Log (SegmentToSegment (L1, L2));
	}
}