/*
 * Projet : TraKERS
 * Module : MIDDLEWARE
 * Sous-Module : Tracking/Gestures
 * Classe : PushDetector
 * 
 * Auteur : alexandre.bastien@iri.centrepompidou.fr
 * 
 * Fonctionnalités : Permet de détecter si l'utilisateur a effectué un Push, en se basant sur
 * des règles appliquées à la positions des noeuds dans le temps.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;

namespace Trakers.Tracking.Gestures
{
    public class PushDetector : GestureDetector
    {
        public enum Direction { PUSH, PULL };
        public enum Hand { LEFT, RIGHT, BOTH, NONE };

        Debug.DebugWindow debug;

        public PushDetector(Debug.DebugWindow _d) : base()
        {
            debug = _d;
            gesturePeriod = (float)0.3;
            indexesPerSecond = 30;
            indexesToCheck = (int)(gesturePeriod * indexesPerSecond);
        }

        /*
         * Lit les noeuds de l'historique du squelette afin de détecter un Push.
         * Règles :
         * Se fait avec une main (gauche ou droite).
         * Chaque nouvelle position de la main doit être plus profonde que la précédente.
         * Chaque nouvelle position de la main ne doit pas dévier trop de l'axe perpendiculaire au plan (X, Y).
         * Le geste doit mesurer en profondeur une certaine distance.
         */
        public Hand CheckForPush()
        {
            //Crée un historique de squelette local, puisque l'historique est mis à jour toutes les ~1/30 s.
            List<List<Joint>> localHistory = new List<List<Joint>>(history);
            
            //Si il n'y a pas assez de positions dans l'historique local pour vérifier le geste.
            if (localHistory.Count < indexesToCheck + 1)
                return Hand.NONE;

            //La distance de référence est ici la distance entre le milieu du dos et le milieu des épaules.
            refDistance = Math.Abs(localHistory[0][spineID].Position.Y - localHistory[0][shoulderCenterID].Position.Y);
            //On commence la position pour les indexesToCheck dernières postures (celle à l'index 0 étant la dernière).
            SkeletonPoint startPointLeft = localHistory[localHistory.Count - indexesToCheck][handLeftID].Position;
            SkeletonPoint startPointRight = localHistory[localHistory.Count - indexesToCheck][handRightID].Position;

            //Booléens indiquant si le mouvement serait valide pour la main gauche ou droite.
            bool leftHandOK = true, rightHandOK = true;

            //De la position p1 à pn, on suit l'algorithme.
            for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
            {
                if (localHistory[i][handRightID].Position.Y > localHistory[i][headID].Position.Y)
                    debug.R1.Fill = System.Windows.Media.Brushes.Blue;
                else
                    debug.R1.Fill = System.Windows.Media.Brushes.DarkGray;

                if (localHistory[i][handRightID].Position.Y < localHistory[i][hipCenterID].Position.Y)
                    debug.R2.Fill = System.Windows.Media.Brushes.Blue;
                else
                    debug.R2.Fill = System.Windows.Media.Brushes.DarkGray;

                if (localHistory[i][handRightID].Position.Z < localHistory[i - 1][handRightID].Position.Z)
                    debug.R3.Fill = System.Windows.Media.Brushes.Blue;
                else
                    debug.R3.Fill = System.Windows.Media.Brushes.DarkGray;

                //Console.Out.WriteLine(Math.Abs(localHistory[i][handRightID].Position.X - startPointRight.X) + " " + Math.Abs(localHistory[i][handRightID].Position.Y - startPointRight.Y) + " " + refDistance / 10);

                if (Math.Abs(localHistory[i][handRightID].Position.X - startPointRight.X) < refDistance / 5 &&
                    Math.Abs(localHistory[i][handRightID].Position.Y - startPointRight.Y) < refDistance / 5)
                    debug.R4.Fill = System.Windows.Media.Brushes.Blue;
                else
                    debug.R4.Fill = System.Windows.Media.Brushes.DarkGray;

                //Si la position Y de la main est plus haute que la tête
                //OU si la position Y de la main est plus basse que la hanche
                //OU si la nouvelle position Z de la main est moins profonde que la précédente
                //OU si la nouvelle position X de la main est plus éloignée de la distance N par rapport à la première position X
                //OU si la nouvelle position Y de la main est plus éloignée de la distance N par rapport à la première position Y
                //Alors la main en question ne fait pas de push.
                if (localHistory[i][handLeftID].Position.Y < localHistory[i][headID].Position.Y ||
                localHistory[i][handLeftID].Position.Y > localHistory[i][hipCenterID].Position.Y ||
                localHistory[i][handLeftID].Position.Z > localHistory[i - 1][handLeftID].Position.Z ||
                Math.Abs(localHistory[i][handLeftID].Position.X - startPointLeft.X) > refDistance / 5 ||
                Math.Abs(localHistory[i][handLeftID].Position.Y - startPointLeft.Y) > refDistance / 5)
                    leftHandOK = false;
                if (localHistory[i][handRightID].Position.Y < localHistory[i][headID].Position.Y ||
                localHistory[i][handRightID].Position.Y > localHistory[i][hipCenterID].Position.Y ||
                localHistory[i][handRightID].Position.Z > localHistory[i - 1][handRightID].Position.Z ||
                Math.Abs(localHistory[i][handRightID].Position.X - startPointRight.X) > refDistance / 5 ||
                Math.Abs(localHistory[i][handRightID].Position.Y - startPointRight.Y) > refDistance / 5)
                    rightHandOK = false;

                if (!leftHandOK && !rightHandOK)
                    return Hand.NONE;
            }

            //Console.Out.WriteLine("OK");

            if (Math.Abs(localHistory[localHistory.Count - 1][handRightID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.Z) * 100 > 20)
                debug.R5.Fill = System.Windows.Media.Brushes.Blue;
            else
                debug.R5.Fill = System.Windows.Media.Brushes.DarkGray;

            if (localHistory[localHistory.Count - 1][handRightID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                debug.R6.Fill = System.Windows.Media.Brushes.Blue;
            else
                debug.R6.Fill = System.Windows.Media.Brushes.DarkGray;

            //Si la distance en Z du geste a été plus courte que la distance N
            //Alors on retourne faux.
            //float dist = (localHistory[localHistory.Count - 1][handRightID].Position.X - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X);

            //Console.WriteLine(Math.Abs(localHistory[0][handLeftID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.Z) * 100 + " " + refDistance);

            if (Math.Abs(localHistory[localHistory.Count - 1][handLeftID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.Z) * 100 < 20)
                leftHandOK = false;
            if (Math.Abs(localHistory[localHistory.Count - 1][handRightID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.Z) * 100 < 20)
                rightHandOK = false;
            
            /*if(rightHandOK || leftHandOK)
                Console.Out.WriteLine("000000000");*/

            //Si la dernière position de la main droite/gauche est sur le côté gauche/droit du corps
            //OU si la première position calculée de la main droite/gauche est sur le côté gauche/droit du corps
            //Alors on retourne faux.
            if (localHistory[localHistory.Count - 1][handLeftID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                leftHandOK = false;
            if (localHistory[localHistory.Count - 1][handRightID].Position.X < localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X < localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                rightHandOK = false;

            /*if (rightHandOK || leftHandOK)
                Console.Out.WriteLine("11111111111");*/

            if (!leftHandOK && !rightHandOK)
                return Hand.NONE;

            /*if (rightHandOK || leftHandOK)
                Console.Out.WriteLine("================");*/

            //On supprime l'historique local.
            localHistory.Clear();

            debug.ExceptionLbl.Background = System.Windows.Media.Brushes.White;
            //Console.WriteLine("PUSH");
            //Console.Read();

            //Si on est arrivé jusqu'ici, toutes les conditions pour un push ont été remplies.
            if (leftHandOK && rightHandOK)
                return Hand.BOTH;
            else if (leftHandOK)
                return Hand.LEFT;
            else if (rightHandOK)
                return Hand.RIGHT;

            return Hand.NONE;
        }

        /*
         * Lit les noeuds de l'historique du squelette afin de détecter un Pull.
         * Règles :
         * Se fait avec une main.
         * Chaque nouvelle position de la main doit être moins profonde que la précédente.
         * Chaque nouvelle position de la main ne doit pas dévier trop de l'axe perpendiculaire au plan (X, Y).
         * Le geste doit mesurer en profondeur une certaine distance.
         */
        public Hand CheckForPull()
        {
            //Crée un historique de squelette local, puisque l'historique est mis à jour toutes les ~1/30 s.
            List<List<Joint>> localHistory = new List<List<Joint>>(history);

            //Si il n'y a pas assez de positions dans l'historique local pour vérifier le geste.
            if (localHistory.Count < indexesToCheck + 1)
                return Hand.NONE;

            //La distance de référence est ici la distance entre le milieu du dos et le milieu des épaules.
            refDistance = Math.Abs(localHistory[0][spineID].Position.Y - localHistory[0][shoulderCenterID].Position.Y);
            //On commence la position pour les indexesToCheck dernières postures (celle à l'index 0 étant la dernière).
            SkeletonPoint startPointLeft = localHistory[localHistory.Count - indexesToCheck][handLeftID].Position;
            SkeletonPoint startPointRight = localHistory[localHistory.Count - indexesToCheck][handRightID].Position;

            //Booléens indiquant si le mouvement serait valide pour la main gauche ou droite.
            bool leftHandOK = true, rightHandOK = true;

            //De la position p1 à pn, on suit l'algorithme.
            for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
            {
                //Si la position Y de la main est plus haute que la tête
                //OU si la position Y de la main est plus basse que la hanche
                //OU si la nouvelle position Z de la main est plus profonde que la précédente
                //OU si la nouvelle position X de la main est plus éloignée de la distance N par rapport à la première position X
                //OU si la nouvelle position Y de la main est plus éloignée de la distance N par rapport à la première position Y
                //Alors la main en question ne fait pas de push.
                if (localHistory[i][handLeftID].Position.Y < localHistory[i][headID].Position.Y ||
                localHistory[i][handLeftID].Position.Y > localHistory[i][hipCenterID].Position.Y ||
                localHistory[i][handLeftID].Position.Z < localHistory[i - 1][handLeftID].Position.Z ||
                Math.Abs(localHistory[i][handLeftID].Position.X - startPointLeft.X) > refDistance / 5 ||
                Math.Abs(localHistory[i][handLeftID].Position.Y - startPointLeft.Y) > refDistance / 5)
                    leftHandOK = false;
                if (localHistory[i][handRightID].Position.Y < localHistory[i][headID].Position.Y ||
                localHistory[i][handRightID].Position.Y > localHistory[i][hipCenterID].Position.Y ||
                localHistory[i][handRightID].Position.Z < localHistory[i - 1][handRightID].Position.Z ||
                Math.Abs(localHistory[i][handRightID].Position.X - startPointRight.X) > refDistance / 5 ||
                Math.Abs(localHistory[i][handRightID].Position.Y - startPointRight.Y) > refDistance / 5)
                    rightHandOK = false;

                if (!leftHandOK && !rightHandOK)
                    return Hand.NONE;
            }

            //Console.Out.WriteLine("OK");

            if (Math.Abs(localHistory[localHistory.Count - 1][handRightID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.Z) * 100 > 20)
                debug.R5.Fill = System.Windows.Media.Brushes.Blue;
            else
                debug.R5.Fill = System.Windows.Media.Brushes.DarkGray;

            if (localHistory[localHistory.Count - 1][handRightID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                debug.R6.Fill = System.Windows.Media.Brushes.Blue;
            else
                debug.R6.Fill = System.Windows.Media.Brushes.DarkGray;

            //Si la distance en Z du geste a été plus courte que la distance N
            //Alors on retourne faux.
            //float dist = (localHistory[localHistory.Count - 1][handRightID].Position.X - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X);

            //Console.WriteLine(Math.Abs(localHistory[0][handLeftID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.Z) * 100 + " " + refDistance);

            if (Math.Abs(localHistory[localHistory.Count - 1][handLeftID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.Z) * 100 < 20)
                leftHandOK = false;
            if (Math.Abs(localHistory[localHistory.Count - 1][handRightID].Position.Z - localHistory[localHistory.Count - indexesToCheck][handRightID].Position.Z) * 100 < 20)
                rightHandOK = false;

            /*if (rightHandOK || leftHandOK)
                Console.Out.WriteLine("000000000");*/

            //Si la dernière position de la main droite/gauche est sur le côté gauche/droit du corps
            //OU si la première position calculée de la main droite/gauche est sur le côté gauche/droit du corps
            //Alors on retourne faux.
            if (localHistory[localHistory.Count - 1][handLeftID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handLeftID].Position.X > localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                leftHandOK = false;
            if (localHistory[localHistory.Count - 1][handRightID].Position.X < localHistory[localHistory.Count - 1][hipCenterID].Position.X ||
               localHistory[localHistory.Count - indexesToCheck][handRightID].Position.X < localHistory[localHistory.Count - 1][hipCenterID].Position.X)
                rightHandOK = false;

            /*if (rightHandOK || leftHandOK)
                Console.Out.WriteLine("11111111111");*/

            if (!leftHandOK && !rightHandOK)
                return Hand.NONE;

            /*if (rightHandOK || leftHandOK)
                Console.Out.WriteLine("================");*/

            //On supprime l'historique local.
            localHistory.Clear();

            debug.ExceptionLbl.Background = System.Windows.Media.Brushes.Black;
            //Console.WriteLine("PUSH");
            //Console.Read();

            //Si on est arrivé jusqu'ici, toutes les conditions pour un push ont été remplies.
            if (leftHandOK && rightHandOK)
                return Hand.BOTH;
            else if (leftHandOK)
                return Hand.LEFT;
            else if (rightHandOK)
                return Hand.RIGHT;

            return Hand.NONE;
        }
    }
}