using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class EyeSkillsVRHeadsetSelectByShake : MonoBehaviour {


    /// <summary>
    /// The camera reflecting the position of the head.
    /// </summary>
    public GameObject trackedCamera;

    /// <summary>
    /// The initial activation delay in seconds. The time before the selector becomes active.
    /// </summary>
    public float initialActivationDelayInSeconds = 2f;

    /// <summary>
    /// The sampling frequency for reading the headset position in seconds.
    /// </summary>
    public float samplingFrequency = 0.05f;

    /// <summary>
    /// The moment we start tracking
    /// </summary>
    private float firstTime;

    /// <summary>
    /// Directions of gaze around the y axis.
    /// </summary>
    private Queue<int> lastDifference = new Queue<int>();

    /// <summary>
    /// The maximum size of our monitoring queue. The length of time being monitored is maxQueueSize * samplingFrequency.
    /// </summary>
    public int maxQueueSize = 20;

    /// <summary>
    /// If there have been X changes of direction on the horizontal axis within the 20 recorded readings, that counts as a head shake.
    /// </summary>
    public int shakeSensitivity = 3;

    /// <summary>
    /// The minimum shake travel - consecutive measurements in the same direction.
    /// </summary>
    public int shakeTravel = 3;

    /// <summary>
    /// Minimum distance from mid-point before recording a motion.
    /// </summary>

    private bool atLeastOneShake = false, currentlyShaking = false;

    private IEnumerator coroutine;

    /// <summary>
    /// The last gaze direction. This is the rotation around the Y axis (i.e. amount looking left/right)
    /// </summary>
    private Quaternion lastHorizontalGazeDirection;

    private Quaternion GetHorizontalGazeAngle()
    {
        return trackedCamera.transform.rotation; 
    }

    /// <summary>
    /// Gets the direction of movement. 1 for relative head movement towards the right, -1 for relative head movement towards the left.
    /// </summary>
    /// <returns>The direction of movement.</returns>
    /// <param name="lastRotation">Last rotation.</param>
    //private int GetDirectionOfMovement(float lastRotation){

    //    float currentRotation = GetHorizontalGazeAngle();

    //    //What makes life a little difficult is that the angles go from 0-360. It's not always > or < to detect direction.
    //    //This isn't perfect, but in the real world, it is satisfactory

    //    int direction = 0;

    //    if (currentRotation.CompareTo(lastRotation)==0) {
    //        direction = 0;
    //    } else if ((currentRotation>lastRotation) || ((lastRotation - currentRotation)>180f)){ //e.g. c=0, l=350 
    //        direction=1;
    //    } else {
    //        direction=-1;
    //    }

    //    Debug.Log("Detected head movement direction of " + direction);
    //    return direction;
    //}
    private int calculateDirectionFromRotation(Quaternion initialRotation, Quaternion currentRotation)
    {
        Quaternion relativeRotation;


        relativeRotation = Quaternion.Inverse(initialRotation) * currentRotation;

        Vector3 euler = relativeRotation.eulerAngles;

        //we want to map continuous 0 -> 360 euler angles to 0->1 going right and 0->-1 going left.
        int direction = 0;
        if (euler.y.Equals(0) || euler.y.Equals(360))
        {
            direction= 0;
        }
        else if ((euler.y <= 180) && (euler.y >0))
        {
            direction = 1;
        }
        else if ((euler.y> 180) && (euler.y < 360))
        {
            direction = -1;
        }
        //Debug.Log("direction of rotation " + direction + " based on euler "+euler);
        return direction;
    }

    private int FindHorizontalDirectionChanges(Queue<int> q)
    {

        //As soon as we encounter a boundary, we store the number of consecutive matches we encountered, and start a new counter.
        //Everytime we encounter more than shakeTravel matches, we check if the last set of consecutive matches (which must be the opposite number) were also > shake Travel.
        //If that is the case, we have a shift. This also means that we can move directly from one direction change to another.

        //For directional matching: we could filter the inputs to -1/1 and -2/2 and on matching,
        //report back which axis it was. In which case, lasti must be matched to the inverse of i.

        int matchSize = shakeTravel;
        int lasti = 3; //out of the ordinary
        int lastGroupCount = 0;
        int groupCount = 0;
        int directionChanges = 0;

        //Queue<int> groupQ = new Queue<int>();

        foreach (int i in q)
        {

            if (i != lasti)
            { //We have hit a boundary.
                //Are this group count and the last group count both larger than matchSize?
                if ((lastGroupCount >= matchSize) && (groupCount >= matchSize))
                {
                    directionChanges += 1;
                }
                lastGroupCount = groupCount;
                groupCount = 0;
                lasti = i;
            }

            if (i == lasti)
            {
                groupCount += 1;
            }
        }

        return directionChanges;

    }

    /// <summary>
    /// Are the thresholds for shake exceeded? We are looking to see if there have been enough direction changes in the time period
    /// captured by the queue.
    /// </summary>
    /// <returns><c>true</c>, if thresholds exceeded was ared, <c>false</c> otherwise.</returns>
    /// <param name="q">Q.</param>
    private bool AreThresholdsExceeded(Queue<int> q)
    {

        int shifts = FindHorizontalDirectionChanges(q);

        if (shifts>0) Debug.Log("Shifts " + shifts);

        if (shifts >= shakeSensitivity)
        {
            Debug.Log("shakeSensitivity threshold exceeded " + shifts);
            return true;
        }
        //Debug.Log("Thresholds not exceeded ");
        return false;
    }


    public void Start()
    {
        coroutine = TrackShake();
        StartCoroutine(coroutine);
    }

    // Update is called once per frame
    void Update () {
		
	}

    /// <summary>
    /// Has the user given a head shake at least once since we started monitoring?
    /// </summary>
    /// <returns><c>true</c>, if least one shake was initiated, <c>false</c> otherwise.</returns>
    public bool IsCancelled(){
        if (atLeastOneShake){
            lastDifference.Clear();
            atLeastOneShake = false;
            currentlyShaking = false;
            return true;
        }
        return false;
    }

    /// <summary>
    /// Is the head currently shaking?
    /// </summary>
    /// <returns><c>true</c>, if shaking was currently occuring, <c>false</c> otherwise.</returns>
    public bool CurrentlyShaking(){
        return currentlyShaking;
    }

    /// <summary>
    /// Tracks the shake of the head from left to right.
    /// </summary>
    /// <returns>The shake.</returns>
    IEnumerator TrackShake()
    {

        //Give the user some time to orient themselves
        yield return new WaitForSeconds(initialActivationDelayInSeconds);

        //Initialise
        lastHorizontalGazeDirection = GetHorizontalGazeAngle();
        firstTime = Time.time;

        Debug.Log("Starting to track head shake");
        while (true)
        {

            //We now calculate if the head is looking more to the right or the left relative to the last reading.

            lastDifference.Enqueue(calculateDirectionFromRotation(lastHorizontalGazeDirection,GetHorizontalGazeAngle()));

            //We are looking for a given number of direction changes within the timeframe recorded by the queue.

            if (lastDifference.Count > maxQueueSize)
            {
                //Debug.Log("Shrinking Queue");
                lastDifference.Dequeue();
            }

            //Calculate if we have exceeded any of our movement thresholds
            if (AreThresholdsExceeded(lastDifference))
            {
                //We just record for now that at least one shake has happened and what the current reading says.
                //We do this because the "Unity way" is to poll, so we can't be sure when the access will happen.
                atLeastOneShake = true;
                currentlyShaking = true;

            } else {
                currentlyShaking = false;
            }

            lastHorizontalGazeDirection = GetHorizontalGazeAngle();
            yield return new WaitForSeconds(samplingFrequency);
        }
    }
}
