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

/// <summary>
/// Eye skills VRH eadset select by movement left/right/up/down
/// With a queue size of 30 and a sampling Frequency of 0.05, and a motion sensitivity of 2 degree with 6 consecutive measurements required in each direction,
/// it implied that the user has to move their head in a direction and back in between  a third of a second and a second, and at least 12 degrees 
/// </summary>
public class EyeSkillsVRHeadsetSelectByCross : 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 looking left.
    /// TODO : Probably can combine the "four" axis into two by just looking for sequences of neg/pos or pos/neg values by not carring about the sign of the first detected sequence
    /// </summary>
    private Queue<int> lastDifferenceLeft = new Queue<int>();
    private Queue<int> lastDifferenceRight = new Queue<int>();
    private Queue<int> lastDifferenceUp = new Queue<int>();
    private Queue<int> lastDifferenceDown = new Queue<int>();

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

    /// <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>
    private int shakeSensitivity = 1;

    /// <summary>
    /// The amount of movement between rotations to be considered adequate for a reading
    /// Difficult with deceleration!  We are actually interested in only starting measurement when we have sufficient acceleration! 
    /// Can't treat as "invalid" in this simple scheme... need to treat as a neutral reading. The problem is small movements around the central position that we want to smother.
    /// TODO: The general answer to this is to detect inflection points based on adequate ACCELERATION and DECELERATION towards and away from the inflection point. That is how to signal the head movement.
    /// </summary>
    public float motionDeltaSensitivity = 0.2f; 

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

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

    private bool lookedLeft = false, lookedRight = false, lookedUp = false, lookedDown = 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 gazeAngle;

    private int l, r, u, d;

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


    /// <summary>
    /// Calculates the direction from rotation. We are interested in up/down and left and right. 
    /// We explicitly model the perceived direction from the perspective of each axis to simplify the signal analysis. i.e. 1 if going in that direction, -1 if not.
    /// We are always looking for a continuous pattern of 11..11-1-1..-1-1 over a given buffer size and over a given time
    /// </summary>
    /// <returns>The direction from rotation.</returns>
    /// <param name="initialRotation">Initial rotation.</param>
    /// <param name="currentRotation">Current rotation.</param>
    /// <param name="relDirection">The direction we are observing. "left/right/up/down"</param>
    private int calculateRelativeDirection(Quaternion initialRotation, Quaternion currentRotation, string relDirection)
    {
        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;
        float rot=0f;

        //Debug.Log("Relative rotation " + euler);

        if (relDirection == "left") rot = 360-euler.y;
        if (relDirection == "right") rot = euler.y;
        if (relDirection == "up") rot = 360 - euler.x;
        if (relDirection == "down") rot = euler.x;

        if (rot >= motionDeltaSensitivity){
            if (rot.Equals(0) || rot.Equals(360))
            {
                direction = 0;
            }
            else if ((rot <= 180) && (rot > 0))
            {
                direction = 1;
            }
            else if ((rot > 180) && (rot < 360))
            {
                direction = -1;
            }
        } else {
            direction = 0; //if moving super slow, regard neutrally
        }

        //Debug.Log("direction of rotation " + direction);
        return direction;
    }

    /// <summary>
    /// Finds a specific direction change. Very different logic to just detecting changes. TODO: A study for how to implement this with neurons!
    /// </summary>
    /// <returns>The direction changes.</returns>
    /// <param name="q">Q.</param>
    private int FindDirectionChange(Queue<int> q,int startingDirection)
    {

        //In this case, we traverse the list looking for a minimum matchSize of startingDirection, we then need to follow that by detecting
        //at least another matchSize in -starting direction (excluding 0s) after that.

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

        foreach (int i in q)
        {
        
            if (i == -startingDirection)
            {
                if ((lastGroupCount >= matchSize) && (groupCount < matchSize)) //found a match before, but we're still not there ourselves
                {
                    groupCount += 1;
                } 
                else if ((lastGroupCount >= matchSize) && (groupCount >= matchSize)) //found a match 
                {
                    return 1;
                }
                else if (lastGroupCount < matchSize) { //Failed to find any start
                    lastGroupCount = 0;
                    groupCount = 0;
                }
            }
            if ((i == startingDirection) && (groupCount>0)) //if we failed to reach enough opposites
            {
                groupCount = 0;
                lastGroupCount = 0; //Start looking again.
            }
            if (i == startingDirection)
            { //only works because no else statement.  Improve this mad side-effect stufff.
                lastGroupCount += 1;
            }

        }

        return 0;

    }

    /// <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, string message)
    {

        int shifts = FindDirectionChange(q,1); //Always looing for positive direction, hence four queues - TODO : can map this down with meta programming to hunt in both axis directions

        //Debug.Log("Shifts " + shifts);

        if (shifts >= shakeSensitivity)
        {
            Debug.Log("Threshold exceeded " + shifts + " for "+message);
            //PrintQueue(lastDifferenceLeft,"L");
            //PrintQueue(lastDifferenceRight, "R");
            //PrintQueue(lastDifferenceUp, "U");
            //PrintQueue(lastDifferenceDown, "D");

            return true;
        }
        //Debug.Log("Thresholds not exceeded ");
        return false;
    }


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

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

    public void ClearQueues(){
        lastDifferenceLeft.Clear();
        lastDifferenceRight.Clear();
        lastDifferenceUp.Clear();
        lastDifferenceDown.Clear();
    }

    private bool checkState(ref bool dir){
        if (dir)
        {
            ClearQueues();
            dir = false;
            Debug.Log("Looked in some direction");
            return true;
        }
        return false;

    }

    public bool isLookingLeft()
    {
        bool o= checkState(ref lookedLeft);
        //Debug.Log(o);
        return o;
    }

    public bool isLookingRight()
    {
        return checkState(ref lookedRight);
    }

    public bool isLookingUp()
    {
        return checkState(ref lookedUp);
    }

    public bool isLookingDown()
    {
        return checkState(ref lookedDown);
    }


    private void PrintQueue(Queue<int> q,string pre){
        string outStr = pre + ": ";
        foreach (int i in q)
        {
            outStr += ", " + i;
        }
        Debug.Log(outStr);
    }

    IEnumerator DebugHelp(){
        Debug.Log("Starting DebugHelp");
        while (true){
            PrintQueue(lastDifferenceLeft,"L");
            yield return new WaitForSeconds(1f);
        }

    }

    /// <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
        gazeAngle = GetGazeAngle();
        firstTime = Time.time;

        Debug.Log("Starting Track Cross");
        while (true)
        {

            //We now calculate if the head is looking more to the right or the left relative to the last reading.
            //TODO: No time to think this through, but it's sooo quick and dirty.  Needs better meta-programming, how best in CSharp?
            Quaternion newGazeAngle = GetGazeAngle();

            l = calculateRelativeDirection(gazeAngle, newGazeAngle, "left");
            r = calculateRelativeDirection(gazeAngle, newGazeAngle, "right");
            u = calculateRelativeDirection(gazeAngle, newGazeAngle, "up");
            d = calculateRelativeDirection(gazeAngle, newGazeAngle, "down");

            //Debug.Log("Detected motion lrud :" + l + r + u + d);

            lastDifferenceLeft.Enqueue(l);
            lastDifferenceRight.Enqueue(r);
            lastDifferenceUp.Enqueue(u);
            lastDifferenceDown.Enqueue(d);

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

            if (lastDifferenceLeft.Count > maxQueueSize) lastDifferenceLeft.Dequeue();
            if (lastDifferenceRight.Count > maxQueueSize) lastDifferenceRight.Dequeue();
            if (lastDifferenceUp.Count > maxQueueSize) lastDifferenceUp.Dequeue();
            if (lastDifferenceDown.Count > maxQueueSize) lastDifferenceDown.Dequeue();

            //Calculate if we have exceeded any of our movement thresholds
            if (AreThresholdsExceeded(lastDifferenceLeft,"left")) lookedLeft = true;
            if (AreThresholdsExceeded(lastDifferenceRight, "right")) lookedRight = true;
            if (AreThresholdsExceeded(lastDifferenceUp, "up")) lookedUp = true;
            if (AreThresholdsExceeded(lastDifferenceDown, "down")) lookedDown = true;

            //TODO: Can I plot these 1/-1 values in a dynamically generated sprite map or something, to improve the understanding of what is happening when developing?

            gazeAngle = GetGazeAngle();
            yield return new WaitForSeconds(samplingFrequency);
        }
    }
}
