using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit;
using Microsoft.Kinect.Toolkit.Interaction;

namespace KinPortal {
	public class HandsTracker : IInteractionClient {
		public double InteractionRegionWidth {get; set;}
		public double InteractionRegionHeight {get; set;}
		private const int InvalidTrackingId = 0;
		private readonly KinectSensorChooser sensorChooser = new KinectSensorChooser();
		private readonly HashSet<int> trackedUsers = new HashSet<int>();

		private InteractionStream interactionStream;
        private KinectSensor kinectSensor;
        private Skeleton[] skeletons;
        private UserInfo[] userInfos;

		public HandsState LeftHand {get; private set;}
		public HandsState RightHand {get; private set;}

		public HandsTracker(double screenWidth, double screenHeight) {
			InteractionRegionWidth = screenWidth;
			InteractionRegionHeight = screenHeight;
			LeftHand = new HandsState();
			RightHand = new HandsState();
		}

		public void Run() {
			this.sensorChooser.KinectChanged += this.OnSensorChanged;
			this.sensorChooser.Start();
		}
        
		#region PressAndGripAdjustment
		public InteractionInfo GetInteractionInfoAtLocation(int skeletonTrackingId, InteractionHandType handType, double x, double y) {
			return new InteractionInfo();
		}
		#endregion PressAndGripAdjustment
        

		#region Configuration
		private void InitializeInteractions(KinectSensor sensor) {
			this.skeletons = new Skeleton[sensor.SkeletonStream.FrameSkeletonArrayLength];
			this.userInfos = new UserInfo[InteractionFrame.UserInfoArrayLength];

			sensor.DepthFrameReady += this.SensorDepthFrameReady;
			sensor.SkeletonFrameReady += this.SensorSkeletonFrameReady;
			
			this.interactionStream = new InteractionStream(sensor, this);
			this.interactionStream.InteractionFrameReady += this.InteractionFrameReady;
		}

		private void UninitializeInteractions(KinectSensor sensor) {
			sensor.DepthFrameReady -= this.SensorDepthFrameReady;
			sensor.SkeletonFrameReady -= this.SensorSkeletonFrameReady;
			
			this.skeletons = null;
			this.userInfos = null;
			
			this.interactionStream.InteractionFrameReady -= this.InteractionFrameReady;
			this.interactionStream.Dispose();
			this.interactionStream = null;
		}

		private void OnSensorChanged(object sender, KinectChangedEventArgs args) {
			if (args.OldSensor != null) {
				this.UninitializeInteractions(args.OldSensor);
				try {
					args.OldSensor.SkeletonStream.AppChoosesSkeletons = false;
					args.OldSensor.DepthStream.Range = DepthRange.Default;
					args.OldSensor.SkeletonStream.EnableTrackingInNearRange = false;
					args.OldSensor.DepthStream.Disable();
					args.OldSensor.SkeletonStream.Disable();
				} catch (InvalidOperationException) {
					// KinectSensor might enter an invalid state while enabling/disabling streams or stream features.
					// E.g.: sensor might be abruptly unplugged.
				}
			}

			this.kinectSensor = null;

			if (args.NewSensor != null) {
				try {
					// InteractionStream needs 640x480 depth data stream
					args.NewSensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
					args.NewSensor.SkeletonStream.Enable();

                    try {
						// Interactions work better in near range
						args.NewSensor.DepthStream.Range = DepthRange.Near;
						args.NewSensor.SkeletonStream.EnableTrackingInNearRange = true;
					} catch (InvalidOperationException) {
						// Non Kinect for Windows devices do not support Near mode, so reset back to default mode.
						args.NewSensor.DepthStream.Range = DepthRange.Default;
						args.NewSensor.SkeletonStream.EnableTrackingInNearRange = false;
					}
				} catch (InvalidOperationException) {
					// KinectSensor might enter an invalid state while enabling/disabling streams or stream features.
					// E.g.: sensor might be abruptly unplugged.
				}

				this.kinectSensor = args.NewSensor;

				this.InitializeInteractions(args.NewSensor);
			}
		}

		#endregion Configuration

        #region Processing
		private void SensorDepthFrameReady(object sender, DepthImageFrameReadyEventArgs depthImageFrameReadyEventArgs) {
			if (this.kinectSensor != sender) {
				return;
			}

			using (DepthImageFrame depthFrame = depthImageFrameReadyEventArgs.OpenDepthImageFrame()) {
				if (null != depthFrame) {
					try {
						// Hand data to Interaction framework to be processed
						this.interactionStream.ProcessDepth(depthFrame.GetRawPixelData(), depthFrame.Timestamp);
					} catch (InvalidOperationException) {
						// DepthFrame functions may throw when the sensor gets
						// into a bad state.  Ignore the frame in that case.
					}
				}
			}
		}

		private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs skeletonFrameReadyEventArgs) {
			if (this.kinectSensor != sender) {
				return;
			}

			using (SkeletonFrame skeletonFrame = skeletonFrameReadyEventArgs.OpenSkeletonFrame()) {
				if (null != skeletonFrame) {
					try {
						skeletonFrame.CopySkeletonDataTo(this.skeletons);
						var accelerometerReading = this.kinectSensor.AccelerometerGetCurrentReading();
						this.interactionStream.ProcessSkeleton(this.skeletons, accelerometerReading, skeletonFrame.Timestamp);
					} catch (InvalidOperationException) {
						// SkeletonFrame functions may throw when the sensor gets
						// into a bad state.  Ignore the frame in that case.
					}
				}
			}
		}

		private void InteractionFrameReady(object sender, InteractionFrameReadyEventArgs e) {
		    // Check for a null userInfos since we may still get posted events
		    // from the stream after we have unregistered our event handler and
		    // deleted our buffers.
		    if (this.userInfos == null) {
		        return;
		    }

		    UserInfo[] localUserInfos = null;
		    long timestamp = 0;
			
			using (InteractionFrame interactionFrame = e.OpenInteractionFrame()) {
			    if (interactionFrame != null) {
			        interactionFrame.CopyInteractionDataTo(this.userInfos);
			        timestamp = interactionFrame.Timestamp;
			        localUserInfos = this.userInfos;
			    }
			}

			if (localUserInfos != null) {
				var currentUserSet = new HashSet<int>();
				var usersToRemove = new HashSet<int>();

				foreach (var info in localUserInfos) {
				    if (info.SkeletonTrackingId == InvalidTrackingId) {
				        // Only look at user information corresponding to valid users
				        continue;
				    }
					
					/*if (!this.trackedUsers.Contains(info.SkeletonTrackingId)) {
					    Console.WriteLine("New user '{0}' entered scene at time {1}", info.SkeletonTrackingId, timestamp);
					}*/

					currentUserSet.Add(info.SkeletonTrackingId);
					this.trackedUsers.Add(info.SkeletonTrackingId);
					
					// Perform hit testing and look for Grip and GripRelease events
					foreach (var handPointer in info.HandPointers) { 
						if (handPointer.HandType.ToString() == "Right") {
							RightHand.Pos = new Point(
								(int) ((handPointer.X / 2) * InteractionRegionWidth),
								(int) ((handPointer.Y / 2) * InteractionRegionHeight)
							);
							RightHand.Depth = handPointer.RawZ;
							RightHand.PrevIsClosed = RightHand.IsClosed;
							
							if(handPointer.HandEventType != InteractionHandEventType.None) {
								if (handPointer.HandEventType.ToString() == "Grip")
									RightHand.IsClosed = true;
								else
									RightHand.IsClosed = false;
							}
						} else {
							LeftHand.Pos = new Point(
								(int) ((handPointer.X / 2) * InteractionRegionWidth),
								(int) ((handPointer.Y / 2) * InteractionRegionHeight)
							);
							LeftHand.Depth = handPointer.RawZ;
							LeftHand.PrevIsClosed = LeftHand.IsClosed;

							if (handPointer.HandEventType != InteractionHandEventType.None) {
								if (handPointer.HandEventType.ToString() == "Grip")
									LeftHand.IsClosed = true;
								else
									LeftHand.IsClosed = false;
							}
						} 
					}
				}

				foreach (var id in this.trackedUsers) {
				    if (!currentUserSet.Contains(id)) {
				        usersToRemove.Add(id);
				    }
				}

				foreach (var id in usersToRemove) {
				    this.trackedUsers.Remove(id);
				}
			}
		}
		
		#endregion Processing
	}
}