using UnityEngine;
using System.Collections;

public class MeasureStick : MonoBehaviour {

	internal float difference = 0.0f;

	public GUIStyle style;

	// Use this for initialization
	void Start () {
	
	}

	void OnGUI()
	{
		GUI.Label(new Rect(10, 10, 100, 100), string.Format("{0:0.000}", difference), style);
	}

	void DrawTransform(Vector3 pos, Quaternion rot, float size = 0.2f, float brightness = 1.0f)
	{
		Debug.DrawRay(pos, rot * (Vector3.forward * size), Color.blue * brightness);
		Debug.DrawRay(pos, rot * (Vector3.right * size), Color.red * brightness);
		Debug.DrawRay(pos, rot * (Vector3.up * size), Color.green * brightness);
	}
	
	// Update is called once per frame
	void Update () {

		DrawTransform(Vector3.zero, Quaternion.identity, 1.0f, 0.9f);
		DrawTransform(Vector3.zero, Quaternion.identity, -1.0f, 0.3f);

		if (Sixense.IsControllerEnabled(0) &&
			Sixense.IsControllerEnabled(1))
		{
			var data = new Sixense.ControllerData[2];
			if (Sixense.GetData(0, 0, ref data[0]) &&
				Sixense.GetData(1, 0, ref data[1]))
			{
				DrawTransform(data[0].Position, data[0].Orientation);
				DrawTransform(data[1].Position, data[1].Orientation);

				Debug.DrawLine(data[0].Position, data[1].Position, Color.black);

				var delta = data[0].Position - data[1].Position;
				float size = delta.magnitude;
				delta *= 1.0f / size; // normalize
				difference = size - 1.0f;
				size = difference * 0.5f;

				Debug.DrawRay(data[0].Position, delta * -size, Color.white);
				Debug.DrawRay(data[1].Position, delta * size, Color.white);
			}
		}
	}
}