/*
* This file is part of the TraKERS\Middleware package.
*
* (c) IRI <http://www.iri.centrepompidou.fr/>
*
* For the full copyright and license information, please view the LICENSE_MIDDLEWARE
* file that was distributed with this source code.
*/

/*
 * Projet : TraKERS
 * Module : MIDDLEWARE
 * Sous-Module : Tracking/Gestures
 * Classe : CircleDetector
 * 
 * Auteur : alexandre.bastien@iri.centrepompidou.fr
 * 
 * Fonctionnalités : Permet de détecter si l'utilisateur a effectué un cercle, 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;
using System.Windows.Media.Media3D;
using System.Drawing;
using Trakers.Debug;

namespace Trakers.Tracking.Gestures
{
    public class CircleDetector : GestureDetector
    {
        public CircleDetector(DebugWindow _debug) : base(_debug)
        {
            gesturePeriod = (float)1;
            indexesPerSecond = 30;
            indexesToCheck = (int)(gesturePeriod * indexesPerSecond);
        }

        /*
         * Lit les noeuds de l'historique du squelette afin de détecter un cercle.
         * Règles :
         * Se fait avec une main.
         * Chaque point est à la même distance du barycentre.
         * On regarde pour la main gauche.
         */
        public bool CheckForLeftCircle()
        {
            //Indique si la main gauche a décrit un cercle.
            bool leftHandDoCircle = true;
            
            //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 false;

            //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][(int)JointType.Spine].Position.Y - localHistory[0][(int)JointType.ShoulderCenter].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][(int)JointType.HandRight].Position;
            
            //Barycentres pour les mains.
            PointF leftBarycenter = new PointF(0, 0);
            //Distances moyennes des points aux barycentres.
            float averageDistToLeftBarycenter = 0;
            
            //Index du point de départ dans la détection.
            int beginIndex = localHistory.Count - indexesToCheck;

            //Calcul du barycentre de la main gauche.
            for (int i = beginIndex; i > 0; i--)
            {
                leftBarycenter.X += localHistory[i][(int)JointType.HandLeft].Position.X;
                leftBarycenter.Y += localHistory[i][(int)JointType.HandLeft].Position.Y;
            }
            leftBarycenter.X /= indexesToCheck;
            leftBarycenter.Y /= indexesToCheck;

            //Estimation de la distance moyenne d'un point au barycentre gauche.
            for (int i = beginIndex; i > 0; i--)
            {
                float ptX = localHistory[i][(int)JointType.HandLeft].Position.X;
                float ptY = localHistory[i][(int)JointType.HandLeft].Position.Y;
                averageDistToLeftBarycenter += (float)Distance2D(ptX, leftBarycenter.X, ptY, leftBarycenter.Y);
            }
            averageDistToLeftBarycenter /= indexesToCheck;

            //Pour les points, on suit l'algorithme.

            //Si la distance moyenne de chaque point de la main gauche au barycentre gauche est trop faible
            //Alors la main gauche n'a pas décrit de cercle.
            if (averageDistToLeftBarycenter < refDistance / 2)
                leftHandDoCircle = false;

            //Indique si on a atteint le point d'arrivée pour la main gauche.
            bool endLeftReached = false;

            if(leftHandDoCircle)
                for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
                {
                    //Si la distance d'un point de la main gauche excède à la distance moyenne au barycentre gauche avec une erreur N
                    //OU si le point de départ de la main gauche est plus éloigné du point d'arrivée de la main gauche de N.
                    //Alors la main gauche n'a pas décrit de cercle.
                    float X = localHistory[i][(int)JointType.HandLeft].Position.X;
                    float Y = localHistory[i][(int)JointType.HandLeft].Position.Y;

                    //Si un point est proche du point de départ.
                    if(Distance2D(X, Y, startPointLeft.X, startPointLeft.Y) < refDistance / 5 && X != startPointLeft.X && Y != startPointLeft.Y)
                        endLeftReached = true;

                    if (Math.Abs((double)Distance2D(X, Y, leftBarycenter.X, leftBarycenter.Y) - averageDistToLeftBarycenter) > refDistance / 5)
                    {
                        leftHandDoCircle = false;
                        break;
                    }

                    //Si la main gauche a atteint une position proche de son point de départ.
                    if (endLeftReached)
                    {
                        //S'il y a trop peu de points
                        if (i - (localHistory.Count - indexesToCheck + 1) < (indexesToCheck / 2))
                            leftHandDoCircle = false;
                        break;
                    }
                }

            //On supprime l'historique local.
            localHistory.Clear();
            //Si on est arrivé jusqu'ici, toutes les conditions pour un swipe left ont été remplies.
            return leftHandDoCircle;
        }

        /*
         * Lit les noeuds de l'historique du squelette afin de détecter un cercle.
         * Règles :
         * Se fait avec une main.
         * Chaque point est à la même distance du barycentre.
         * On regarde pour la main droite.
         */
        public bool CheckForRightCircle()
        {
            //Indique si la main droite a décrit un cercle.
            bool rightHandDoCircle = true;

            //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);

            //if (Math.Abs(Math.Abs(localHistory[0][(int)JointType.HandLeft].Position.X - localHistory[0][(int)JointType.HandRight].Position.X) - refDistance) < 10)
              //  Console.Out.WriteLine("REF");

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

            //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][(int)JointType.Spine].Position.Y - localHistory[0][(int)JointType.ShoulderCenter].Position.Y);
            //On commence la position pour les indexesToCheck dernières postures (celle à l'index 0 étant la dernière).
            SkeletonPoint startPointRight = localHistory[localHistory.Count - indexesToCheck][(int)JointType.HandRight].Position;

            //Barycentres pour la main droite.
            PointF rightBarycenter = new PointF(0, 0);
            //Distances moyennes des points aux barycentres.
            float averageDistToRightBarycenter = 0;

            //Index du point de départ dans la détection.
            int beginIndex = localHistory.Count - indexesToCheck;

            //Index du point d'arrivée.
            int endRightIndex = 0;

            //On cherche le point d'arrivée.
            /*for (int i = beginIndex; i > 0; i--)
            {
                float X = localHistory[i][(int)JointType.HandRight].Position.X;
                float Y = localHistory[i][(int)JointType.HandRight].Position.Y;

                //Si un point est proche du point de départ.
                if (Distance2D(X, Y, startPointRight.X, startPointRight.Y) < refDistance / 2 && X != startPointRight.X && Y != startPointRight.Y)
                {
                    Console.Out.WriteLine("REACHED");
                    endRightIndex = i;
                    break;
                }
            }*/

            /*//S'il n'y a pas assez de points.
            if (beginIndex - endRightIndex < indexesToCheck)
                return false;*/

            //Calcul du barycentre de la main droite.
            for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
            {
                rightBarycenter.X += localHistory[i][(int)JointType.HandRight].Position.X;
                rightBarycenter.Y += localHistory[i][(int)JointType.HandRight].Position.Y;
            }
            rightBarycenter.X /= indexesToCheck;
            rightBarycenter.Y /= indexesToCheck;

            //Estimation de la distance moyenne d'un point au barycentre droit.
            for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
            {
                float ptX = localHistory[i][(int)JointType.HandRight].Position.X;
                float ptY = localHistory[i][(int)JointType.HandRight].Position.Y;
                averageDistToRightBarycenter += (float)Distance2D(ptX, rightBarycenter.X, ptY, rightBarycenter.Y);
            }
            averageDistToRightBarycenter /= indexesToCheck;

            //Pour les points, on suit l'algorithme.

            //Si la distance moyenne de chaque point de la main droite au barycentre droit est trop faible
            //Alors la main droite n'a pas décrit de cercle.
            if (averageDistToRightBarycenter < refDistance / 2)
                rightHandDoCircle = false;

            float globalPercent = 0;

            if (rightHandDoCircle)
                for (int i = localHistory.Count - indexesToCheck + 1; i < localHistory.Count; i++)
                {
                    //Si la distance moyenne de chaque point de la main droite au barycentre droit est trop faible
                    //OU si la distance d'un point de la main droite excède à la distance moyenne au barycentre droit avec une erreur N
                    //OU si le point de départ de la main gauche est plus éloigné du point d'arrivée de la main gauche de N.
                    //Alors la main droite n'a pas décrit de cercle.

                    float X = localHistory[i][(int)JointType.HandRight].Position.X;
                    float Y = localHistory[i][(int)JointType.HandRight].Position.Y;
                    float R = averageDistToRightBarycenter, r = (float)Math.Abs((double)Distance2D(X, Y, rightBarycenter.X, rightBarycenter.Y));
                    float percent = 0;

                    if (r < R)
                        percent = 100 * r / R;
                    else
                        percent = 100 * R / r;

                    globalPercent += percent;

                    /*if (Math.Abs((double)Distance2D(X, Y, rightBarycenter.X, rightBarycenter.Y) - averageDistToRightBarycenter) > refDistance / 5)
                    {
                        //Console.Out.WriteLine("FAIL");
                        return false;
                    }*/
                }

            float res = ((float)globalPercent / indexesToCheck);

            Console.Out.WriteLine("p:" + ((float)globalPercent / indexesToCheck));

            if (res >= 50)
                Console.In.Read();

            //On supprime l'historique local.
            localHistory.Clear();
            //Si on est arrivé jusqu'ici, toutes les conditions pour un swipe left ont été remplies.
            return true;
        }

        public double Distance2D(float x1, float x2, float y1, float y2)
        {
            return Math.Sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
        }
    }
}