﻿using System.Collections;
using Augumenta;
using UnityEngine;

/**
 * PoseHandler adds hand detection to the game object.
 * It manages the registration of the hand pose detection.
 *
 * OnPose and OnPoseLost callbacks will be called when selected
 * hand pose event occurs.
 */
public class PoseHandler: MonoBehaviour
{

	protected UnityAgapi agapi=UnityAgapi.Instance;
	[Tooltip("The origin for the detection when using absolute coordinates as GameObject, when needed")]
	public GameObject headReferential=null;
	// selected pose, orientation, start angle and end angle
	[Tooltip("Pose to register")]
	public Augumenta.Pose pose=Augumenta.Pose.P001;
	[Tooltip("Parent to register (only for P501)")]
	public Augumenta.ParentPose parentPose = Augumenta.ParentPose.NONE;
	[Tooltip("Pose orientation")]
	public Orientation orientation=Orientation.BOTH;
	[Tooltip("Pose angle range start")]
	public float startAngle=0f;
	[Tooltip("Pose angle range end")]
	public float endAngle=0f;
	[Tooltip("Calibration matrix for camera-display-eye")]
	public Matrix4x4 calibration = Matrix4x4.identity;
	[Tooltip("Panel payload string")]
	public string panelPayload;
	[Tooltip("Register pose even if the object is disabled")]
	public bool registerAlways=false;
	[Tooltip("Attach pose listeners only, don't register for poses")]
	public bool listenersOnly=false;
	// track if poses are registered so that they are correctly unregistered
	private bool registered=false;
	// track if poses are listened so that there is only only listener set at the a time
	private bool listening=false;
	// registered pose, orientation, start angle and end angle
	private Augumenta.Pose _pose;
	private Augumenta.Pose _parentPose;
	private Orientation _orientation;
	private float _startAngle;
	private float _endAngle;
	// store payload for un-registration
	private string _panelPayload;
	protected GameObject referential=null;


	[ExecuteInEditMode]
	public virtual void OnValidate()
	{
		startAngle=Mathf.Clamp(startAngle, 0, 90);
		endAngle=Mathf.Clamp(endAngle, 0, 90);

		if(startAngle > endAngle) {
			Debug.LogWarning("Start angle must be smaller than end angle");
		}
		if(pose!=Augumenta.Pose.P501) {
			// We only have parents for P501
			parentPose = Augumenta.ParentPose.NONE;
		} else {
			if(parentPose!=Augumenta.ParentPose.PALM) {
				// Only for palm we can have other orientation than BOTH
				orientation = Orientation.BOTH;
			}
			startAngle = 0f;
			endAngle = 90f;
			panelPayload = null;
		}
	}

	public virtual void Awake()
	{
		// register pose when script is loaded if the 'registerAlways' is set
		if(registerAlways) {
			registerPose();
		}
		if(calibration.isIdentity) {
			if(!CalibrationReader.GetCalibrationMatrix(out calibration)) {
				Debug.LogWarning("No calibration found for sensor. Using identity.");
			} else {
				Debug.Log("Using calibrated sensor from files: "
					  + calibration);
			}
		} else {
			Debug.Log("Using Editor values from user for calibration: "
					  + calibration);
		}
	}

	public virtual void Start() { }

	public virtual void OnDestroy()
	{
		// make sure that everything is unregistered when object is destroyed
		Debug.Log("OnDestroy called");
		unregisterPose();
	}

	public virtual void Update()
	{
		if(pose!=_pose||orientation!=_orientation||startAngle!=_startAngle||endAngle!=_endAngle||
				(_panelPayload!=null&&_panelPayload.Length!=0&&panelPayload!=_panelPayload)) {
			// if pose or orientation has changed then unregister previous registration
			// and register for the new pose and orientation
			// Debug.Log ("Re-register");
			unregisterPose();
			registerPose();
		}
	}

	public virtual void OnEnable()
	{
		if(!registerAlways) {
			// register for the pose when this is enabled
			registerPose();
		}
	}

	public virtual void OnDisable()
	{
		if(!registerAlways) {
			// unregister the pose when this is disabled
			unregisterPose();
		}
	}

	protected virtual void registerPose()
	{
		// cache current pose and orientation for un-registration
		_pose=pose;
		// Use the Augumenta.Pose here
		_parentPose=(Augumenta.Pose)parentPose;
		_orientation=orientation;
		_startAngle=startAngle;
		_endAngle=endAngle;

		// for X001 we have panel format and payload
		if(pose==Augumenta.Pose.X001||_parentPose==Augumenta.Pose.X001) {
			_panelPayload=panelPayload;
		}
		// note that these are stored before trying to register, because
		// if registering fails then in the next update call it won't try to register.

		if(!listenersOnly&&!registered) {
			// child poses
			if(pose==Augumenta.Pose.P501) {
				if(_parentPose==Augumenta.Pose.P002 ||
						_parentPose==Augumenta.Pose.X001) {
					// The parent pose is with angles 0, 0
					if(_parentPose==Augumenta.Pose.P002) {
						registered=agapi.RegisterPose(_parentPose, orientation,
													  0, 0);
						if(!registered) {
							Debug.LogWarning("ParentPose.Palm needs to be registered" +
											 " with Orientation.BOTH, startAngle 0 " +
											 "and endAngle 0. " +
											 "Parent pose not registered!");
							return;
						}
					}
					if(_parentPose==Augumenta.Pose.X001) {
						registered=agapi.RegisterPose(_parentPose, orientation,
													  startAngle, endAngle);
						if(!registered) {
							if(_parentPose==Augumenta.Pose.X001 &&
									startAngle!=0 &&
									endAngle!=90 &&
									orientation!=Orientation.BOTH) {
								Debug.LogWarning("ParentPose.PANEL needs to be registered" +
												 " with Orientation.BOTH, startAngle 0 " +
												 "and endAngle 90. ");
							}
							Debug.LogWarning("Parent pose not registered!");
							return;
						}
					}
				}
				// TODO: Allow child poses with different area to be registered...
				registered=agapi.RegisterChildPose(_parentPose,
												   orientation, startAngle, endAngle,
												   _pose, 0, 0, 1, 1);
			} else {
				registered=agapi.RegisterPose(pose, orientation, startAngle, endAngle);
			}
			if(!registered) {
				Debug.LogWarning("Failed to register pose "+pose+", "+orientation+", range: "+startAngle+" - "+endAngle);
				if(pose == Augumenta.Pose.X001 && startAngle != 0 && endAngle != 90 && orientation != Orientation.BOTH) {
					Debug.LogWarning("Pose.X001 needs to be registered with Orientation.BOTH, startAngle 0 and endAngle 90");
				}
				return;
			}
			if(pose==Augumenta.Pose.P501) {
				Debug.Log("Registered child pose "+_pose+", "+orientation+", range: "+startAngle+" - "+endAngle + " parent " + _parentPose + ")");
			} else {
				Debug.Log("Registered pose "+_pose+", "+orientation+", range: "+startAngle+" - "+endAngle);
			}
		}

		if(!listening) {
			// add pose listeners
			agapi.onPoseEvent+=OnPoseListener;
			agapi.onPoseLostEvent+=OnPoseLostListener;
			listening=true;
		}
	}

	protected virtual void unregisterPose()
	{
		if(listening) {
			// remove pose listeners
			agapi.onPoseEvent-=OnPoseListener;
			agapi.onPoseLostEvent-=OnPoseLostListener;
			listening=false;
		}

		if(registered) {
			bool err;
			// child poses
			if(_pose==Augumenta.Pose.P501) {
				err=agapi.UnregisterChildPose(_parentPose, _orientation, _pose);
			} else {
				err=agapi.UnregisterPose(_pose, _orientation, _startAngle, _endAngle);
			}
			registered=false;
			if(!err) {
				Debug.LogWarning("Failed to unregister pose"+_pose+", "+_orientation+", range: "+_startAngle+" - "+_endAngle);
				return;
			} else {
				Debug.Log("Unregistered pose "+_pose+", "+_orientation+", range: "+_startAngle+" - "+_endAngle);
			}
		}
	}

	/**
	 * Check if pose event should be handled by this script.
	 */
	protected virtual bool IsEventWanted(PoseEvent e)
	{
		return e.Equals(pose, orientation, startAngle, endAngle) && (string.IsNullOrEmpty(panelPayload) || e.data == panelPayload);
	}

	private void OnPoseListener(PoseEvent e, bool isNew)
	{
		// check if the event is the selected one
		if(!IsEventWanted(e)) {
			return;
		}
		Matrix4x4 poseCalibration = Matrix4x4.identity;
		// We only apply the calibration from the sensor when the pose is
		// P501 and the parent is 0!
		if(e.pose==Augumenta.Pose.P501&&e.parentId==0) {
			poseCalibration=calibration;
		}

		var headPosition=transform.forward;
		var headRotation=new Quaternion(0, 0, 0, 1);  // no rotation
		// if headReferential is null, use Camera.main. If that doesn't exists, use no rotation
		if(headReferential != null) {
			headPosition=headReferential.transform.position;
			headRotation=headReferential.transform.rotation;
			referential=headReferential;
		} else if(Camera.main != null) {
			headPosition=Camera.main.transform.position;
			headRotation=Camera.main.transform.rotation;
			referential=Camera.main.gameObject;
		} else {
			Debug.LogWarning("headReferential is missing and there is no Camera.main!");
		}
		// NOTE: When we detect Fingers without Panels we should use the
		// values for sensor calibration. That is because only with 3D ToF we
		// can detect P501 without a parent.
		var absoluteCameraPos = new Vector3(e.absoluteCamera.centerX, e.absoluteCamera.centerY, e.absoluteCamera.centerZ);
		// Apply the camera calibration
		absoluteCameraPos = poseCalibration.MultiplyPoint(absoluteCameraPos);
		// Scale it to Unity space
		absoluteCameraPos=Vector3.Scale(absoluteCameraPos,
										new Vector3(1/1000f, -1/1000f, 1/1000f));
		// Move it to the right head location
		var position=headPosition+headRotation * absoluteCameraPos;
#if UNITY_2017_2_OR_NEWER
		var rotation=headRotation * (poseCalibration.rotation * e.GetRotation());
#else
		var rotation=headRotation * e.GetRotation();
		Debug.LogError("headRotation not supported for your Unity Version");
#endif
		// trigger callback for selected pose
		OnPose(e, isNew, position, rotation);
	}
	private void OnPoseLostListener(PoseEvent e)
	{
		// check if the event is the selected one
		if(!IsEventWanted(e)) {
			return;
		}
		// trigger callback for selected pose
		OnPoseLost(e);
	}

	/**
	 * Pose event callback
	 */
	public virtual void OnPose(PoseEvent e, bool isNew, Vector3 position, Quaternion rotation)
	{
		Debug.Log("OnPose(new: "+isNew+"): "+e);
	}

	/**
	 * Pose lost event callback
	 */
	public virtual void OnPoseLost(PoseEvent e)
	{
		Debug.Log("OnPoseLost: "+e);
	}
	public void debugState()
	{
		string state="PoseHandler state";
		state+="  current: "+pose+", "+orientation+", range: "+startAngle+" - "+endAngle+"\n";
		state+="  cached: "+_pose+", "+_orientation+", range: "+_startAngle+" - "+_endAngle+"\n";
		Debug.Log(state);
	}
}