text: ﻿using System;
  using System.Collections.Generic;
  using System.Linq;
  using System.Text;
  using WindowsPreview.Kinect;
  using Microsoft.Kinect.Face;
  using Windows.UI.Xaml.Controls;
  using Windows.UI;
  using Windows.Foundation;
  using System.Diagnostics;
  using Windows.UI.Xaml.Shapes;
  using Windows.UI.Xaml.Media;
  
  namespace KinectFace
  {
      public class FaceManager
      {
          /// <summary>
          /// Face rotation display angle increment in degrees
          /// </summary>
          private const double FaceRotationIncrementInDegrees = 5.0;
  
          /// <summary>
          /// Active Kinect sensor
          /// </summary>
          private KinectSensor kinectSensor = null;
  
          /// <summary>
          /// Reader for body frames
          /// </summary>
          private BodyFrameReader bodyFrameReader = null;
  
          /// <summary>
          /// Array to store bodies
          /// </summary>
          private Body[] bodies = null;
  
          /// <summary>
          /// Number of bodies tracked
          /// </summary>
          private int bodyCount;
  
          /// <summary>
          /// Face frame sources
          /// </summary>
          private FaceFrameSource[] faceFrameSources = null;
  
          /// <summary>
          /// Face frame readers
          /// </summary>
          private FaceFrameReader[] faceFrameReaders = null;
  
          /// <summary>
          /// List of colors for each face tracked
          /// </summary>
          private List<Color> faceColors;
  
          /// <summary>
          /// Create a new FaceManager using an existing sensor which has already opened.
          /// The faceFrameResult will return the requested faceFeatures.
          /// </summary>
          /// <param name="sensor"></param>
          /// <param name="faceFeatures"></param>
          public FaceManager(KinectSensor sensor, FaceFrameFeatures faceFeatures)
          {
              this.kinectSensor = sensor;
  
              // open the reader for the body frames
              this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();
  
              // wire handler for body frame arrival
              this.bodyFrameReader.FrameArrived += this.Reader_BodyFrameArrived;
  
              this.bodyCount = this.kinectSensor.BodyFrameSource.BodyCount;
  
              // allocate storage to store body objects
              this.bodies = new Body[this.bodyCount];
  
              // create a face frame source + reader to track each face in the FOV
              this.faceFrameSources = new FaceFrameSource[this.bodyCount];
              this.faceFrameReaders = new FaceFrameReader[this.bodyCount];
  
              for (int i = 0; i < this.bodyCount; i++)
              {
                  // create the face frame source with the required face frame features and an initial tracking Id of 0
                  this.faceFrameSources[i] = new FaceFrameSource(this.kinectSensor, 0, faceFeatures);
  
                  // open the corresponding reader
                  this.faceFrameReaders[i] = this.faceFrameSources[i].OpenReader();
              }
  
              // populate face result colors - one for each face index
              this.faceColors = new List<Color>()
              {
                  Colors.Red,
                  Colors.Orange,
                  Colors.Green,
                  Colors.LightBlue,
                  Colors.Indigo,
                  Colors.Violet
              };
          }
  
          /// <summary>
          /// Stop all the frame readers required for face tracking.
          /// </summary>
          /// <returns></returns>
          public bool StopFrameReaders()
          {
              for (int i = 0; i < this.bodyCount; i++)
              {
                  if (this.faceFrameReaders[i] != null)
                  {
                      // FaceFrameReader is IDisposable
                      this.faceFrameReaders[i].Dispose();
                      this.faceFrameReaders[i] = null;
                  }
  
                  if (this.faceFrameSources[i] != null)
                  {
                      this.faceFrameSources[i] = null;
                  }
              }
  
              if (this.bodyFrameReader != null)
              {
                  // BodyFrameReader is IDisposable
                  this.bodyFrameReader.Dispose();
                  this.bodyFrameReader = null;
              }
              return false;
          }
  
          /// <summary>
          /// Responding to the BodyFrameArrived event. This maps the faceFrameSources 
          /// tracking id's to those of the tracked bodies.
          /// </summary>
          private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
          {
              using (var bodyFrame = e.FrameReference.AcquireFrame())
              {
                  if (bodyFrame != null)
                  {
                      // update body data
                      bodyFrame.GetAndRefreshBodyData(this.bodies);
  
                      // iterate through each face source
                      for (int i = 0; i < this.bodyCount; i++)
                      {
                          // check if a valid face is tracked in this face source
                          if (!this.faceFrameSources[i].IsTrackingIdValid)
                          {
                              // check if the corresponding body is tracked 
                              if (this.bodies[i].IsTracked)
                              {
                                  // update the face frame source to track this body
                                  this.faceFrameSources[i].TrackingId = this.bodies[i].TrackingId;
                              }
                          }
                      }
                  }
              }
          }
  
          /// <summary>
          /// Get the latest FaceFrameResult from a single bodyIndex. If there is a valid body tracked
          /// at that index then the FaceFrameResult will be valid.
          /// </summary>
          /// <param name="bodyId">The index of the tracked body</param>
          /// <returns>A FaceFrameResult from the matching FaceFrameReader</returns>
          public FaceFrameResult GetLatestFaceFrameResult(int bodyId)
          {
              if (this.faceFrameReaders[bodyId] != null)
              {
                  return this.faceFrameReaders[bodyId].AcquireLatestFrame().FaceFrameResult;
              }
              return null;
          }
  
          /// <summary>
          /// Acquire the latest FaceFrameResults from all the FaceFrameReaders. 
          /// They must be checked for validity.
          /// </summary>
          /// <returns>Array of FaceFrames of bodyCount size (6 with KinectV2)</returns>
          public FaceFrameResult[] GetLatestFaceFrameResults()
          {
              FaceFrameResult[] results = new FaceFrameResult[bodyCount];
              for (int i = 0; i < this.bodyCount; i++)
              {
                  if (this.faceFrameReaders[i] != null)
                  {
                      // wire handler for face frame arrival
                      FaceFrame frame= this.faceFrameReaders[i].AcquireLatestFrame();
                      if (frame != null && frame.FaceFrameResult != null)
                      {
                          results[i] = frame.FaceFrameResult;
                      }
                  }
              }
  
              return results;
          }
  
          /// <summary>
          /// Acquire the latest valid face frames and draw their data to a panel with 
          /// WPF shapes and text.
          /// </summary>
          /// <param name="parentPanel">The Panel in which the shapes will be children. 
          /// Make sure the parentPanel has its width and height set to what is expected 
          /// for the requested features.</param>
          /// <param name="displayFeatures">The FaceFrameFeatures to be rendered, use flags to 
          /// display many features. Be aware of ColorSpace vs InfraredSpace when choosing.</param>
          public void DrawLatestFaceResults(Panel parentPanel, FaceFrameFeatures displayFeatures)
          {
              parentPanel.Children.Clear();
              for (int i = 0; i < this.bodyCount; i++)
              {
                  if (this.faceFrameReaders[i] != null)
                  {
                      FaceFrame frame = this.faceFrameReaders[i].AcquireLatestFrame();
                      if (frame != null && frame.FaceFrameResult != null)
                      {
                          DrawFaceFeatures(parentPanel, frame.FaceFrameResult, displayFeatures, this.faceColors[i], i);
                      }
                  }
              }
          }
  
          /// <summary>
          /// Create shapes and text to apply to a parent panel which represent the
          /// current state of a faceFrameResult.
          /// </summary>
          /// <param name="parentPanel">The Panel in which the shapes will be added</param>
          /// <param name="faceFrameResult">The reult of the face tracking</param>
          /// <param name="displayFeatures">The FaceFrameFeatures to be drawn</param>
          /// <param name="color">The color of the shapes and text</param>
          /// <param name="bodyIndex">The index of the body/face</param>
          private void DrawFaceFeatures(Panel parentPanel, FaceFrameResult faceFrameResult, FaceFrameFeatures displayFeatures, Color color, int bodyIndex)
          {
              if (parentPanel.Width == 0 ||
                  Double.IsNaN(parentPanel.Width) ||
                  parentPanel.Height == 0 ||
                  Double.IsNaN(parentPanel.Height))
              {
                  // The parent Panel must have a size to be rendered on
                  return;
              }
  
              string messages = "";
              bool renderMessages = false;
              int fontSize = (int)((double)parentPanel.Height * (double)0.023);
              Point messagesPosition = new Point(fontSize * bodyIndex * 10, 0);
  
  
              // Face points and bounding boxes
              if (displayFeatures.HasFlag(FaceFrameFeatures.BoundingBoxInColorSpace))
              {
                  double lineSize = 7;
                  int posX = faceFrameResult.FaceBoundingBoxInColorSpace.Left;
                  int posY = faceFrameResult.FaceBoundingBoxInColorSpace.Top;
                  int width = faceFrameResult.FaceBoundingBoxInColorSpace.Right - posX + (int)lineSize;
                  int height = faceFrameResult.FaceBoundingBoxInColorSpace.Bottom - posY + (int)lineSize;
                  Rectangle rect = CreateFaceBoxRectangle(color, lineSize, width, height);
                  Canvas.SetLeft(rect, posX);
                  Canvas.SetTop(rect, posY);
                  parentPanel.Children.Add(rect);
                  messagesPosition = new Point(posX, posY + height);
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.BoundingBoxInInfraredSpace))
              {
                  double lineSize = 7;
                  int posX = faceFrameResult.FaceBoundingBoxInInfraredSpace.Left;
                  int posY = faceFrameResult.FaceBoundingBoxInInfraredSpace.Top;
                  int width = faceFrameResult.FaceBoundingBoxInInfraredSpace.Right - posX + (int)lineSize;
                  int height = faceFrameResult.FaceBoundingBoxInInfraredSpace.Bottom - posY + (int)lineSize;
                  Rectangle rect = CreateFaceBoxRectangle(color, lineSize, width, height);
                  Canvas.SetLeft(rect, posX);
                  Canvas.SetTop(rect, posY);
                  parentPanel.Children.Add(rect);
                  messagesPosition = new Point(posX, posY + height);
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.PointsInColorSpace))
              {
                  foreach (KeyValuePair<FacePointType, Point> facePointKVP in
                          faceFrameResult.FacePointsInColorSpace)
                  {
                      Size ellipseSize = new Size(10, 10);
                      Ellipse ellipse = CreateFacePointEllipse(color, ellipseSize);
                      Canvas.SetLeft(ellipse, facePointKVP.Value.X - (ellipseSize.Width / 2));
                      Canvas.SetTop(ellipse, facePointKVP.Value.Y - (ellipseSize.Height / 2));
                      parentPanel.Children.Add(ellipse);
                  }
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.PointsInInfraredSpace))
              {
                  foreach (KeyValuePair<FacePointType, Point> facePointKVP in
                          faceFrameResult.FacePointsInInfraredSpace)
                  {
                      Size ellipseSize = new Size(3, 3);
                      Ellipse ellipse = CreateFacePointEllipse(color, ellipseSize);
                      Canvas.SetLeft(ellipse, facePointKVP.Value.X - (ellipseSize.Width / 2));
                      Canvas.SetTop(ellipse, facePointKVP.Value.Y - (ellipseSize.Height / 2));
                      parentPanel.Children.Add(ellipse);
                  }
              }
              // Rotation stuff
              if (displayFeatures.HasFlag(FaceFrameFeatures.RotationOrientation))
              {
                  int pitch, yaw, roll = 0;
                  ExtractFaceRotationInDegrees(faceFrameResult.FaceRotationQuaternion,
                      out pitch, out yaw, out roll);
                  messages += "Rotation Pitch: " + pitch + "\n";
                  messages += "Rotation Yaw: " + yaw + "\n";
                  messages += "Rotation Roll: " + roll + "\n";
                  renderMessages = true;
              }
  
              // Other Face Properties and states
              if (displayFeatures.HasFlag(FaceFrameFeatures.FaceEngagement))
              {
                  messages += FacePropertyToString(FaceProperty.Engaged,
                      faceFrameResult.FaceProperties[FaceProperty.Engaged]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.Glasses))
              {
                  messages += FacePropertyToString(FaceProperty.WearingGlasses,
                      faceFrameResult.FaceProperties[FaceProperty.WearingGlasses]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.Happy))
              {
                  messages += FacePropertyToString(FaceProperty.Happy,
                      faceFrameResult.FaceProperties[FaceProperty.Happy]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.LeftEyeClosed))
              {
                  messages += FacePropertyToString(FaceProperty.LeftEyeClosed,
                      faceFrameResult.FaceProperties[FaceProperty.LeftEyeClosed]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.RightEyeClosed))
              {
                  messages += FacePropertyToString(FaceProperty.RightEyeClosed,
                      faceFrameResult.FaceProperties[FaceProperty.RightEyeClosed]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.LookingAway))
              {
                  messages += FacePropertyToString(FaceProperty.LookingAway,
                      faceFrameResult.FaceProperties[FaceProperty.LookingAway]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.MouthMoved))
              {
                  messages += FacePropertyToString(FaceProperty.MouthMoved,
                      faceFrameResult.FaceProperties[FaceProperty.MouthMoved]);
                  renderMessages = true;
              }
              if (displayFeatures.HasFlag(FaceFrameFeatures.MouthOpen))
              {
                  messages += FacePropertyToString(FaceProperty.MouthOpen,
                      faceFrameResult.FaceProperties[FaceProperty.MouthOpen]);
                  renderMessages = true;
              }
  
              if (renderMessages)
              {
                  TextBlock textBlock = new TextBlock();
                  textBlock.Text = messages;
                  textBlock.Foreground = new SolidColorBrush(color);
                  textBlock.FontSize = fontSize;
                  Canvas.SetLeft(textBlock, messagesPosition.X);
                  Canvas.SetTop(textBlock, messagesPosition.Y);
                  parentPanel.Children.Add(textBlock);
              }
          }
  
          /// <summary>
          /// Create a Rectangle to display
          /// </summary>
          /// <param name="color">The Color of the stroke</param>
          /// <param name="strokeThickness">The thickness of the stroke</param>
          /// <param name="width">The width of the Rectangle</param>
          /// <param name="height">The height of the Rectangle</param>
          /// <returns></returns>
          private Rectangle CreateFaceBoxRectangle(Color color, double strokeThickness, double width, double height)
          {
              Rectangle rect = new Rectangle();
              rect.Width = width;
              rect.Height = height;
              rect.Stroke = new SolidColorBrush(color);
              rect.StrokeThickness = strokeThickness;
              rect.StrokeLineJoin = PenLineJoin.Round;
              rect.Fill = new SolidColorBrush(Color.FromArgb(0, 0, 0, 0));
              return rect;
          }
  
          /// <summary>
          /// Create an Ellipse to display
          /// </summary>
          /// <param name="color">The Color of the Ellipse</param>
          /// <param name="size">The Size of the Ellipse</param>
          /// <returns></returns>
          private Ellipse CreateFacePointEllipse(Color color, Size size)
          {
              Ellipse baseEllipse = new Ellipse();
              baseEllipse.Width = size.Width;
              baseEllipse.Height = size.Height;
              baseEllipse.Fill = new SolidColorBrush(color);
              return baseEllipse;
          }
  
          /// <summary>
          /// Convert the FaceProperty and DetectionResult to a display string   
          /// </summary>
          /// <param name="prop">The FaceProperty enum type</param>
          /// <param name="result">The DetectionResult of the FaceProperty</param>
          /// <returns></returns>
          private string FacePropertyToString(FaceProperty prop, DetectionResult result)
          {
              string str = "";
              str += Enum.GetName(typeof(FaceProperty), prop) + ": ";
              str += Enum.GetName(typeof(DetectionResult), result) + "\n";
              return str;
          }
  
  
          /// <summary>
          /// Converts rotation quaternion to Euler angles 
          /// And then maps them to a specified range of values to control the refresh rate
          /// </summary>
          /// <param name="rotQuaternion">face rotation quaternion</param>
          /// <param name="pitch">rotation about the X-axis</param>
          /// <param name="yaw">rotation about the Y-axis</param>
          /// <param name="roll">rotation about the Z-axis</param>
          private static void ExtractFaceRotationInDegrees(Vector4 rotQuaternion, out int pitch, out int yaw, out int roll)
          {
              double x = rotQuaternion.X;
              double y = rotQuaternion.Y;
              double z = rotQuaternion.Z;
              double w = rotQuaternion.W;
  
              // convert face rotation quaternion to Euler angles in degrees
              double yawD, pitchD, rollD;
              pitchD = Math.Atan2(2 * ((y * z) + (w * x)), (w * w) - (x * x) - (y * y) + (z * z)) / Math.PI * 180.0;
              yawD = Math.Asin(2 * ((w * y) - (x * z))) / Math.PI * 180.0;
              rollD = Math.Atan2(2 * ((x * y) + (w * z)), (w * w) + (x * x) - (y * y) - (z * z)) / Math.PI * 180.0;
  
              // clamp the values to a multiple of the specified increment to control the refresh rate
              double increment = FaceRotationIncrementInDegrees;
              pitch = (int)(Math.Floor((pitchD + ((increment / 2.0) * (pitchD > 0 ? 1.0 : -1.0))) / increment) * increment);
              yaw = (int)(Math.Floor((yawD + ((increment / 2.0) * (yawD > 0 ? 1.0 : -1.0))) / increment) * increment);
              roll = (int)(Math.Floor((rollD + ((increment / 2.0) * (rollD > 0 ? 1.0 : -1.0))) / increment) * increment);
          }
  
      }
  }
  