/*

 * Projet : KINECT PROJECTS

 * Module : MIDDLEWARE

 * Sous-Module : Tracking

 * Classe : KinectMain

 * 

 * Auteur : alexandre.bastien@iri.centrepompidou.fr

 * 

 * Fonctionnalités : Récupère les trames de données de la Kinect, les squelettes détectés via le SDK 1.0 de Microsoft.

 * Interprète ces trames de façon à afficher le flux vidéo couleurs, et récupérer la distance de l'utilisateur et les

 * noeuds de son squelette. Lance des événements lorsque la main gauche/droite entre dans/quitte le champ.

 * Envoie des données au sous-module de debug de manière a afficher un retour visuel sur la position de l'utilisateur,

 * son squelette, la détection de ses mains.

 */

using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using System.Windows;

using System.Windows.Controls;

using System.Windows.Data;

using System.Windows.Documents;

using System.Windows.Input;

using System.Windows.Media;

using System.Windows.Media.Imaging;

using System.Windows.Navigation;

using System.Windows.Shapes;

using System.Drawing;

using System.Windows.Media.Media3D;

using Microsoft.Kinect;

using Coding4Fun.Kinect.Wpf;

using System.ComponentModel;

using Trakers.Debug;

using Tuio;

using Trakers.Communication;

using System.IO;

using Trakers.Tracking.Gestures;

using Trakers.Tracking.Events;

using System.Configuration;

using Kinect.Toolbox;

namespace Trakers.Tracking

{

    //Il s'agit des fonctions permettant d'appeler les fonctions des événements Main droite/gauche entre/quitte le champ.

    public delegate void LeftHandTrackedHandler(object o, LeftHandTrackedEventArgs e);

    public delegate void RightHandTrackedHandler(object o, RightHandTrackedEventArgs e);

    public delegate void LeftHandQuitHandler(object o, LeftHandQuitEventArgs e);

    public delegate void RightHandQuitHandler(object o, RightHandQuitEventArgs e);

    //Il s'agit des fonctions permettant d'appeler les fonctions des événements Swipe left/right/up/down.

    public delegate void SwipeHandler(object o, SwipeEventArgs e);

    public class KinectMain

    {

        //Fenêtre de debug.

        private Debug.DebugWindow debug;

        //Squelettes (Il y en a 6 par défaut).

        private Skeleton[] skeletons;

        //Caméra infrarouge (sensor) de la Kinect.

        private KinectSensor kinectSensor;

        //Détecteur de gestes.

        private SwipeDetector swipeDetector;

        //Distances min/max délimitant le champ de recherche.

        private float minDistHands;

        private float maxDistHands;

        //Serveur TUIO pour la connexion du Middleware vers le Front Atelier.

        private Server server;

        //Les événements des mains pour la recherche.

        public static event LeftHandTrackedHandler LeftHandTrackedEvent;

        public static event RightHandTrackedHandler RightHandTrackedEvent;

        public static event LeftHandQuitHandler LeftHandQuitEvent;

        public static event RightHandQuitHandler RightHandQuitEvent;

        //Les événements swipe.

        public static event SwipeHandler SwipeEvent;

        //Voici les ID des noeuds d'un squelette.

        private int hipCenterID = 0, spineID = 1, shoulderCenterID = 2, headID = 3;

        private int shoulderLeftID = 4, elbowLeftID = 5, wristLeftID = 6, handLeftID = 7;

        private int shoulderRightID = 8, elbowRightID = 9, wristRightID = 10, handRightID = 11;

        private int hipLeftID = 12, kneeLeftID = 13, ankleLeftID = 14, footLeftID = 15;

        private int hipRightID = 16, kneeRightID = 17, ankleRightID = 18, footRightID = 19;

        /*

        *  Initialisation de la classe principale.

        *  Affiche l'écran de debug dans lequel on voit la distance à la Kinect,

        *  les mains détectées et le squelette de l'utilisateur.

        */

        public KinectMain()

        {

            //On crée la fenêtre de debug.

            debug = new Debug.DebugWindow(this);

            //On crée le détecteur de gestes.

            swipeDetector = new SwipeDetector();

            //On tente de charger les paramètres du fichier params.ini.

            //Si on n'y arrive pas, on affiche une erreur et on charge les paramètres par défaut.

            if (!loadParameters())

            {

                debug.ExceptionLbl.Content = "Impossible de charger les paramètres. Paramètres par défaut.";

                //Distances de détection des mains par défaut pour la recherche (ici de 1m à 2m de la Kinect).

                minDistHands = (float)1.0;

                maxDistHands = (float)1.5;

            }

            else if (maxDistHands <= 0 || minDistHands <= 0 || maxDistHands > 4 || minDistHands > 4 || minDistHands >= maxDistHands)

            {

                debug.ExceptionLbl.Content = "Paramètres incorrects. Paramètres par défaut.";

                //Distances de détection des mains par défaut pour la recherche (ici de 1m à 2m de la Kinect).

                minDistHands = (float)1.0;

                maxDistHands = (float)1.5;

            }

            //On affiche la fenêtre de debug.

            try

            {

                debug.ShowDialog();

            }

            catch(Exception){}

        }

        /*

        *  Initialisation du sensor de la Kinect.

        */

        public void KinectInitialization()

        {

            try

            {

                //On sélectionne la première kinect détectée.

                kinectSensor = KinectSensor.KinectSensors.FirstOrDefault(s => s.Status == KinectStatus.Connected);

                //La caméra couleur est activée avec une résolution 640x480 et un framerate de 30 FPS.

                kinectSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);

                //La caméra de profondeur est activée.

                kinectSensor.DepthStream.Enable();

                //Le squelette est activé.

                kinectSensor.SkeletonStream.Enable();

                //Quand le Middleware reçoit des trames de la Kinect, on va dans cette fonction.

                kinectSensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(AllFramesReady);

                //On démarre la Kinect.

                kinectSensor.Start();

                debug.ExceptionLbl.Content = "";

            }

            catch (System.Exception)

            {

                debug.ExceptionLbl.Content = "Kinect non connectée.";

            }

            //Pour les événements main gauche/droite entre dans/quitte le champ, on a 4 listeners.

            //Fonction appelée lorsque la main gauche entre dans le champ de recherche.

            LeftHandTrackedListener leftHandTrackedListener = new LeftHandTrackedListener();

            LeftHandTrackedEvent += new LeftHandTrackedHandler(leftHandTrackedListener.ShowOnScreen);

            //Fonction appelée lorsque la main droite entre dans le champ de recherche.

            RightHandTrackedListener rightHandTrackedListener = new RightHandTrackedListener();

            RightHandTrackedEvent += new RightHandTrackedHandler(rightHandTrackedListener.ShowOnScreen);

            //Fonction appelée lorsque la main gauche quitte le champ de recherche.

            LeftHandQuitListener leftHandQuitListener = new LeftHandQuitListener();

            LeftHandQuitEvent += new LeftHandQuitHandler(leftHandQuitListener.ShowOnScreen);

            //Fonction appelée lorsque la main droite quitte le champ de recherche.

            RightHandQuitListener rightHandQuitListener = new RightHandQuitListener();

            RightHandQuitEvent += new RightHandQuitHandler(rightHandQuitListener.ShowOnScreen);

            //Fonction appelée lorsque l'utilisateur effectue un swipe right.

            SwipeEventListener swipeListener = new SwipeEventListener();

            SwipeEvent += new SwipeHandler(swipeListener.ShowOnScreen);

            //On connecte le serveur à l'adresse locale sur le port 80.

            server = new Server("127.0.0.1", 80);

        }

        /*

        *  Fermeture du sensor de la Kinect.

        */

        public void KinectClose()

        {

            try

            {

                //On stoppe la Kinect.

                kinectSensor.Stop();

                //On met a zero l'image d'affichage et le serveur.

                debug.DebugImage.Source = null;

                //server = null;

                debug.ExceptionLbl.Content = "";

            }

            catch (System.Exception)

            {

                debug.ExceptionLbl.Content = "Kinect non connectée.";

            }

        }

        /*

        *  Récupère le premier squelette.

        */

        Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)

        {

            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())

            {

                if (skeletonFrameData == null)

                    return null;

                if ((skeletons == null) || (skeletons.Length != skeletonFrameData.SkeletonArrayLength))

                    skeletons = new Skeleton[skeletonFrameData.SkeletonArrayLength];

                skeletonFrameData.CopySkeletonDataTo(skeletons);

                //On obtient le premier skelette.

                Skeleton first = (from s in skeletons where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();

                return first;

            }

        }

        /*

        *  Récupère le squelette le plus proche.

        */

        Skeleton GetNearestSkeleton(AllFramesReadyEventArgs e)

        {

            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())

            {

                if (skeletonFrameData == null)

                    return null;

                if ((skeletons == null) || (skeletons.Length != skeletonFrameData.SkeletonArrayLength))

                    skeletons = new Skeleton[skeletonFrameData.SkeletonArrayLength];

                skeletonFrameData.CopySkeletonDataTo(skeletons);

                Skeleton s;

                float minDist = (float)-1.0;

                int minID = 0;

                //Pour tous les squelettes.

                for(int i = 0 ; i < skeletons.Count() ; i++)

                {

                    s = skeletons.ElementAt(i);

                    //S'il est tracké.

                    if(s.TrackingState == SkeletonTrackingState.Tracked)

                    {

                        //On récupère sa position et on obtient la distance min et l'ID du squelette qui est à la distance min.

                        float dist = skeletons.ElementAt(i).Position.Z;

                        if (minDist == -1)

                        {

                            minDist = dist;

                            minID = i;

                        }

                        else if(minDist > dist)

                        {

                            minDist = dist;

                            minID = i;

                        }

                    }

                }

                //On renvoie le skelette le plus proche.

                return skeletons.ElementAt(minID);

            }

        }

        /*

        *  Récupère le squelette le plus proche.

        */

        private void AllFramesReady(object sender, AllFramesReadyEventArgs e)

        {

            //On ne calcule rien si la fenêtre de debug se ferme.

            if (debug.isClosing())

                return;

            //On met à jour la vidéo de debug.

            debug.RefreshVideo(e);

            //On récupère le premier squelette tracké.

            //Skeleton first = GetFirstSkeleton(e);

            //On récupère le plus proche squelette tracké.

            Skeleton first = GetNearestSkeleton(e);

            //Si celui-ci n’est pas nul

            if (first == null)

                return;

            //Si ce squelette est tracké (donc suivi et reconnu par la camera)

            if (first.TrackingState == SkeletonTrackingState.Tracked)

            {

                //Ensemble des noeuds du squelette.

                Joint hipCenter = getJoint(first, hipCenterID), spine = getJoint(first, spineID), shoulderCenter = getJoint(first, shoulderCenterID), head = getJoint(first, headID);

                Joint shoulderLeft = getJoint(first, shoulderLeftID), elbowLeft = getJoint(first, elbowLeftID), wristLeft = getJoint(first, wristLeftID), handLeft = getJoint(first, handLeftID);

                Joint shoulderRight = getJoint(first, shoulderRightID), elbowRight = getJoint(first, elbowRightID), wristRight = getJoint(first, wristRightID), handRight = getJoint(first, handRightID);

                Joint hipLeft = getJoint(first, hipLeftID), kneeLeft = getJoint(first, kneeLeftID), ankleLeft = getJoint(first, ankleLeftID), footLeft = getJoint(first, footLeftID);

                Joint hipRight = getJoint(first, hipRightID), kneeRight = getJoint(first, kneeRightID), ankleRight = getJoint(first, ankleRightID), footRight = getJoint(first, footRightID);

                //On construit l'historique des postures.

                /*List<Joint> joints = new List<Joint>();

                joints.Clear();

                joints.Add(footRight);

                joints.Add(ankleRight);

                joints.Add(kneeRight);

                joints.Add(hipRight);

                joints.Add(footLeft);

                joints.Add(ankleLeft);

                joints.Add(kneeLeft);

                joints.Add(hipLeft);

                joints.Add(handRight);

                joints.Add(wristRight);

                joints.Add(elbowRight);

                joints.Add(shoulderRight);

                joints.Add(handLeft);

                joints.Add(wristLeft);

                joints.Add(elbowLeft);

                joints.Add(shoulderLeft);

                joints.Add(head);

                joints.Add(shoulderCenter);

                joints.Add(spine);

                joints.Add(hipCenter);

                swipeDetector.UpdateSkeletonHistory(joints);*/

                SkeletonDisplayManager sdm = new SkeletonDisplayManager(kinectSensor, debug.DebugCanvas);

                //sdm.Draw();

                //On obtient sa distance à la Kinect.

                float distance = first.Position.Z;

                //On affiche la distance dans le debug.

                debug.showDistance(distance);

                //Si la main gauche est dans le champ, on lance l'événement approprié.

                if (handLeft.Position.Z < maxDistHands && handLeft.Position.Z > minDistHands)

                {

                    LeftHandTrackedEventArgs leftHandTrackedEvent = new LeftHandTrackedEventArgs(handLeft, handLeft.Position.Z, debug, server);

                    OnLeftHandTrackedEvent(leftHandTrackedEvent);

                }

                //Si la main gauche quitte le champ, on lance l'événement approprié.

                else

                {

                    LeftHandQuitEventArgs leftHandQuitEvent = new LeftHandQuitEventArgs(handLeft, handLeft.Position.Z, debug, server);

                    OnLeftHandQuitEvent(leftHandQuitEvent);

                }

                //Si la main droite est dans le champ, on lance l'événement approprié.

                if (handRight.Position.Z < maxDistHands && handRight.Position.Z > minDistHands)

                {

                    RightHandTrackedEventArgs rightHandTrackedEvent = new RightHandTrackedEventArgs(handRight, handRight.Position.Z, debug, server);

                    OnRightHandTrackedEvent(rightHandTrackedEvent);

                }

                //Si la main droite quitte le champ, on lance l'événement approprié.

                else

                {

                    RightHandQuitEventArgs rightHandQuitEvent = new RightHandQuitEventArgs(handRight, handRight.Position.Z, debug, server);

                    OnRightHandQuitEvent(rightHandQuitEvent);

                }

                //Si l'utilisateur effectue un swipe left.

                if (swipeDetector.CheckForSwipeLeft())

                {

                    SwipeEventArgs swipeEvent = new SwipeEventArgs(debug, server, SwipeDetector.Direction.LEFT);

                    OnSwipeEvent(swipeEvent);

                }

                //Dessine le squelette dans le debug.

                //debug.drawJoints(first.Joints, first);

                //debug.showSkeleton(hipCenter, spine, shoulderCenter, head, shoulderLeft, elbowLeft, wristLeft, handLeft, shoulderRight, elbowRight, wristRight, handRight, hipLeft, kneeLeft, ankleLeft, footLeft, hipRight, kneeRight, ankleRight, footRight);

            }

        }

        /*

        *  Change l'échelle des coordonnées d'un noeud pour qu'en X et Y il corresponde à la résolution et en Z à la distance à la Kinect.

        */

        public Joint getJoint(Skeleton ske, int jointID)

        {

            return Coding4Fun.Kinect.Wpf.SkeletalExtensions.ScaleTo(ske.Joints.ElementAt(jointID), 600, 400, 0.75f, 0.75f);

        }

        /*

        *  Initialise l'événement et fait appel aux fonctions du listener quand la main gauche entre dans le champ.

        */

        public static void OnLeftHandTrackedEvent(LeftHandTrackedEventArgs e)

        {

            if (LeftHandTrackedEvent != null)

                LeftHandTrackedEvent(new object(), e);