using UnityEngine;
using Augumenta;

public class StereoDisplayAutoSetup : MonoBehaviour
{

	public Camera leftEye, rightEye;
	Matrix4x4 viewL, viewR;
	Matrix4x4 U3DcalibratedL, U3DcalibratedR;
	Matrix4x4 Unity2OpenGLMAT;
	private Augumenta.AgapiProductProfiles.Product product=null;
	void Start()
	{
		Matrix4x4 calibratedL, calibratedR;
		// Change the coordinate space from Unity to OpenGL convention
		Unity2OpenGLMAT=Matrix4x4.identity;
		Unity2OpenGLMAT[10]=-1; // sets the raw 2 to flip to - raw 2
#if UNITY_ANDROID && !UNITY_EDITOR
		// We need to read the Calibrations from External SDCard
		if(UnityEngine.Android.Permission.HasUserAuthorizedPermission(
					UnityEngine.Android.Permission.ExternalStorageRead)) {
			Debug.Log("ExternalStorageRead granted");
		} else {
			Debug.LogError("ExternalStorageRead permission not granted!");
			UnityEngine.Android.Permission.RequestUserPermission(
				UnityEngine.Android.Permission.ExternalStorageRead);
		}
#endif
		// Get default product (you can use overloaded method if you want to
		// force it to one specific product)
		product=Augumenta.Agapi.CurrentProduct;
		if(product!=null && product.displays!=null) {
			foreach(Augumenta.AgapiProductProfiles.Product.Display display in product.displays) {
				if(display.id=="LEFT") {
					SetupDisplayMatrix(leftEye.transform, display);
				}
				if(display.id=="RIGHT") {
					SetupDisplayMatrix(rightEye.transform, display);
				}
			}
		}
		if(Augumenta.CalibrationReader.GetEyesMatrices(
					out calibratedL, out calibratedR)) {
			Debug.Log("Left: " + calibratedL + " Right: " + calibratedR);
		} else {
			// Use the agapi Front camera to get the transform
			// Get the product for Epson
			if(product!=null && product.cameras!=null) {
				Debug.LogError("Couldn't get the eye calibration");
				foreach(Augumenta.AgapiProductProfiles.Product.Camera camera
						in product.cameras) {
					if(camera.id=="FRONT") {
						calibratedL=calibratedR=camera.transformation.GetMatrix();
					}
				}
			}
		}
		if(product!=null && (product.displays!=null || product.cameras!=null)) {
			U3DcalibratedL=Unity2OpenGLMAT
						   * calibratedL
						   * Unity2OpenGLMAT;
			U3DcalibratedR=Unity2OpenGLMAT
						   * calibratedR
						   * Unity2OpenGLMAT;
			Debug.LogFormat("calibratedL=[{0}] " +
							"calibratedR=[{1}]",
							calibratedL.ToString("F8"),
							calibratedR.ToString("F8"));
		}
	}
	void Update()
	{
		if(product==null) {
			return;
		}
		// On each update we make sure to have the correct eyes setup
		if(leftEye!=null) {
			leftEye.worldToCameraMatrix=
				Unity2OpenGLMAT
				* leftEye.transform.worldToLocalMatrix
				* U3DcalibratedL;
		}
		if(rightEye!=null) {
			rightEye.worldToCameraMatrix=
				Unity2OpenGLMAT
				* rightEye.transform.worldToLocalMatrix
				* U3DcalibratedR;
		}
	}
	private void SetupDisplayMatrix(Transform eye,
									AgapiProductProfiles.Product.Display display)
	{
		Debug.Log("Setup display matrices for: "+display.id);
		UnityEngine.Camera camera = eye.gameObject.GetComponent<UnityEngine.Camera>();
		Matrix4x4 mutableMatrix = UnityAgapi.BASE_MATRIX;
		Matrix4x4 preProjectionMatrix = display.projection.transformation.GetMatrix();
		SetPlanes(ref mutableMatrix, camera.nearClipPlane, camera.farClipPlane);
		Matrix4x4 agapiDispTransform = Unity2OpenGLMAT *
									   display.transformation.GetMatrix();
		eye.localPosition=new Vector3(
			agapiDispTransform.m03,
			agapiDispTransform.m13,
			agapiDispTransform.m23);
		eye.localRotation=GetQuaternion(agapiDispTransform);
		camera.projectionMatrix = mutableMatrix * preProjectionMatrix;
	}
	public void SetPlanes(ref Matrix4x4 matrix,float near, float far)
	{
		matrix[2,2] = -(far / (far - near));
		matrix[2,3] = -((far * near) / (far - near));
	}
	public Quaternion GetQuaternion(Matrix4x4 m)
	{
		float tr = m.m00 + m.m11 + m.m22;
		float qw, qx, qy, qz;

		if(tr > 0) {
			float S = Mathf.Sqrt(tr+1.0f) * 2; // S=4*qw
			qw = 0.25f * S;
			qx = (m.m21 - m.m12) / S;
			qy = (m.m02 - m.m20) / S;
			qz = (m.m10 - m.m01) / S;
		} else if((m.m00 > m.m11)&(m.m00 > m.m22)) {
			float S = Mathf.Sqrt(1.0f + m.m00 - m.m11 - m.m22) * 2; // S=4*qx
			qw = (m.m21 - m.m12) / S;
			qx = 0.25f * S;
			qy = (m.m01 + m.m10) / S;
			qz = (m.m02 + m.m20) / S;
		} else if(m.m11 > m.m22) {
			float S = Mathf.Sqrt(1.0f + m.m11 - m.m00 - m.m22) * 2; // S=4*qy
			qw = (m.m02 - m.m20) / S;
			qx = (m.m01 + m.m10) / S;
			qy = 0.25f * S;
			qz = (m.m12 + m.m21) / S;
		} else {
			float S = Mathf.Sqrt(1.0f + m.m22 - m.m00 - m.m11) * 2; // S=4*qz
			qw = (m.m10 - m.m01) / S;
			qx = (m.m02 + m.m20) / S;
			qy = (m.m12 + m.m21) / S;
			qz = 0.25f * S;
		}
		return new Quaternion(qx, qy, qz, qw);
	}
	/// <summary>
	/// Get the point on the display relative coordinate (0,0 is bottom left)
	/// at distance z
	/// </summary>
	/// <param name="x">relative display coordinate (horizontal)</param>
	/// <param name="y">relative display coordinate (vertical)</param>
	/// <param name="z">distance in world coordinates</param>
	/// <returns></returns>
	public Vector3 StereoViewportToWorldPoint(float x, float y, float z)
	{
		if(leftEye==null||rightEye==null) {
			return Vector3.zero; // origin as no cameras
		}
		Vector3 LUC=leftEye.ViewportToWorldPoint(new Vector3(0,1, z));
		Vector3 LBC=leftEye.ViewportToWorldPoint(new Vector3(0,0, z));
		Vector3 RUC=rightEye.ViewportToWorldPoint(new Vector3(1,1, z));
		Vector3 RBC=rightEye.ViewportToWorldPoint(new Vector3(1,0, z));
		float CBY=Mathf.Max(LBC.y, RBC.y);
		float CUY=Mathf.Min(LUC.y, RUC.y);
		return new Vector3(
				   LBC.x+x*(RBC.x-LBC.x),
				   CUY+y*(CBY-CUY),
				   LUC.z);
	}
}
