#if (UNITY_STANDALONE_WIN)
using UnityEngine;
using System.Collections;
using Windows.Kinect;
using System.Runtime.InteropServices;
//using Microsoft.Kinect.Face;
using System.Collections.Generic;
using System;

namespace com.rfilkov.kinect
{
    /// <summary>
    /// Kinect2Interface is sensor-interface to the Kinect-v2 sensors.
    /// </summary>
    public class Kinect2Interface : DepthSensorBase
    {
        // change this to false, if you aren't using Kinect-v2 only and want KM to check for available sensors
        public static bool sensorAlwaysAvailable = true;

        [Tooltip("Whether the detected users face backwards or not.")]
        public bool usersFaceBackwards = false;

        public KinectSensor kinectSensor;
        public CoordinateMapper coordMapper;

        private BodyFrameReader bodyFrameReader;
        private BodyIndexFrameReader bodyIndexFrameReader;
        private ColorFrameReader colorFrameReader;
        private DepthFrameReader depthFrameReader;
        private InfraredFrameReader infraredFrameReader;

        private MultiSourceFrameReader multiSourceFrameReader;
        private MultiSourceFrame multiSourceFrame;

        private ColorFrame msColorFrame = null;
        private DepthFrame msDepthFrame = null;
        private InfraredFrame msInfraredFrame = null;
        private BodyFrame msBodyFrame = null;
        private BodyIndexFrame msBodyIndexFrame = null;


        private int kinectBodyCount = 0;
        private int kinectJointCount = 0;
        private Body[] kinectBodyData;

        private bool floorPlaneDetected = false;
        private Windows.Kinect.Vector4 vFloorPlane;


        // depth sensor settings
        [System.Serializable]
        public class K2SensorSettings : DepthSensorBase.BaseSensorSettings
        {
            public bool usersFaceBackwards;
        }


        public override KinectInterop.DepthSensorPlatform GetSensorPlatform()
        {
            return KinectInterop.DepthSensorPlatform.KinectV2;
        }


        public override System.Type GetSensorSettingsType()
        {
            return typeof(K2SensorSettings);
        }


        public override BaseSensorSettings GetSensorSettings(BaseSensorSettings settings)
        {
            if (settings == null)
            {
                settings = new K2SensorSettings();
            }

            K2SensorSettings extSettings = (K2SensorSettings)base.GetSensorSettings(settings);
            extSettings.usersFaceBackwards = usersFaceBackwards;

            return settings;
        }

        public override void SetSensorSettings(BaseSensorSettings settings)
        {
            if (settings == null)
                return;

            base.SetSensorSettings(settings);

            K2SensorSettings extSettings = (K2SensorSettings)settings;
            usersFaceBackwards = extSettings.usersFaceBackwards;
        }

        public override List<KinectInterop.SensorDeviceInfo> GetAvailableSensors()
        {
            List<KinectInterop.SensorDeviceInfo> alSensorInfo = new List<KinectInterop.SensorDeviceInfo>();
            KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo();

            KinectSensor sensor = KinectSensor.GetDefault();
            if (sensor != null)
            {
                if (sensorAlwaysAvailable)
                {
                    sensorInfo.sensorId = "KinectV2";
                    sensorInfo.sensorName = "Kinect-v2 Sensor";
                    sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll; // & ~KinectInterop.FrameSource.TypePose;
                    if (consoleLogMessages)
                        Debug.Log(string.Format("  D{0}: {1}, id: {2}", 0, sensorInfo.sensorName, sensorInfo.sensorId));

                    alSensorInfo.Add(sensorInfo);

                    return alSensorInfo;
                }
                else
                {
                    // check for available sensor
                    if (!sensor.IsOpen)
                    {
                        sensor.Open();
                    }

                    float fWaitTime = Time.realtimeSinceStartup + 3f;
                    while (!sensor.IsAvailable && Time.realtimeSinceStartup < fWaitTime)
                    {
                        // wait for availability
                    }

                    if (sensor.IsAvailable)
                    {
                        sensorInfo.sensorId = "KinectV2";
                        sensorInfo.sensorName = "Kinect-v2 Sensor";
                        sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll; // & ~KinectInterop.FrameSource.TypePose;
                        if (consoleLogMessages)
                            Debug.Log(string.Format("  D{0}: {1}, id: {2}", 0, sensorInfo.sensorName, sensorInfo.sensorId));

                        alSensorInfo.Add(sensorInfo);
                    }

                    if (sensor.IsOpen)
                    {
                        sensor.Close();
                    }

                    fWaitTime = Time.realtimeSinceStartup + 3f;
                    while (sensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
                    {
                        // wait for sensor to close
                    }
                }

                sensor = null;
            }

            //if(alSensorInfo.Count == 0)
            //{
            //    Debug.Log("  No sensor devices found.");
            //}

            return alSensorInfo;
        }

        public override KinectInterop.SensorData OpenSensor(KinectManager kinectManager, KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
        {
            // save initial parameters
            base.OpenSensor(kinectManager, dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);

            if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
            {
                Debug.LogWarning("Please use Kinect Studio v2.0 to play the sensor data recording!");
                //return null;
            }

            List<KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
            if (deviceIndex >= alSensors.Count)
            {
                Debug.LogError("  D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
                return null;
            }

            // try to get reference to the default sensor
            sensorDeviceId = alSensors[deviceIndex].sensorId;
            sensorPlatform = KinectInterop.DepthSensorPlatform.KinectV2;

            kinectSensor = KinectSensor.GetDefault();
            if (kinectSensor == null)
            {
                Debug.LogError("Kinect-v2 sensor not found!");
                return null;
            }

            KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
            sensorData.sensorIntPlatform = sensorPlatform;

            sensorData.sensorId = alSensors[deviceIndex].sensorId;
            sensorData.sensorName = alSensors[deviceIndex].sensorName;
            sensorData.sensorCaps = alSensors[deviceIndex].sensorCaps;

            // get reference to the coordinate mapper
            coordMapper = kinectSensor.CoordinateMapper;

            // flip color & depth image vertically
            sensorData.colorImageScale = new Vector3(1f, -1f, 1f);
            sensorData.depthImageScale = new Vector3(1f, -1f, 1f);
            sensorData.infraredImageScale = new Vector3(1f, -1f, 1f);
            sensorData.sensorSpaceScale = new Vector3(1f, 1f, 1f);
            sensorData.unitToMeterFactor = 1f;

            // depth camera offset & matrix z-flip
            sensorRotOffset = Vector3.zero;   // if for instance the depth camera is tilted downwards
            sensorRotFlipZ = false;
            sensorRotIgnoreY = false;

            // color
            var frameDesc = kinectSensor.ColorFrameSource.CreateFrameDescription(ColorImageFormat.Rgba);
            sensorData.colorImageWidth = frameDesc.Width;
            sensorData.colorImageHeight = frameDesc.Height;

            sensorData.colorImageFormat = TextureFormat.RGBA32;
            sensorData.colorImageStride = 4;  // 4 bytes per pixel

            if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
            {
                if (!isSyncDepthAndColor)
                    colorFrameReader = kinectSensor.ColorFrameSource.OpenReader();

                rawColorImage = new byte[frameDesc.LengthInPixels * frameDesc.BytesPerPixel];

                sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
                sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
                sensorData.colorImageTexture.filterMode = FilterMode.Point;
            }

            // depth
            sensorData.depthImageWidth = kinectSensor.DepthFrameSource.FrameDescription.Width;
            sensorData.depthImageHeight = kinectSensor.DepthFrameSource.FrameDescription.Height;

            if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
            {
                if (!isSyncDepthAndColor)
                    depthFrameReader = kinectSensor.DepthFrameSource.OpenReader();

                rawDepthImage = new ushort[kinectSensor.DepthFrameSource.FrameDescription.LengthInPixels];
                sensorData.depthImage = new ushort[kinectSensor.DepthFrameSource.FrameDescription.LengthInPixels];
            }

            // infrared
            if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
            {
                if (!isSyncDepthAndColor)
                    infraredFrameReader = kinectSensor.InfraredFrameSource.OpenReader();

                rawInfraredImage = new ushort[kinectSensor.InfraredFrameSource.FrameDescription.LengthInPixels];
                sensorData.infraredImage = new ushort[kinectSensor.InfraredFrameSource.FrameDescription.LengthInPixels];

                minInfraredValue = 0f;
                maxInfraredValue = 10000f;
            }

            if ((dwFlags & (KinectInterop.FrameSource.TypeBody | KinectInterop.FrameSource.TypeBodyIndex)) != 0)
            {
                if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
                {
                    if (!(isSyncDepthAndColor && isSyncBodyAndDepth))
                        bodyIndexFrameReader = kinectSensor.BodyIndexFrameSource.OpenReader();

                    //rawBodyIndexImage = new byte[kinectSensor.BodyIndexFrameSource.FrameDescription.LengthInPixels];  // created by InitBodyTracking()
                }

                if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
                {
                    if (!(isSyncDepthAndColor && isSyncBodyAndDepth))
                        bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();

                    kinectBodyCount = 6;
                    kinectJointCount = 25;
                    kinectBodyData = new Body[kinectBodyCount];
                }

                // init body tracking data
                InitBodyTracking(dwFlags, sensorData);
            }

            // don't get all frames
            getAllSensorFrames = false;

            //if(!kinectSensor.IsOpen)
            {
                //Debug.Log("Opening sensor, available: " + kinectSensor.IsAvailable);
                kinectSensor.Open();
            }

            float fWaitTime = Time.realtimeSinceStartup + 3f;
            while (!kinectSensor.IsAvailable && Time.realtimeSinceStartup < fWaitTime)
            {
                // wait for sensor to be available
            }

            //fWaitTime = Time.realtimeSinceStartup + 3f;
            while (!kinectSensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
            {
                // wait for sensor to open
            }

            if (consoleLogMessages)
                Debug.Log("D" + deviceIndex + " K2-sensor " + (kinectSensor.IsOpen ? "opened" : "closed") + ", available: " + kinectSensor.IsAvailable);

            if (isSyncDepthAndColor && dwFlags != KinectInterop.FrameSource.TypeNone && kinectSensor.IsOpen)
            {
                multiSourceFrameReader = kinectSensor.OpenMultiSourceFrameReader((FrameSourceTypes)((int)dwFlags & 0x3F));
            }

            //if (deviceMode == KinectInterop.DepthSensorMode.CreateRecording)
            //{
            //    Debug.LogWarning("Please use Kinect Studio v2.0 to save sensor data recordings.");
            //}

            return sensorData;
        }

        public override void CloseSensor(KinectInterop.SensorData sensorData)
        {
            base.CloseSensor(sensorData);

            if (coordMapper != null)
            {
                coordMapper = null;
            }

            if (colorFrameReader != null)
            {
                colorFrameReader.Dispose();
                colorFrameReader = null;
            }

            if (depthFrameReader != null)
            {
                depthFrameReader.Dispose();
                depthFrameReader = null;
            }

            if (infraredFrameReader != null)
            {
                infraredFrameReader.Dispose();
                infraredFrameReader = null;
            }

            if (bodyFrameReader != null)
            {
                bodyFrameReader.Dispose();
                bodyFrameReader = null;
            }

            if (bodyIndexFrameReader != null)
            {
                bodyIndexFrameReader.Dispose();
                bodyIndexFrameReader = null;
            }

            if (multiSourceFrameReader != null)
            {
                multiSourceFrameReader.Dispose();
                multiSourceFrameReader = null;
            }

            if (kinectSensor != null)
            {
                //if (kinectSensor.IsOpen)
                {
                    //Debug.Log("Closing sensor, available: " + kinectSensor.IsAvailable);
                    kinectSensor.Close();
                }

                float fWaitTime = Time.realtimeSinceStartup + 3f;
                while (kinectSensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
                {
                    // wait for sensor to close
                }

                if (consoleLogMessages)
                    Debug.Log("D" + deviceIndex + " K2-sensor " + (kinectSensor.IsOpen ? "opened" : "closed") + ", available: " + kinectSensor.IsAvailable);

                kinectSensor = null;
            }
        }

        public override void PollSensorFrames(KinectInterop.SensorData sensorData)
        {
            // check for single-source or multi-source frames
            if (multiSourceFrameReader == null)
            {
                // single source - check for color frame
                if (colorFrameReader != null)
                {
                    msColorFrame = colorFrameReader.AcquireLatestFrame();
                }

                // check for depth frame
                if (depthFrameReader != null)
                {
                    msDepthFrame = depthFrameReader.AcquireLatestFrame();
                }

                // check for IR frame
                if (infraredFrameReader != null)
                {
                    msInfraredFrame = infraredFrameReader.AcquireLatestFrame();
                }

                // check for body index frame
                if (bodyIndexFrameReader != null)
                {
                    msBodyIndexFrame = bodyIndexFrameReader.AcquireLatestFrame();
                }

                // check for body frame
                if (bodyFrameReader != null)
                {
                    msBodyFrame = bodyFrameReader.AcquireLatestFrame();
                }

                // process currently read sensor frames
                ProcessSensorFrames(sensorData);
            }
            else
            {
                // multi-source frames
                multiSourceFrame = multiSourceFrameReader.AcquireLatestFrame();

                if (multiSourceFrame != null)
                {
                    // try to get all frames at once
                    msColorFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeColor) != 0 ? multiSourceFrame.ColorFrameReference.AcquireFrame() : null;
                    msDepthFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeDepth) != 0 ? multiSourceFrame.DepthFrameReference.AcquireFrame() : null;
                    msInfraredFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeInfrared) != 0 ? multiSourceFrame.InfraredFrameReference.AcquireFrame() : null;
                    msBodyFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeBody) != 0 ? multiSourceFrame.BodyFrameReference.AcquireFrame() : null;
                    msBodyIndexFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0 ? multiSourceFrame.BodyIndexFrameReference.AcquireFrame() : null;

                    bool bAllSet =
                        ((frameSourceFlags & KinectInterop.FrameSource.TypeColor) == 0 || msColorFrame != null) &&
                        ((frameSourceFlags & KinectInterop.FrameSource.TypeDepth) == 0 || msDepthFrame != null) &&
                        ((frameSourceFlags & KinectInterop.FrameSource.TypeInfrared) == 0 || msInfraredFrame != null);

                    if(isSyncBodyAndDepth)
                    {
                        bAllSet &= ((frameSourceFlags & KinectInterop.FrameSource.TypeBody) == 0 || msBodyFrame != null) &&
                        ((frameSourceFlags & KinectInterop.FrameSource.TypeBodyIndex) == 0 || msBodyIndexFrame != null);
                    }

                    if (bAllSet)
                    {
                        // process currently read sensor frames
                        ProcessSensorFrames(sensorData);
                    }

                    //release all frames
                    if (msColorFrame != null)
                    {
                        msColorFrame.Dispose();
                        msColorFrame = null;
                    }

                    if (msDepthFrame != null)
                    {
                        msDepthFrame.Dispose();
                        msDepthFrame = null;
                    }

                    if (msInfraredFrame != null)
                    {
                        msInfraredFrame.Dispose();
                        msInfraredFrame = null;
                    }

                    if (msBodyFrame != null)
                    {
                        msBodyFrame.Dispose();
                        msBodyFrame = null;
                    }

                    if (msBodyIndexFrame != null)
                    {
                        msBodyIndexFrame.Dispose();
                        msBodyIndexFrame = null;
                    }

                    if (multiSourceFrame != null)
                    {
                        multiSourceFrame = null;
                    }
                }
            }
        }

        // processes the currently read sensor frames
        // todo: provide thread sync
        private void ProcessSensorFrames(KinectInterop.SensorData sensorData)
        {
            // depth frame
            ulong depthFrameTime = msDepthFrame != null ? (ulong)msDepthFrame.RelativeTime.Ticks : 0;

            if (msDepthFrame != null)
            {
                if (getAllSensorFrames || rawDepthTimestamp == sensorData.lastDepthFrameTime)
                {
                    lock (depthFrameLock)
                    {
                        var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
                        msDepthFrame.CopyFrameDataToIntPtr(pDepthData.AddrOfPinnedObject(), (uint)rawDepthImage.Length * sizeof(ushort));
                        pDepthData.Free();

                        rawDepthTimestamp = (ulong)msDepthFrame.RelativeTime.Ticks;
                        //Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
                    }
                }

                msDepthFrame.Dispose();
                msDepthFrame = null;
            }

            // color frame
            if (msColorFrame != null)
            {
                if(getAllSensorFrames || rawColorTimestamp == sensorData.lastColorFrameTime)
                {
                    lock (colorFrameLock)
                    {
                        var pColorData = GCHandle.Alloc(rawColorImage, GCHandleType.Pinned);
                        msColorFrame.CopyConvertedFrameDataToIntPtr(pColorData.AddrOfPinnedObject(), (uint)rawColorImage.Length, ColorImageFormat.Rgba);
                        pColorData.Free();

                        rawColorTimestamp = isSyncDepthAndColor && depthFrameTime != 0 ? depthFrameTime : (ulong)msColorFrame.RelativeTime.Ticks;
                        //Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
                    }
                }

                msColorFrame.Dispose();
                msColorFrame = null;
            }

            // infrared frame
            if (msInfraredFrame != null)
            {
                if(getAllSensorFrames || rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
                {
                    lock (infraredFrameLock)
                    {
                        var pInfraredData = GCHandle.Alloc(rawInfraredImage, GCHandleType.Pinned);
                        msInfraredFrame.CopyFrameDataToIntPtr(pInfraredData.AddrOfPinnedObject(), (uint)rawInfraredImage.Length * sizeof(ushort));
                        pInfraredData.Free();

                        rawInfraredTimestamp = isSyncDepthAndColor && depthFrameTime != 0 ? depthFrameTime : (ulong)msInfraredFrame.RelativeTime.Ticks;
                        //Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
                    }
                }

                msInfraredFrame.Dispose();
                msInfraredFrame = null;
            }

            // body index frame
            //bool bProcessBodyFrame = (getAllSensorFrames || rawBodyTimestamp == sensorData.lastBodyFrameTime);

            if (msBodyIndexFrame != null)
            {
                //if(bProcessBodyFrame)
                if (getAllSensorFrames || rawBodyIndexTimestamp == sensorData.lastBodyIndexFrameTime)
                {
                    lock (bodyTrackerLock)
                    {
                        var pBodyIndexData = GCHandle.Alloc(rawBodyIndexImage, GCHandleType.Pinned);
                        msBodyIndexFrame.CopyFrameDataToIntPtr(pBodyIndexData.AddrOfPinnedObject(), (uint)rawBodyIndexImage.Length);
                        pBodyIndexData.Free();

                        rawBodyIndexTimestamp = isSyncBodyAndDepth && depthFrameTime != 0 ? depthFrameTime : (ulong)msBodyIndexFrame.RelativeTime.Ticks;
                        //Debug.Log("RawBodyIndexTimestamp: " + rawBodyIndexTimestamp);
                    }
                }

                msBodyIndexFrame.Dispose();
                msBodyIndexFrame = null;
            }

            // body frame
            if (msBodyFrame != null)
            {
                //if (bProcessBodyFrame)
                if(getAllSensorFrames || rawBodyTimestamp == sensorData.lastBodyFrameTime)
                {
                    lock (bodyTrackerLock)
                    {
                        ProcessBodyFrame(msBodyFrame, depthFrameTime, sensorData);
                    }

                    if (floorPlaneDetected && ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0))
                    {
                        lock (poseFrameLock)
                        {
                            // update the sensor pose
                            if(vFloorPlane.X != 0f || vFloorPlane.Y != 0f || vFloorPlane.Z != 0f)
                            {
                                Vector3 vFloorNormal = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z);
                                rawPoseRotation = Quaternion.FromToRotation(vFloorNormal, Vector3.up);

                                if (vFloorPlane.W != 0f)
                                {
                                    rawPosePosition = new Vector3(0f, vFloorPlane.W, 0f) - initialPosePosition;
                                }

                                rawPoseTimestamp = rawBodyTimestamp;
                            }
                        }
                    }
                }

                msBodyFrame.Dispose();
                msBodyFrame = null;
            }
        }


        // processes the acquired body frame
        private void ProcessBodyFrame(BodyFrame frame, ulong depthFrameTime, KinectInterop.SensorData sensorData)
        {
            frame.GetAndRefreshBodyData(kinectBodyData);
            ulong bodyFrameTimestamp = (ulong)frame.RelativeTime.Ticks;

            // get the floor plane
            vFloorPlane = frame.FloorClipPlane;
            floorPlaneDetected = true;

            frame.Dispose();
            frame = null;

            // get sensor-to-world matrix
            Matrix4x4 sensorToWorld = GetSensorToWorldMatrix();
            float scaleX = sensorData.depthImageScale.x;
            //float scaleY = sensorData.depthImageScale.y;

            // create the needed slots
            while (alTrackedBodies.Count < kinectBodyCount)
            {
                alTrackedBodies.Add(new KinectInterop.BodyData((int)KinectInterop.JointType.Count));
            }

            trackedBodiesCount = 0;

            for (int i = 0; i < kinectBodyCount; i++)
            {
                Body body = kinectBodyData[i];

                if (body == null)
                    continue;

                KinectInterop.BodyData bodyData = alTrackedBodies[(int)trackedBodiesCount];

                bodyData.liTrackingID = body.TrackingId;
                bodyData.iBodyIndex = i;

                bodyData.bIsTracked = body.IsTracked;
                bodyData.bodyTimestamp = bodyFrameTimestamp;

                if (!bodyData.bIsTracked)
                    continue;

                // cache the body joints (following the advice of Brian Chasalow)
                Dictionary<Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints;

                for (int jKJ = 0; jKJ < kinectJointCount; jKJ++)
                {
                    Windows.Kinect.Joint joint = bodyJoints[(Windows.Kinect.JointType)jKJ];

                    int j = KinectJoint2JointType[jKJ];

                    if (j >= 0)
                    {
                        KinectInterop.JointData jointData = bodyData.joint[j];

                        jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState;

                        float jPosZ = (bIgnoreZCoordinates && j > 0) ? bodyData.joint[0].kinectPos.z : joint.Position.Z;
                        jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
                        jointData.position = sensorToWorld.MultiplyPoint3x4(new Vector3(joint.Position.X * scaleX, joint.Position.Y, jPosZ));

                        jointData.orientation = Quaternion.identity;

                        if (j == 0)
                        {
                            bodyData.kinectPos = jointData.kinectPos;
                            bodyData.position = jointData.position;
                            bodyData.orientation = jointData.orientation;
                            //floorPlaneDetected = true;
                        }

                        bodyData.joint[j] = jointData;
                    }
                }

                bodyJoints.Clear();

                // hand states
                bodyData.leftHandState = (KinectInterop.HandState)body.HandLeftState;
                bodyData.rightHandState = (KinectInterop.HandState)body.HandRightState;
                //bodyData.leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence;
                //bodyData.rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence;

                if (usersFaceBackwards)
                {
                    // switch the left and right joints
                    SwitchLeftRightJoints(ref bodyData);
                }

                // estimate additional joints
                CalcBodySpecialJoints(ref bodyData);

                // filter joint positions
                if (jointPositionFilter != null)
                {
                    jointPositionFilter.UpdateFilter(ref bodyData);
                }

                // calculate bone dirs
                KinectInterop.CalcBodyJointDirs(ref bodyData);

                // calculate joint orientations
                CalcBodyJointOrients(ref bodyData);

                // body orientation
                bodyData.normalRotation = bodyData.joint[0].normalRotation;
                bodyData.mirroredRotation = bodyData.joint[0].mirroredRotation;

                alTrackedBodies[(int)trackedBodiesCount] = bodyData;
                trackedBodiesCount++;

                //Debug.Log("  (T)User ID: " + bodyData.liTrackingID + ", body: " + (trackedBodiesCount - 1) + ", bi: " + bodyData.iBodyIndex + ", pos: " + bodyData.joint[0].kinectPos + ", rot: " + bodyData.joint[0].normalRotation.eulerAngles);
            }

            // clean up user history
            if (jointPositionFilter != null)
            {
                jointPositionFilter.CleanUpUserHistory();
            }

            rawBodyTimestamp = isSyncBodyAndDepth && depthFrameTime != 0 ? depthFrameTime : bodyFrameTimestamp;
            //Debug.Log("RawBodyTimestamp: " + rawBodyTimestamp + ", Time: " + DateTime.Now.ToString("HH.mm.ss.fff"));
        }

        // switch the left and right body joints
        private void SwitchLeftRightJoints(ref KinectInterop.BodyData bodyData)
        {
            //SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ClavicleLeft, (int)KinectInterop.JointType.ClavicleRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ShoulderLeft, (int)KinectInterop.JointType.ShoulderRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ElbowLeft, (int)KinectInterop.JointType.ElbowRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.WristLeft, (int)KinectInterop.JointType.WristRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandLeft, (int)KinectInterop.JointType.HandRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ThumbLeft, (int)KinectInterop.JointType.ThumbRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandtipLeft, (int)KinectInterop.JointType.HandtipRight);

            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HipLeft, (int)KinectInterop.JointType.HipRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.KneeLeft, (int)KinectInterop.JointType.KneeRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.AnkleLeft, (int)KinectInterop.JointType.AnkleRight);
            SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.FootLeft, (int)KinectInterop.JointType.FootRight);

            //SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.EyeLeft, (int)KinectInterop.JointType.EyeRight);
            //SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.EarLeft, (int)KinectInterop.JointType.EarRight);
        }

        // switches the positional data of two joints
        private void SwitchJointsData(ref KinectInterop.BodyData bodyData, int jointL, int jointR)
        {
            KinectInterop.TrackingState trackingStateL = bodyData.joint[jointL].trackingState;
            Vector3 kinectPosL = bodyData.joint[jointL].kinectPos;
            Vector3 positionL = bodyData.joint[jointL].position;

            KinectInterop.TrackingState trackingStateR = bodyData.joint[jointR].trackingState;
            Vector3 kinectPosR = bodyData.joint[jointR].kinectPos;
            Vector3 positionR = bodyData.joint[jointR].position;

            bodyData.joint[jointL].trackingState = trackingStateR;
            bodyData.joint[jointL].kinectPos = kinectPosR;
            bodyData.joint[jointL].position = positionR;

            bodyData.joint[jointR].trackingState = trackingStateL;
            bodyData.joint[jointR].kinectPos = kinectPosL;
            bodyData.joint[jointR].position = positionL;
        }

        // estimates additional joints for the given body
        protected override void CalcBodySpecialJoints(ref KinectInterop.BodyData bodyData)
        {
            // clavicle right
            {
                int l = (int)KinectInterop.JointType.ClavicleLeft;
                int r = (int)KinectInterop.JointType.ClavicleRight;

                KinectInterop.JointData jointData = bodyData.joint[r];
                jointData.trackingState = bodyData.joint[l].trackingState;
                jointData.orientation = bodyData.joint[l].orientation;

                jointData.kinectPos = bodyData.joint[l].kinectPos;
                jointData.position = bodyData.joint[l].position;

                bodyData.joint[r] = jointData;
            }

            // spine naval
            {
                int p = (int)KinectInterop.JointType.Pelvis;
                int sc = (int)KinectInterop.JointType.SpineChest;
                int sn = (int)KinectInterop.JointType.SpineNaval;

                KinectInterop.JointData jointData = bodyData.joint[sn];
                jointData.trackingState = bodyData.joint[sc].trackingState;
                jointData.orientation = bodyData.joint[sc].orientation;

                Vector3 posChest = bodyData.joint[sc].kinectPos;
                Vector3 posPelvis = bodyData.joint[p].kinectPos;
                jointData.kinectPos = (posPelvis + posChest) * 0.5f;

                posChest = bodyData.joint[sc].position;
                posPelvis = bodyData.joint[p].position;
                jointData.position = (posPelvis + posChest) * 0.5f;

                bodyData.joint[sn] = jointData;
            }

            // ankle left
            int knee = (int)KinectInterop.JointType.KneeLeft;
            int ank = (int)KinectInterop.JointType.AnkleLeft;
            int foot = (int)KinectInterop.JointType.FootLeft;

            if (bodyData.joint[knee].trackingState != KinectInterop.TrackingState.NotTracked &&
               bodyData.joint[ank].trackingState != KinectInterop.TrackingState.NotTracked &&
               bodyData.joint[foot].trackingState != KinectInterop.TrackingState.NotTracked)
            {
                Vector3 vAnkDir = bodyData.joint[ank].kinectPos - bodyData.joint[knee].kinectPos;
                Vector3 vFootDir = bodyData.joint[foot].kinectPos - bodyData.joint[ank].kinectPos;

                Vector3 vFootProj = Vector3.Project(vFootDir, vAnkDir);
                bodyData.joint[ank].kinectPos += vFootProj;

                vAnkDir = bodyData.joint[ank].position - bodyData.joint[knee].position;
                vFootDir = bodyData.joint[foot].position - bodyData.joint[ank].position;

                vFootProj = Vector3.Project(vFootDir, vAnkDir);
                bodyData.joint[ank].position += vFootProj;
            }

            // ankle right
            knee = (int)KinectInterop.JointType.KneeRight;
            ank = (int)KinectInterop.JointType.AnkleRight;
            foot = (int)KinectInterop.JointType.FootRight;

            if (bodyData.joint[knee].trackingState != KinectInterop.TrackingState.NotTracked &&
               bodyData.joint[ank].trackingState != KinectInterop.TrackingState.NotTracked &&
               bodyData.joint[foot].trackingState != KinectInterop.TrackingState.NotTracked)
            {
                Vector3 vAnkDir = bodyData.joint[ank].kinectPos - bodyData.joint[knee].kinectPos;
                Vector3 vFootDir = bodyData.joint[foot].kinectPos - bodyData.joint[ank].kinectPos;

                Vector3 vFootProj = Vector3.Project(vFootDir, vAnkDir);
                bodyData.joint[ank].kinectPos += vFootProj;

                vAnkDir = bodyData.joint[ank].position - bodyData.joint[knee].position;
                vFootDir = bodyData.joint[foot].position - bodyData.joint[ank].position;

                vFootProj = Vector3.Project(vFootDir, vAnkDir);
                bodyData.joint[ank].position += vFootProj;
            }
        }


        protected static readonly int[] KinectJoint2JointType =
        {
            (int)KinectInterop.JointType.Pelvis,
            (int)KinectInterop.JointType.SpineChest,
            (int)KinectInterop.JointType.Neck,
            (int)KinectInterop.JointType.Head,

            (int)KinectInterop.JointType.ShoulderLeft,
            (int)KinectInterop.JointType.ElbowLeft,
            (int)KinectInterop.JointType.WristLeft,
            (int)KinectInterop.JointType.HandLeft,

            (int)KinectInterop.JointType.ShoulderRight,
            (int)KinectInterop.JointType.ElbowRight,
            (int)KinectInterop.JointType.WristRight,
            (int)KinectInterop.JointType.HandRight,

            (int)KinectInterop.JointType.HipLeft,
            (int)KinectInterop.JointType.KneeLeft,
            (int)KinectInterop.JointType.AnkleLeft,
            (int)KinectInterop.JointType.FootLeft,

            (int)KinectInterop.JointType.HipRight,
            (int)KinectInterop.JointType.KneeRight,
            (int)KinectInterop.JointType.AnkleRight,
            (int)KinectInterop.JointType.FootRight,

            (int)KinectInterop.JointType.ClavicleLeft,

            (int)KinectInterop.JointType.HandtipLeft,
            (int)KinectInterop.JointType.ThumbLeft,
            (int)KinectInterop.JointType.HandtipRight,
            (int)KinectInterop.JointType.ThumbRight
        };


        public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
        {
            base.UpdateSensorData(sensorData, kinectManager, isPlayMode);

            if (sensorData.depthCamIntr == null && coordMapper != null)
            {
                lock (depthFrameLock)
                {
                    // get depth camera intrinsics
                    CameraIntrinsics depthCamIntr = coordMapper.GetDepthCameraIntrinsics();

                    if (depthCamIntr.PrincipalPointX != 0f && depthCamIntr.PrincipalPointY != 0f)
                    {
                        GetDepthCameraIntrinsics(depthCamIntr, ref sensorData.depthCamIntr, sensorData.depthImageWidth, sensorData.depthImageHeight);
                    }
                }
            }

            if (sensorData.colorCamIntr == null && coordMapper != null)
            {
                lock (colorFrameLock)
                {
                    GetColorCameraIntrinsics(ref sensorData.colorCamIntr, sensorData.colorImageWidth, sensorData.colorImageHeight);
                }
            }

            if((sensorData.depth2ColorExtr == null || sensorData.color2DepthExtr == null) && 
                sensorData.depthCamIntr != null && sensorData.colorCamIntr != null)
            {
                GetDepth2ColorCameraExtrinsics(ref sensorData.depth2ColorExtr);
                GetColor2DepthCameraExtrinsics(ref sensorData.color2DepthExtr);
            }

            return true;
        }


        // gets the depth camera intrinsics
        private void GetDepthCameraIntrinsics(CameraIntrinsics camIntr, ref KinectInterop.CameraIntrinsics intr, int camWidth, int camHeight)
        {
            intr = new KinectInterop.CameraIntrinsics();

            intr.cameraType = 0;
            intr.width = camWidth;
            intr.height = camHeight;

            intr.ppx = camIntr.PrincipalPointX;
            intr.ppy = camHeight - camIntr.PrincipalPointY;

            intr.fx = camIntr.FocalLengthX;
            intr.fy = camIntr.FocalLengthY;

            intr.distCoeffs = new float[3];
            intr.distCoeffs[0] = camIntr.RadialDistortionSecondOrder;
            intr.distCoeffs[1] = camIntr.RadialDistortionFourthOrder;
            intr.distCoeffs[2] = camIntr.RadialDistortionSixthOrder;

            intr.distType = KinectInterop.DistortionType.BrownConrady;

            EstimateFOV(intr);
        }


        // gets the color camera intrinsics
        private void GetColorCameraIntrinsics(ref KinectInterop.CameraIntrinsics intr, int camWidth, int camHeight)
        {
            intr = new KinectInterop.CameraIntrinsics();

            intr.cameraType = 1;
            intr.width = camWidth;
            intr.height = camHeight;

            intr.ppx = 946.0374f;
            intr.ppy = camHeight - 537.392f;

            intr.fx = 1065.267f;
            intr.fy = 1065.409f;

            intr.distCoeffs = new float[3];
            intr.distCoeffs[0] = 0.014655f;
            intr.distCoeffs[1] = -0.000476f;
            intr.distCoeffs[2] = 0f;

            intr.distType = KinectInterop.DistortionType.BrownConrady;

            EstimateFOV(intr);
        }


        // gets the depth to color camera extrinsics
        private void GetDepth2ColorCameraExtrinsics(ref KinectInterop.CameraExtrinsics extr)
        {
            extr = new KinectInterop.CameraExtrinsics();

            extr.rotation = new float[9];

            extr.translation = new float[3];
        }


        // gets the color to depth camera extrinsics
        private void GetColor2DepthCameraExtrinsics(ref KinectInterop.CameraExtrinsics extr)
        {
            extr = new KinectInterop.CameraExtrinsics();

            extr.rotation = new float[9];

            extr.translation = new float[3];
        }


        // unprojects plane point into the space
        public override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
        {
            float x = (pixel.x - intr.ppx) / intr.fx;
            float y = (intr.ppy - pixel.y) / intr.fy;

            // undistort
            float r2 = x * x + y * y;
            float f = 1 - intr.distCoeffs[0] * r2 - intr.distCoeffs[1] * r2 * r2 - intr.distCoeffs[2] * r2 * r2 * r2;

            x = x * f * depth;
            y = y * f * depth;

            return new Vector3(x, y, depth);
        }


        // projects space point onto a plane
        public override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
        {
            float x = point.x / point.z;
            float y = point.y / point.z;

            // distort
            float r2 = x * x + y * y;
            float f = 1f + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[2] * r2 * r2 * r2;

            x = x * f * intr.fx + intr.ppx;
            y = intr.ppy - y * f * intr.fy;

            return new Vector2(x, y);
        }


        // transforms a point from one space to another
        public override Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point)
        {
            float toPointX = extr.rotation[0] * point.x + extr.rotation[3] * point.y + extr.rotation[6] * point.z + extr.translation[0];
            float toPointY = extr.rotation[1] * point.x + extr.rotation[4] * point.y + extr.rotation[7] * point.z + extr.translation[1];
            float toPointZ = extr.rotation[2] * point.x + extr.rotation[5] * point.y + extr.rotation[8] * point.z + extr.translation[2];

            return new Vector3(toPointX, toPointY, toPointZ);
        }


        public override void PollCoordTransformFrames(KinectInterop.SensorData sensorData)
        {
            if (lastDepthCoordFrameTime != rawDepthTimestamp)
            {
                lastDepthCoordFrameTime = rawDepthTimestamp;

                //// depth2space frame
                //if (depth2SpaceCoordFrame != null)
                //{
                //    lock (depth2SpaceFrameLock)
                //    {
                //        MapDepthFrameToSpaceCoords(sensorData, ref depth2SpaceCoordFrame);
                //        lastDepth2SpaceFrameTime = lastDepthCoordFrameTime;
                //    }
                //}

                // depth2color frame
                if (depthCamColorCoordFrame != null && rawDepthImage != null)
                {
                    lock (depthCamColorFrameLock)
                    {
                        var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
                        var pColorCoordsData = GCHandle.Alloc(depthCamColorCoordFrame, GCHandleType.Pinned);

                        coordMapper.MapDepthFrameToColorSpaceUsingIntPtr(
                            pDepthData.AddrOfPinnedObject(),
                            rawDepthImage.Length * sizeof(ushort),
                            pColorCoordsData.AddrOfPinnedObject(),
                            (uint)depthCamColorCoordFrame.Length);

                        pColorCoordsData.Free();
                        pDepthData.Free();

                        //int di = (sensorData.depthImageHeight / 2) * sensorData.depthImageWidth + (sensorData.depthImageWidth / 2);
                        //Debug.Log("d2cCoordData: " + depth2ColorCoordFrame[di]);

                        lastDepthCamColorFrameTime = lastDepthCoordFrameTime;
                        //Debug.Log("DepthCamColorFrameTime: " + lastDepthCamColorFrameTime);
                    }
                }

                // color2depth frame
                if (colorCamDepthCoordFrame != null)
                {
                    lock (colorCamDepthFrameLock)
                    {
                        var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
                        var pDepthCoordsData = GCHandle.Alloc(colorCamDepthCoordFrame, GCHandleType.Pinned);

                        coordMapper.MapColorFrameToDepthSpaceUsingIntPtr(
                            pDepthData.AddrOfPinnedObject(),
                            (uint)rawDepthImage.Length * sizeof(ushort),
                            pDepthCoordsData.AddrOfPinnedObject(),
                            (uint)colorCamDepthCoordFrame.Length);

                        //int ci = (sensorData.colorImageHeight / 2) * sensorData.colorImageWidth + (sensorData.colorImageWidth / 2);
                        //Debug.Log("c2dCoordData: " + color2DepthCoordFrame[ci]);

                        pDepthCoordsData.Free();
                        pDepthData.Free();

                        lastColorCamDepthFrameTime = lastDepthCoordFrameTime;
                        if (pointCloudInfraredBuffer != null)
                            lastColorCamInfraredFrameTime = lastDepthCoordFrameTime;

                        //Debug.Log("ColorCamDepthFrameTime: " + lastColorCamDepthFrameTime);
                    }
                }
            }
        }


        //public override bool UpdateSensorData(KinectInterop.SensorData sensorData)
        //{
        //    base.UpdateSensorData(sensorData);
        //    return true;
        //}


        // creates the point-cloud vertex shader and its respective buffers, as needed
        protected override bool CreatePointCloudVertexShader(KinectInterop.SensorData sensorData)
        {
            if (pointCloudResolution != PointCloudResolution.ColorCameraResolution)
            {
                return base.CreatePointCloudVertexShader(sensorData);
            }

            // for K2 color camera resolution only
            pointCloudVertexRes = GetPointCloudTexResolution(sensorData);
            pointCloudPlayerIndices = GetPointCloudPlayerIndices();

            if (pointCloudVertexRT == null)
            {
                pointCloudVertexRT = new RenderTexture(pointCloudVertexRes.x, pointCloudVertexRes.y, 0, RenderTextureFormat.ARGBHalf);
                pointCloudVertexRT.enableRandomWrite = true;
                pointCloudVertexRT.Create();
            }

            if (pointCloudVertexShader == null)
            {
                if(pointCloudPlayerIndices == null)
                    pointCloudVertexShader = Resources.Load("PointCloudVertexShaderCRK2") as ComputeShader;
                else
                    pointCloudVertexShader = Resources.Load("PointCloudBodyVertexShaderCRK2") as ComputeShader;

                pointCloudVertexKernel = pointCloudVertexShader != null ? pointCloudVertexShader.FindKernel("BakeVertexTexColorResK2") : -1;
            }

            if (pointCloudSpaceBuffer == null)
            {
                int spaceBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 3;
                pointCloudSpaceBuffer = new ComputeBuffer(spaceBufferLength, sizeof(float));

                // depth2space table
                //int depthImageLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
                //Vector3[] depth2SpaceTable = new Vector3[depthImageLength];

                //for (int dy = 0, di = 0; dy < sensorData.depthImageHeight; dy++)
                //{
                //    for (int dx = 0; dx < sensorData.depthImageWidth; dx++, di++)
                //    {
                //        Vector2 depthPos = new Vector2(dx, dy);
                //        depth2SpaceTable[di] = MapDepthPointToSpaceCoords(sensorData, depthPos, 1000);
                //    }
                //}

                Vector3[] depth2SpaceTable = GetDepthCameraSpaceTable(sensorData);
                pointCloudSpaceBuffer.SetData(depth2SpaceTable);
                depth2SpaceTable = null;
            }

            if (pointCloudDepthBuffer == null)
            {
                int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
                pointCloudDepthBuffer = new ComputeBuffer(depthBufferLength, sizeof(uint));
            }

            if (pointCloudCoordBuffer == null)
            {
                int coordBufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;  // Vector2 = 2 x float
                pointCloudCoordBuffer = new ComputeBuffer(coordBufferLength, sizeof(float));
            }

            if (colorCamDepthCoordFrame == null)
            {
                colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
            }

            if (pointCloudPlayerIndices != null)
            {
                int biBufferLength = pointCloudVertexRes.x * pointCloudVertexRes.y / 4;
                pointCloudBodyIndexBuffer = new ComputeBuffer(biBufferLength, sizeof(uint));
            }

            return true;
        }


        // updates the point-cloud vertex shader with the actual data
        protected override bool UpdatePointCloudVertexShader(KinectInterop.SensorData sensorData, KinectManager kinectManager)
        {
            if (pointCloudResolution != PointCloudResolution.ColorCameraResolution)
            {
                return base.UpdatePointCloudVertexShader(sensorData, kinectManager);
            }

            // for K2 color camera resolution only
            if (pointCloudVertexShader != null && sensorData.depthImage != null && pointCloudVertexRT != null &&
                sensorData.lastDepth2SpaceFrameTime != sensorData.lastDepthFrameTime)
            {
                sensorData.lastDepth2SpaceFrameTime = sensorData.lastDepthFrameTime;

                KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, sensorData.depthImage, sensorData.depthImage.Length >> 1, sizeof(uint));

                lock (colorCamDepthFrameLock)
                {
                    int coordBufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
                    KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, coordBufferLength, sizeof(float));
                }

                if (GetPointCloudBodyIndices(sensorData, kinectManager, pointCloudPlayerIndices, ref pointCloudBodyIndices))
                {
                    // body index
                    if (pointCloudBodyIndexBuffer != null && sensorData.bodyIndexImage != null)
                    {
                        KinectInterop.SetComputeBufferData(pointCloudBodyIndexBuffer, sensorData.bodyIndexImage, sensorData.bodyIndexImage.Length >> 2, sizeof(uint));
                        pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "BodyIndexMap", pointCloudBodyIndexBuffer);
                    }

                    // body params
                    int uCount = pointCloudBodyIndices.Length;
                    pointCloudVertexShader.SetInt("BodyIndexAll", uCount == 1 && pointCloudPlayerIndices[0] == -1 ? 1 : 0);

                    pointCloudVertexShader.SetInt("BodyIndex0", uCount > 0 ? pointCloudBodyIndices[0] : -1);
                    pointCloudVertexShader.SetInt("BodyIndex1", uCount > 1 ? pointCloudBodyIndices[1] : -1);
                    pointCloudVertexShader.SetInt("BodyIndex2", uCount > 2 ? pointCloudBodyIndices[2] : -1);
                    pointCloudVertexShader.SetInt("BodyIndex3", uCount > 3 ? pointCloudBodyIndices[3] : -1);
                    pointCloudVertexShader.SetInt("BodyIndex4", uCount > 4 ? pointCloudBodyIndices[4] : -1);

                    //Debug.Log("All: " + (pointCloudPlayerIndices[0] == -1 ? 1 : 0) + ", Ind: " +
                    //    (uCount > 0 ? pointCloudBodyIndices[0] : -1) + " " +
                    //    (uCount > 1 ? pointCloudBodyIndices[1] : -1) + " " +
                    //    (uCount > 2 ? pointCloudBodyIndices[2] : -1) + " " +
                    //    (uCount > 3 ? pointCloudBodyIndices[3] : -1) + " " +
                    //    (uCount > 4 ? pointCloudBodyIndices[4] : -1));
                }

                KinectInterop.SetComputeShaderInt2(pointCloudVertexShader, "PointCloudRes", pointCloudVertexRes.x, pointCloudVertexRes.y);
                KinectInterop.SetComputeShaderInt2(pointCloudVertexShader, "DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
                KinectInterop.SetComputeShaderFloat2(pointCloudVertexShader, "SpaceScale", sensorData.sensorSpaceScale.x, sensorData.sensorSpaceScale.y);
                pointCloudVertexShader.SetInt("MinDepth", (int)(minDepthDistance * 1000f));
                pointCloudVertexShader.SetInt("MaxDepth", (int)(maxDepthDistance * 1000f));
                pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "SpaceTable", pointCloudSpaceBuffer);
                pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "DepthMap", pointCloudDepthBuffer);
                pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "ColorToDepthMap", pointCloudCoordBuffer);
                pointCloudVertexShader.SetTexture(pointCloudVertexKernel, "PointCloudVertexTex", pointCloudVertexRT);
                pointCloudVertexShader.Dispatch(pointCloudVertexKernel, pointCloudVertexRes.x / 8, pointCloudVertexRes.y / 8, 1);

                if (pointCloudVertexTexture != null)
                {
                    Graphics.Blit(pointCloudVertexRT, pointCloudVertexTexture);
                }

                return true;
            }

            return false;
        }


        // creates the point-cloud color shader and its respective buffers, as needed
        protected override bool CreatePointCloudColorShader(KinectInterop.SensorData sensorData)
        {
            if (pointCloudResolution != PointCloudResolution.DepthCameraResolution)
            {
                return base.CreatePointCloudColorShader(sensorData);
            }

            // for K2 depth camera resolution only
            if (pointCloudColorRT == null)
            {
                pointCloudColorRT = new RenderTexture(sensorData.depthImageWidth, sensorData.depthImageHeight, 0, RenderTextureFormat.ARGB32);
                pointCloudColorRT.enableRandomWrite = true;
                pointCloudColorRT.Create();
            }

            if (pointCloudColorShader == null)
            {
                pointCloudColorShader = Resources.Load("PointCloudColorShaderK2") as ComputeShader;
                pointCloudColorKernel = pointCloudColorShader != null ? pointCloudColorShader.FindKernel("BakeColorTex") : -1;
            }

            if (pointCloudCoordBuffer == null)
            {
                int coordBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 2;  // Vector2 = 2 x float
                pointCloudCoordBuffer = new ComputeBuffer(coordBufferLength, sizeof(float));
            }

            if (depthCamColorCoordFrame == null)
            {
                depthCamColorCoordFrame = new Vector2[sensorData.depthImageWidth * sensorData.depthImageHeight];
            }

            return true;
        }


        // updates the point-cloud color shader with the actual data
        protected override bool UpdatePointCloudColorShader(KinectInterop.SensorData sensorData)
        {
            if (pointCloudResolution != PointCloudResolution.DepthCameraResolution)
            {
                return base.UpdatePointCloudColorShader(sensorData);
            }

            // for K2 depth camera resolution only
            if (pointCloudColorShader != null && pointCloudCoordBuffer != null && sensorData.colorImageTexture != null && pointCloudColorRT != null &&
                sensorData.lastDepthCamColorFrameTime != lastDepthCamColorFrameTime)
            {
                sensorData.lastDepthCamColorFrameTime = lastDepthCamColorFrameTime;

                lock (depthCamColorFrameLock)
                {
                    int coordBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 2;
                    KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, depthCamColorCoordFrame, coordBufferLength, sizeof(float));
                }

                KinectInterop.SetComputeShaderInt2(pointCloudColorShader, "DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
                pointCloudColorShader.SetBuffer(pointCloudColorKernel, "DepthToColorMap", pointCloudCoordBuffer);
                pointCloudColorShader.SetTexture(pointCloudColorKernel, "ColorTex", sensorData.colorImageTexture);
                pointCloudColorShader.SetTexture(pointCloudColorKernel, "PointCloudColorTex", pointCloudColorRT);
                pointCloudColorShader.Dispatch(pointCloudColorKernel, sensorData.depthImageWidth / 8, sensorData.depthImageHeight / 8, 1);

                if(pointCloudColorTexture != null)
                {
                    Graphics.Blit(pointCloudColorRT, pointCloudColorTexture);
                }

                if (sensorData.depthCamColorImageTexture != null)
                {
                    Texture2D depth2colorTex2d  = (Texture2D)sensorData.depthCamColorImageTexture;
                    KinectInterop.RenderTex2Tex2D(pointCloudColorTexture, ref depth2colorTex2d);
                }

                return true;
            }

            return false;
        }


        // creates the color-depth shader and its respective buffers, as needed
        protected override bool CreateColorDepthShader(KinectInterop.SensorData sensorData)
        {
            if (colorDepthShader == null)
            {
                colorDepthShader = Resources.Load("ColorDepthShaderK2") as ComputeShader;
                colorDepthKernel = colorDepthShader != null ? colorDepthShader.FindKernel("BakeColorDepth") : -1;
            }

            if (pointCloudDepthBuffer == null)
            {
                int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
                pointCloudDepthBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
            }

            if (pointCloudCoordBuffer == null)
            {
                int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;  // Vector2 = 2 x float
                pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
            }

            if (colorCamDepthCoordFrame == null)
            {
                colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
            }

            if (sensorData.colorDepthTexture == null)
            {
                sensorData.colorDepthTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
                //sensorData.colorDepthTexture.enableRandomWrite = true;
                sensorData.colorDepthTexture.Create();
            }

            colorDepthShaderInited = true;

            return true;
        }


        // updates the color-depth shader with the actual data
        protected override bool UpdateColorDepthShader(KinectInterop.SensorData sensorData)
        {
            // for K2 depth camera resolution only
            if (colorDepthShader != null && pointCloudDepthBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
            {
                if (/**sensorData.usedColorDepthBufferTime == sensorData.lastColorDepthBufferTime && */
                    sensorData.lastColorDepthBufferTime != lastColorCamDepthFrameTime)
                {
                    sensorData.lastColorDepthBufferTime = lastColorCamDepthFrameTime;

                    if (sensorData.colorImageTexture != null)
                    {
                        Graphics.Blit(sensorData.colorImageTexture, sensorData.colorDepthTexture);
                    }

                    KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, sensorData.depthImage, sensorData.depthImage.Length >> 1, sizeof(uint));

                    lock (colorCamDepthFrameLock)
                    {
                        int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
                        KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
                    }

                    if(sensorData.colorDepthBuffer != null)
                    {
                        KinectInterop.SetComputeShaderInt2(colorDepthShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
                        KinectInterop.SetComputeShaderInt2(colorDepthShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);

                        colorDepthShader.SetBuffer(colorDepthKernel, "_DepthMap", pointCloudDepthBuffer);
                        colorDepthShader.SetBuffer(colorDepthKernel, "_Color2DepthMap", pointCloudCoordBuffer);
                        //colorDepthShader.SetTexture(colorDepthKernel, "_ColorTex", sensorData.colorImageTexture);
                        colorDepthShader.SetBuffer(colorDepthKernel, "_ColorDepthMap", sensorData.colorDepthBuffer);
                        colorDepthShader.Dispatch(colorDepthKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);

                        if (sensorData.colorCamDepthImage != null)
                        {
                            sensorData.colorDepthBuffer.GetData(sensorData.colorCamDepthImage);
                            sensorData.lastColorCamDepthFrameTime = lastColorCamDepthFrameTime;
                        }
                    }
                }

                return true;
            }

            return false;
        }


        // creates the color-infrared shader and its respective buffers, as needed
        protected override bool CreateColorInfraredShader(KinectInterop.SensorData sensorData)
        {
            if (colorInfraredShader == null)
            {
                colorInfraredShader = Resources.Load("ColorInfraredShaderK2") as ComputeShader;
                colorInfraredKernel = colorInfraredShader != null ? colorInfraredShader.FindKernel("BakeColorInfrared") : -1;
            }

            if (pointCloudInfraredBuffer == null)
            {
                int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
                pointCloudInfraredBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
            }

            if (pointCloudCoordBuffer == null)
            {
                int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;  // Vector2 = 2 x float
                pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
            }

            if (colorCamDepthCoordFrame == null)
            {
                colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
            }

            if (sensorData.colorInfraredTexture == null)
            {
                sensorData.colorInfraredTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
                //sensorData.colorInfraredTexture.enableRandomWrite = true;
                sensorData.colorInfraredTexture.Create();
            }

            Shader infraredImageShader = Shader.Find("Kinect/InfraredImageShader");
            if (infraredImageShader)
            {
                sensorData.colorInfraredMaterial = new Material(infraredImageShader);
            }

            colorInfraredShaderInited = true;

            return true;
        }


        // updates the color-infrared shader with the actual data
        protected override bool UpdateColorInfraredShader(KinectInterop.SensorData sensorData)
        {
            // for K2 depth camera resolution only
            if (colorInfraredShader != null && pointCloudInfraredBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
            {
                if (/**sensorData.usedColorInfraredBufferTime == sensorData.lastColorInfraredBufferTime && */
                    sensorData.lastColorInfraredBufferTime != lastColorCamInfraredFrameTime)
                {
                    sensorData.lastColorInfraredBufferTime = lastColorCamInfraredFrameTime;
                    //Debug.Log("LastColorCamInfraredFrameTime: " + lastColorCamInfraredFrameTime);

                    //if (sensorData.colorImageTexture != null)
                    //{
                    //    Graphics.Blit(sensorData.colorImageTexture, sensorData.colorInfraredTexture);
                    //}

                    KinectInterop.SetComputeBufferData(pointCloudInfraredBuffer, sensorData.infraredImage, sensorData.infraredImage.Length >> 1, sizeof(uint));

                    lock (colorCamDepthFrameLock)
                    {
                        int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
                        KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
                    }

                    if (sensorData.colorInfraredBuffer != null)
                    {
                        KinectInterop.SetComputeShaderInt2(colorInfraredShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
                        KinectInterop.SetComputeShaderInt2(colorInfraredShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);

                        colorInfraredShader.SetBuffer(colorInfraredKernel, "_InfraredMap", pointCloudInfraredBuffer);
                        colorInfraredShader.SetBuffer(colorInfraredKernel, "_Color2DepthMap", pointCloudCoordBuffer);
                        //colorInfraredShader.SetTexture(colorInfraredKernel, "_ColorTex", sensorData.colorImageTexture);
                        colorInfraredShader.SetBuffer(colorInfraredKernel, "_ColorInfraredMap", sensorData.colorInfraredBuffer);
                        colorInfraredShader.Dispatch(colorInfraredKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);

                        if (sensorData.colorCamInfraredImage != null)
                        {
                            sensorData.colorInfraredBuffer.GetData(sensorData.colorCamInfraredImage);
                            sensorData.lastColorCamInfraredFrameTime = lastColorCamInfraredFrameTime;
                        }
                    }
                }

                return true;
            }

            return false;
        }


        // creates the color-depth shader and its respective buffers, as needed
        protected override bool CreateColorBodyIndexShader(KinectInterop.SensorData sensorData)
        {
            if (colorBodyIndexShader == null)
            {
                colorBodyIndexShader = Resources.Load("ColorBodyIndexShaderK2") as ComputeShader;
                colorBodyIndexKernel = colorBodyIndexShader != null ? colorBodyIndexShader.FindKernel("BakeColorBodyIndex") : -1;
            }

            if (bodyIndexBuffer == null)
            {
                int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 4;
                bodyIndexBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
            }

            if (pointCloudCoordBuffer == null)
            {
                int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;  // Vector2 = 2 x float
                pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
            }

            if (colorCamDepthCoordFrame == null)
            {
                colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
            }

            if (sensorData.colorBodyIndexTexture == null)
            {
                sensorData.colorBodyIndexTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
                //sensorData.colorBodyIndexTexture.enableRandomWrite = true;
                sensorData.colorBodyIndexTexture.Create();
            }

            colorBodyIndexShaderInited = true;

            return true;
        }


        // updates the color body index shader with the actual data
        protected override bool UpdateColorBodyIndexShader(KinectInterop.SensorData sensorData)
        {
            // for K2 depth camera resolution only
            if (colorBodyIndexShader != null && bodyIndexBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
            {
                if (/**sensorData.usedColorBodyIndexBufferTime == sensorData.lastColorBodyIndexBufferTime && */
                    sensorData.bodyIndexImage != null && sensorData.lastColorBodyIndexBufferTime != lastColorCamDepthFrameTime)
                {
                    sensorData.lastColorBodyIndexBufferTime = lastColorCamDepthFrameTime;

                    if (sensorData.colorImageTexture != null)
                    {
                        Graphics.Blit(sensorData.colorImageTexture, sensorData.colorBodyIndexTexture);
                    }

                    KinectInterop.SetComputeBufferData(bodyIndexBuffer, sensorData.bodyIndexImage, sensorData.bodyIndexImage.Length >> 2, sizeof(uint));

                    lock (colorCamDepthFrameLock)
                    {
                        int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
                        KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
                    }

                    if(sensorData.colorBodyIndexBuffer != null)
                    {
                        KinectInterop.SetComputeShaderInt2(colorBodyIndexShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
                        KinectInterop.SetComputeShaderInt2(colorBodyIndexShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);

                        colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_BodyIndexMap", bodyIndexBuffer);
                        colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_Color2DepthMap", pointCloudCoordBuffer);
                        //colorBodyIndexShader.SetTexture(colorBodyIndexKernel, "_ColorTex", sensorData.colorImageTexture);
                        colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_ColorBodyIndexMap", sensorData.colorBodyIndexBuffer);
                        colorBodyIndexShader.Dispatch(colorBodyIndexKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);

                        if (sensorData.colorCamBodyIndexImage != null)
                        {
                            sensorData.colorBodyIndexBuffer.GetData(sensorData.colorCamBodyIndexImage);
                            sensorData.lastColorCamBodyIndexFrameTime = lastColorCamDepthFrameTime;
                        }
                    }
                }

                return true;
            }

            return false;
        }


        public override Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
        {
            Vector3 vPoint = Vector3.zero;

            if (coordMapper != null && depthPos != Vector2.zero)
            {
                DepthSpacePoint depthPoint = new DepthSpacePoint();
                depthPoint.X = depthPos.x;
                depthPoint.Y = depthPos.y;

                DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
                depthPoints[0] = depthPoint;

                ushort[] depthVals = new ushort[1];
                depthVals[0] = depthVal;

                CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
                coordMapper.MapDepthPointsToCameraSpace(depthPoints, depthVals, camPoints);

                CameraSpacePoint camPoint = camPoints[0];
                vPoint.x = camPoint.X;
                vPoint.y = camPoint.Y;
                vPoint.z = camPoint.Z;
            }

            return vPoint;
        }

        public override Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
        {
            Vector2 vPoint = Vector2.zero;

            if (coordMapper != null)
            {
                CameraSpacePoint camPoint = new CameraSpacePoint();
                camPoint.X = spacePos.x;
                camPoint.Y = spacePos.y;
                camPoint.Z = spacePos.z;

                CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
                camPoints[0] = camPoint;

                DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
                coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints);

                DepthSpacePoint depthPoint = depthPoints[0];

                if (depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth &&
                   depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight)
                {
                    vPoint.x = depthPoint.X;
                    vPoint.y = depthPoint.Y;
                }
            }

            return vPoint;
        }

        public override Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
        {
            Vector2 vPoint = Vector2.zero;

            if (coordMapper != null && depthPos != Vector2.zero)
            {
                DepthSpacePoint depthPoint = new DepthSpacePoint();
                depthPoint.X = depthPos.x;
                depthPoint.Y = depthPos.y;

                DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
                depthPoints[0] = depthPoint;

                ushort[] depthVals = new ushort[1];
                depthVals[0] = depthVal;

                ColorSpacePoint[] colPoints = new ColorSpacePoint[1];
                coordMapper.MapDepthPointsToColorSpace(depthPoints, depthVals, colPoints);

                ColorSpacePoint colPoint = colPoints[0];
                vPoint.x = colPoint.X;
                vPoint.y = colPoint.Y;
            }

            return vPoint;
        }

        //public override bool MapDepthFrameToSpaceCoords(KinectInterop.SensorData sensorData, ref Vector3[] vSpaceCoords)
        //{
        //    if (coordMapper != null && sensorData.depthImage != null)
        //    {
        //        var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
        //        var pSpaceCoordsData = GCHandle.Alloc(vSpaceCoords, GCHandleType.Pinned);

        //        coordMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
        //            pDepthData.AddrOfPinnedObject(),
        //            sensorData.depthImage.Length * sizeof(ushort),
        //            pSpaceCoordsData.AddrOfPinnedObject(),
        //            (uint)vSpaceCoords.Length);

        //        pSpaceCoordsData.Free();
        //        pDepthData.Free();

        //        return true;
        //    }

        //    return false;
        //}

        //public override bool MapDepthFrameToColorCoords(KinectInterop.SensorData sensorData, ref Vector2[] vColorCoords)
        //{
        //    if (coordMapper != null && sensorData.colorImageTexture != null && sensorData.depthImage != null)
        //    {
        //        var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
        //        var pColorCoordsData = GCHandle.Alloc(vColorCoords, GCHandleType.Pinned);

        //        coordMapper.MapDepthFrameToColorSpaceUsingIntPtr(
        //            pDepthData.AddrOfPinnedObject(),
        //            sensorData.depthImage.Length * sizeof(ushort),
        //            pColorCoordsData.AddrOfPinnedObject(),
        //            (uint)vColorCoords.Length);

        //        pColorCoordsData.Free();
        //        pDepthData.Free();

        //        return true;
        //    }

        //    return false;
        //}

        //public override bool MapColorFrameToDepthCoords(KinectInterop.SensorData sensorData, ref Vector2[] vDepthCoords)
        //{
        //    if (coordMapper != null && sensorData.colorImageTexture != null && sensorData.depthImage != null)
        //    {
        //        var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
        //        var pDepthCoordsData = GCHandle.Alloc(vDepthCoords, GCHandleType.Pinned);

        //        coordMapper.MapColorFrameToDepthSpaceUsingIntPtr(
        //            pDepthData.AddrOfPinnedObject(),
        //            (uint)sensorData.depthImage.Length * sizeof(ushort),
        //            pDepthCoordsData.AddrOfPinnedObject(),
        //            (uint)vDepthCoords.Length);

        //        pDepthCoordsData.Free();
        //        pDepthData.Free();

        //        return true;
        //    }

        //    return false;
        //}


        // enables or disables color camera depth frame processing
        public override void EnableColorCameraDepthFrame(KinectInterop.SensorData sensorData, bool isEnable)
        {
            base.EnableColorCameraDepthFrame(sensorData, isEnable);

            if (isEnable && sensorData.colorDepthBuffer == null)
            {
                if (sensorData.colorImageWidth > 0 && sensorData.colorImageHeight > 0)
                {
                    int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight / 2;
                    sensorData.colorDepthBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
                }
            }
            else if (!isEnable && sensorData.colorDepthBuffer != null)
            {
                sensorData.colorDepthBuffer.Release();
                sensorData.colorDepthBuffer.Dispose();
                sensorData.colorDepthBuffer = null;
            }
        }

        // enables or disables color camera body-index frame processing
        public override void EnableColorCameraBodyIndexFrame(KinectInterop.SensorData sensorData, bool isEnable)
        {
            base.EnableColorCameraBodyIndexFrame(sensorData, isEnable);

            if (isEnable && sensorData.colorBodyIndexBuffer == null)
            {
                if (sensorData.colorImageWidth > 0 && sensorData.colorImageHeight > 0)
                {
                    int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight / 4;
                    sensorData.colorBodyIndexBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
                }
            }
            else if (!isEnable && sensorData.colorBodyIndexBuffer != null)
            {
                sensorData.colorBodyIndexBuffer.Release();
                sensorData.colorBodyIndexBuffer.Dispose();
                sensorData.colorBodyIndexBuffer = null;
            }
        }

    }
}

#endif