using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Microsoft.Azure.Kinect.Sensor;
using System;

namespace com.rfilkov.kinect
{
    public class Kinect4AzureInterface : DepthSensorBase
    {
        [Tooltip("Color camera resolution.")]
        public ColorCameraMode colorCameraMode = ColorCameraMode._1920_x_1080_30Fps;
        public enum ColorCameraMode : int { _1280_x_720_30Fps = 1, _1920_x_1080_30Fps = 2, _2560_x_1440_30Fps = 3, _2048_x_1536_30Fps = 4, _3840_x_2160_30Fps = 5, _4096_x_3072_15Fps = 6 }

        [Tooltip("Depth camera mode.")]
        public DepthCameraMode depthCameraMode = DepthCameraMode._640_x_576_30Fps_3_86mNfov;
        public enum DepthCameraMode : int { _320_x_288_30Fps_5_46mNfov = 1, _640_x_576_30Fps_3_86mNfov = 2, _512_x_512_30Fps_2_88mWfov = 3, _1024x1024_15Fps_2_21mWfov = 4, PassiveIR_30Fps = 5 }

        [Tooltip("Device sync mode, in case of multiple wired sensors.")]
        public WiredSyncMode deviceSyncMode = WiredSyncMode.Standalone;

        [Tooltip("Subordinate device delay off master (in usec), in case of multiple wired sensors.")]
        public int subDeviceDelayUsec = 0;

        [Tooltip("Whether to flip the IMU rotation.")]
        public bool flipImuRotation = false;


        // references to the sensor
        public Device kinectSensor = null;
        public Calibration coordMapper;
        public Transformation coordMapperTransform = null;
        private Image d2cColorData = null;
        private Image c2dDepthData = null;

        // playback and record
        public Playback kinectPlayback = null;
        private long playbackStartTime = 0;

        // status of the cameras
        private bool isCamerasStarted = false;
        private bool isImuStarted = false;

        // current frame number
        private ulong currentFrameNumber = 0;
        private TimeSpan timeToWait = TimeSpan.FromMilliseconds(0);

        // imu
        private ImuSample lastImuSample = null;
        private ImuSample curImuSample = null;

        private AHRS.MahonyAHRS imuAhrs = null;
        private Quaternion imuTurnRot1 = Quaternion.identity;
        private Quaternion imuTurnRot2 = Quaternion.identity;
        //private bool lastFlipImuRot = false;

        private Quaternion imuRotation = Quaternion.identity;
        private float prevRotZ = 0f;
        private float[] prevQuat = new float[4];

        private Vector3 imuYawAtStart = Vector3.zero;
        private Quaternion imuFixAtStart = Quaternion.identity;
        private bool imuYawAtStartSet = false;

        // TODO: to be removed later
        //public event Action<ImuSample, ulong, ulong> OnImuSample;



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

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

            int deviceCount = Device.GetInstalledCount();
            for (int i = 0; i < deviceCount; i++)
            {
                KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo();
                sensorInfo.sensorId = i.ToString();
                sensorInfo.sensorName = "Kinect4Azure";

                sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll;

                Debug.Log(string.Format("  D{0}: {1}, id: {2}", i, sensorInfo.sensorName, sensorInfo.sensorId));

                alSensorInfo.Add(sensorInfo);
            }

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

            return alSensorInfo;
        }

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

            // try to open the sensor or play back the recording
            KinectInterop.SensorData sensorData = new KinectInterop.SensorData();

            if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
            {
                if (string.IsNullOrEmpty(recordingFile))
                {
                    Debug.LogError("Playback selected, but the path to recording file is missing.");
                    return null;
                }

                if (!System.IO.File.Exists(recordingFile))
                {
                    Debug.LogError("PlayRecording selected, but the recording file cannot be found: " + recordingFile);
                    return null;
                }

                Debug.Log("Playing back: " + recordingFile);
                kinectPlayback = new Playback(recordingFile);

                colorCameraMode = (ColorCameraMode)kinectPlayback.playback_config.color_resolution;
                depthCameraMode = (DepthCameraMode)kinectPlayback.playback_config.depth_mode;
                deviceSyncMode = kinectPlayback.playback_config.wired_sync_mode;

                coordMapper = kinectPlayback.playback_calibration;
                playbackStartTime = 0;

                Debug.Log(string.Format("color_track_enabled: {0}, depth_track_enabled: {1}, ir_track_enabled: {2}, imu_track_enabled: {3}, depth_delay_off_color_usec: {4}",
                    kinectPlayback.playback_config.color_track_enabled, kinectPlayback.playback_config.depth_track_enabled,
                    kinectPlayback.playback_config.ir_track_enabled, kinectPlayback.playback_config.imu_track_enabled,
                    kinectPlayback.playback_config.depth_delay_off_color_usec));
            }
            else
            {
                List<KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
                if (deviceIndex >= alSensors.Count)
                {
                    Debug.Log("  D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
                    return null;
                }

                // try to open the sensor
                kinectSensor = Device.Open(deviceIndex);
                if (kinectSensor == null)
                {
                    Debug.LogError("  D" + recordingFile + " cannot be opened.");
                    return null;
                }

                if(deviceSyncMode == WiredSyncMode.Master && (dwFlags & KinectInterop.FrameSource.TypeColor) == 0)
                {
                    // fix by Andreas Pedroni - master requires color camera on
                    dwFlags |= KinectInterop.FrameSource.TypeColor;
                }

                DeviceConfiguration kinectConfig = new DeviceConfiguration();
                kinectConfig.SynchronizedImagesOnly = isSyncDepthAndColor;
                kinectConfig.WiredSyncMode = deviceSyncMode;
                kinectConfig.SuboridinateDelayOffMaster = new TimeSpan(subDeviceDelayUsec * 10);

                // color
                kinectConfig.ColorFormat = ImageFormat.ColorBGRA32;
                if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
                {
                    kinectConfig.ColorResolution = (ColorResolution)colorCameraMode;
                }
                else
                {
                    kinectConfig.ColorResolution = ColorResolution.Off;
                }

                // depth
                if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
                {
                    kinectConfig.DepthMode = (DepthMode)depthCameraMode;
                }
                else
                {
                    kinectConfig.DepthMode = DepthMode.Off;
                }

                // fps
                if(colorCameraMode != ColorCameraMode._4096_x_3072_15Fps && depthCameraMode != DepthCameraMode._1024x1024_15Fps_2_21mWfov)
                {
                    kinectConfig.CameraFPS = FPS.FPS30;
                }
                else
                {
                    kinectConfig.CameraFPS = FPS.FPS15;
                }

                // infrared
                if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
                {
                    // ??
                }

                // start the cameras
                kinectSensor.StartCameras(kinectConfig);
                isCamerasStarted = true;

                if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
                {
                    // start the IMU
                    kinectSensor.StartImu();
                    isImuStarted = true;
                }

                // get reference to the coordinate mapper
                coordMapper = kinectSensor.GetCalibration();
            }

            // reset the frame number
            currentFrameNumber = 0;

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

            // depth camera offset & matrix z-flip
            sensorRotOffset = new Vector3(6f, 0f, 0f);   // the depth camera is tilted 6 degrees downwards
            sensorRotFlipZ = true;
            sensorRotIgnoreY = true;

            // color camera data & intrinsics
            sensorData.colorImageFormat = TextureFormat.BGRA32;
            sensorData.colorImageStride = 4;  // 4 bytes per pixel

            if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
            {
                CameraCalibration colorCamera = coordMapper.ColorCameraCalibration;
                sensorData.colorImageWidth = colorCamera.ResolutionWidth;
                sensorData.colorImageHeight = colorCamera.ResolutionHeight;

                rawColorImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * 4];

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

            // depth camera data & intrinsics
            if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
            {
                CameraCalibration depthCamera = coordMapper.DepthCameraCalibration;
                sensorData.depthImageWidth = depthCamera.ResolutionWidth;
                sensorData.depthImageHeight = depthCamera.ResolutionHeight;

                rawDepthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
                sensorData.depthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
            }

            // infrared data
            if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
            {
                if (sensorData.depthImageWidth == 0 || sensorData.depthImageHeight == 0)
                {
                    CameraCalibration depthCamera = coordMapper.DepthCameraCalibration;
                    sensorData.depthImageWidth = depthCamera.ResolutionWidth;
                    sensorData.depthImageHeight = depthCamera.ResolutionHeight;
                }

                rawInfraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
                sensorData.infraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];

                minInfraredValue = 0f;
                maxInfraredValue = 1000f;
            }

            // imu data
            if (kinectPlayback != null || (dwFlags & KinectInterop.FrameSource.TypePose) != 0)
            {
                imuAhrs = new AHRS.MahonyAHRS(0.0006f, 5f);  // 600us

                imuTurnRot1 = Quaternion.Euler(0f, 90f, 90f);
                imuTurnRot2 = Quaternion.Euler(90f, !flipImuRotation ? 90f : -90f, 0f);
                //lastFlipImuRot = flipImuRotation;
            }

            // calibration data
            GetCameraIntrinsics(CalibrationDeviceType.Color, coordMapper.ColorCameraCalibration, ref sensorData.colorCamIntr);
            GetCameraIntrinsics(CalibrationDeviceType.Depth, coordMapper.DepthCameraCalibration, ref sensorData.depthCamIntr);
            GetCameraExtrinsics(coordMapper.ColorCameraCalibration.Extrinsics, ref sensorData.depth2ColorExtr);
            GetCameraExtrinsics(coordMapper.DepthCameraCalibration.Extrinsics, ref sensorData.color2DepthExtr);

            // body & body-index data
            if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
            {
                InitBodyTracking(dwFlags, sensorData, coordMapper, true);
            }

            Debug.Log("Kinect4Azure-sensor opened.");

            return sensorData;
        }


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

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

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

            if (coordMapperTransform != null)
            {
                // dispose coord-mapper transform
                coordMapperTransform.Dispose();
                coordMapperTransform = null;
            }

            if(kinectPlayback != null)
            {
                // close the playback file
                kinectPlayback.Dispose();
                kinectPlayback = null;
            }

            if (kinectSensor != null)
            {
                // stop cameras, if needed
                if(isCamerasStarted)
                {
                    isCamerasStarted = false;
                    kinectSensor.StopCameras();
                }

                // stop IMU, if needed
                if(isImuStarted)
                {
                    isImuStarted = false;
                    kinectSensor.StopImu();
                }

                // close the sensor
                kinectSensor.Dispose();
                kinectSensor = null;
            }

            Debug.Log("Kinect4Azure-sensor closed.");

        }


        public override void PollSensorFrames(KinectInterop.SensorData sensorData)
        {
            try
            {
                if (kinectPlayback != null)
                {
                    if (kinectPlayback.IsEndOfStream())
                        return;

                    if(playbackStartTime == 0)
                    {
                        // start time
                        playbackStartTime = DateTime.Now.Ticks;
                    }

                    long currentPlayTime = DateTime.Now.Ticks - playbackStartTime;

                    if ((frameSourceFlags & (KinectInterop.FrameSource)0x7F) != 0)
                    {
                        kinectPlayback.SeekTimestamp((ulong)(currentPlayTime / 10));
                        Capture capture = kinectPlayback.GetNextCapture();

                        if (capture != null)
                        {
                            ProcessCameraFrames(sensorData, capture);
                            capture.Dispose();

                            currentFrameNumber++;
                        }
                        else
                        {
                            Debug.Log("End of recording detected.");
                        }
                    }

                    if ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0)
                    {
                        ImuSample imuSample = kinectPlayback.GetNextImuSample();

                        while (imuSample != null)
                        {
                            ProcessImuFrame(imuSample);

                            ulong imuTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks;
                            if (kinectPlayback.IsEndOfStream() || imuTimestamp >= rawDepthTimestamp)
                                break;

                            imuSample = kinectPlayback.GetNextImuSample();
                        }
                    }
                }
                else
                {
                    if (isCamerasStarted)
                    {
                        Capture capture = kinectSensor.GetCapture(timeToWait);
                        ProcessCameraFrames(sensorData, capture);
                        capture.Dispose();

                        currentFrameNumber++;
                    }

                    if (isImuStarted)
                    {
                        ImuSample imuSample = kinectSensor.GetImuSample(timeToWait);

                        while(imuSample != null)
                        {
                            ProcessImuFrame(imuSample);
                            imuSample = kinectSensor.GetImuSample(timeToWait);
                        }
                    }
                }
            }
            catch (System.TimeoutException)
            {
                // do nothing
            }
            catch (System.Exception ex)
            {
                Debug.LogException(ex);
            }
        }


        // processes the camera frames
        private void ProcessCameraFrames(KinectInterop.SensorData sensorData, Capture capture)
        {
            //// check for color & depth sync
            //if (isSyncDepthAndColor && (capture.Color == null || capture.Depth == null))
            //    return;

            Capture btCapture = null;

            try
            {
                if (bodyTracker != null)
                {
                    // check for available body frame
                    btCapture = PollBodyFrame(sensorData, capture);

                    if(isSyncBodyAndDepth)
                    {
                        // process the body tracking capture
                        capture = btCapture;
                        //if(capture != null)
                        //    Debug.Log("BtDepthTimestamp: " + capture.Depth.DeviceTimestamp.Ticks);
                    }
                }

                // check for body & depth sync
                if (!isSyncBodyAndDepth || btCapture != null /**&& (btQueueCount == 0 && sensorData.lastBodyFrameTime == rawDepthTimestamp)*/)  // currentBodyTimestamp
                {
                    // color frame
                    if (capture.Color != null && rawColorImage != null && rawColorTimestamp == sensorData.lastColorFrameTime)
                    {
                        if (kinectPlayback != null)
                            WaitForPlaybackTimestamp("color", capture.Color.DeviceTimestamp.Ticks);

                        lock (colorFrameLock)
                        {
                            //capture.Color.CopyBytesTo(rawColorImage, 0, 0, rawColorImage.Length);
                            KinectInterop.CopyBytes(capture.Color.GetBuffer(), (int)capture.Color.Size, rawColorImage, rawColorImage.Length, sizeof(byte));
                            rawColorTimestamp = (ulong)capture.Color.DeviceTimestamp.Ticks;
                            //Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
                        }
                    }

                    // depth frame
                    if (capture.Depth != null && rawDepthImage != null && rawDepthTimestamp == sensorData.lastDepthFrameTime)
                    {
                        if (kinectPlayback != null)
                            WaitForPlaybackTimestamp("depth", capture.Depth.DeviceTimestamp.Ticks);

                        lock (depthFrameLock)
                        {
                            //capture.Depth.CopyTo(rawDepthImage, 0, 0, rawDepthImage.Length);
                            KinectInterop.CopyBytes(capture.Depth.GetBuffer(), (int)capture.Depth.Size, rawDepthImage, rawDepthImage.Length, sizeof(ushort));
                            rawDepthTimestamp = (ulong)capture.Depth.DeviceTimestamp.Ticks;
                            //Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
                        }
                    }

                    // infrared frame
                    if (capture.IR != null && rawInfraredImage != null && rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
                    {
                        if (kinectPlayback != null)
                            WaitForPlaybackTimestamp("ir", capture.IR.DeviceTimestamp.Ticks);

                        lock (infraredFrameLock)
                        {
                            //capture.IR.CopyTo(rawInfraredImage, 0, 0, rawInfraredImage.Length);
                            KinectInterop.CopyBytes(capture.IR.GetBuffer(), (int)capture.IR.Size, rawInfraredImage, rawInfraredImage.Length, sizeof(ushort));
                            rawInfraredTimestamp = (ulong)capture.IR.DeviceTimestamp.Ticks;
                            //Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
                        }
                    }

                    // transformation data frames
                    if ((depth2ColorDataFrame != null || color2DepthDataFrame != null) && capture.Color != null && capture.Depth != null &&
                        (rawColorTimestamp != sensorData.lastColorFrameTime || rawDepthTimestamp != sensorData.lastDepthFrameTime))
                    {
                        if (coordMapperTransform == null)
                        {
                            coordMapperTransform = coordMapper.CreateTransformation();
                        }

                        if (depth2ColorDataFrame != null)
                        {
                            if(d2cColorData == null)
                            {
                                d2cColorData = new Image(ImageFormat.ColorBGRA32, sensorData.depthImageWidth, sensorData.depthImageHeight, sensorData.depthImageWidth * sensorData.colorImageStride);
                            }

                            lock (depth2ColorFrameLock)
                            {
                                coordMapperTransform.ColorImageToDepthCamera(capture.Depth, capture.Color, d2cColorData);
                                d2cColorData.CopyTo<byte>(depth2ColorDataFrame, 0, 0, depth2ColorDataFrame.Length);
                                lastDepth2ColorFrameTime = (ulong)capture.Depth.DeviceTimestamp.Ticks;
                            }
                        }

                        if (color2DepthDataFrame != null)
                        {
                            if (c2dDepthData == null)
                            {
                                c2dDepthData = new Image(ImageFormat.Depth16, sensorData.colorImageWidth, sensorData.colorImageHeight, sensorData.colorImageWidth * sizeof(ushort));
                            }

                            lock (color2DepthFrameLock)
                            {
                                coordMapperTransform.DepthImageToColorCamera(capture.Depth, c2dDepthData);
                                c2dDepthData.CopyTo<ushort>(color2DepthDataFrame, 0, 0, color2DepthDataFrame.Length);
                                lastColor2DepthFrameTime = (ulong)capture.Color.DeviceTimestamp.Ticks;
                            }
                        }
                    }

                }
                else
                {
                    // ignore the capture
                    capture = null;
                }

                if (btCapture != null)
                {
                    // dispose body capture
                    btCapture.Dispose();
                }
            }
            catch (System.Exception ex)
            {
                Debug.LogException(ex);
            }
        }


        // in playback mode - waits until the given time stamp
        private void WaitForPlaybackTimestamp(string frameSource, long frameTimestamp)
        {
            //Debug.Log(string.Format("{0} ts: {1}, sys: {2}", frameSource, frameTimestamp, (DateTime.Now.Ticks - playbackStartTime)));

            long currentPlayTime = DateTime.Now.Ticks - playbackStartTime;

            while (currentPlayTime < frameTimestamp)
            {
                currentPlayTime = DateTime.Now.Ticks - playbackStartTime;
            }
        }

        // processes the IMU frame
        private void ProcessImuFrame(ImuSample imuSample)
        {
            if (kinectPlayback != null)
            {
                //WaitForPlaybackTimestamp("acc", imuSample.AccelerometerTimestampInUsec);
                //WaitForPlaybackTimestamp("gyro", imuSample.GyroTimestampInUsec);
            }

            lastImuSample = curImuSample;
            curImuSample = imuSample;

            if (!flipImuRotation && lastImuSample == null && Mathf.Abs(imuSample.AccelerometerSample.Y) >= 1f)
            {
                // check for rolled imu
                flipImuRotation = true;
                imuTurnRot2 = Quaternion.Euler(90f, -90f, 0f);
            }

            if (lastImuSample != null && imuAhrs != null && imuSample.AccelerometerTimestamp.Ticks != lastImuSample.AccelerometerTimestamp.Ticks)
            {
                prevRotZ = imuRotation.eulerAngles.z;
                Array.Copy(imuAhrs.Quaternion, prevQuat, prevQuat.Length);

                imuAhrs.SamplePeriod = ((imuSample.AccelerometerTimestamp.Ticks - lastImuSample.AccelerometerTimestamp.Ticks) / 10) * 0.000001f;
                imuAhrs.Update(imuSample.GyroSample.X, imuSample.GyroSample.Y, imuSample.GyroSample.Z, imuSample.AccelerometerSample.X, imuSample.AccelerometerSample.Y, imuSample.AccelerometerSample.Z);

                float[] ahrsQuat = imuAhrs.Quaternion;
                imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]);
                Quaternion transRotation = imuTurnRot2 * imuRotation * imuTurnRot1;

                if (imuAhrs.E2 < 0.0001f && Mathf.Abs(imuRotation.eulerAngles.z - prevRotZ) <= 0.1f)
                {
                    // correct the continuous yaw, when imu doesn't move
                    Array.Copy(prevQuat, imuAhrs.Quaternion, prevQuat.Length);

                    ahrsQuat = prevQuat;
                    imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]);
                    transRotation = imuTurnRot2 * imuRotation * imuTurnRot1;

                    if (!imuYawAtStartSet)
                    {
                        // initial yaw
                        imuYawAtStartSet = true;
                        imuYawAtStart = new Vector3(0f, transRotation.eulerAngles.y, 0f);
                        imuFixAtStart = Quaternion.Inverse(Quaternion.Euler(imuYawAtStart));
                    }
                }

                if (imuYawAtStartSet)
                {
                    // fix the initial yaw
                    transRotation = imuFixAtStart * transRotation;
                }

                lock (poseFrameLock)
                {
                    // rawPosePosition
                    rawPoseRotation = transRotation;

                    rawPoseTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks;
                    //poseFrameNumber = currentFrameNumber;
                }
            }

            //var onImuSample = OnImuSample;
            //if(onImuSample != null)
            //{
            //    onImuSample.Invoke(imuSample, rawDepthTimestamp, rawColorTimestamp);
            //}
        }


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

        //    if (lastFlipImuRot != flipImuRotation)
        //    {
        //        Vector3 imuRot2 = imuTurnRot2.eulerAngles;
        //        imuRot2.y = -imuRot2.y;
        //        imuTurnRot2 = Quaternion.Euler(imuRot2);
        //    }

        //    return bSuccess;
        //}


        // gets the given camera intrinsics
        private void GetCameraIntrinsics(CalibrationDeviceType camType, CameraCalibration camParams, ref KinectInterop.CameraIntrinsics intr)
        {
            Intrinsics camIntr = camParams.Intrinsics;
            if (camIntr.Parameters.Length < 15)
                throw new System.Exception("Intrinsics length is less than expected: " + camIntr.ParameterCount);

            intr = new KinectInterop.CameraIntrinsics();

            intr.cameraType = (int)camType;
            intr.width = camParams.ResolutionWidth;
            intr.height = camParams.ResolutionHeight;

            // 0        float cx;
            // 1        float cy;
            intr.ppx = camIntr.Parameters[0];
            intr.ppy = camIntr.Parameters[1];

            // 2        float fx;            /**< Focal length x */
            // 3        float fy;            /**< Focal length y */
            intr.fx = camIntr.Parameters[2];
            intr.fy = camIntr.Parameters[3];

            // 4        float k1;
            // 5        float k2;
            // 6        float k3;
            // 7        float k4;
            // 8        float k5;
            // 9        float k6;
            intr.distCoeffs = new float[6];
            intr.distCoeffs[0] = camIntr.Parameters[4];
            intr.distCoeffs[1] = camIntr.Parameters[5];
            intr.distCoeffs[2] = camIntr.Parameters[6];
            intr.distCoeffs[3] = camIntr.Parameters[7];
            intr.distCoeffs[4] = camIntr.Parameters[8];
            intr.distCoeffs[5] = camIntr.Parameters[9];

            if (camIntr.Type == CalibrationModelType.Theta)
                intr.distType = KinectInterop.DistortionType.Theta;
            else if (camIntr.Type == CalibrationModelType.Polynomial3K)
                intr.distType = KinectInterop.DistortionType.Polynomial3K;
            else if (camIntr.Type == CalibrationModelType.Rational6KT)
                intr.distType = KinectInterop.DistortionType.Rational6KT;
            else
                intr.distType = (KinectInterop.DistortionType)camIntr.Type;

            // 10            float codx;
            // 11            float cody;
            intr.codx = camIntr.Parameters[10];
            intr.cody = camIntr.Parameters[11];

            // 12            float p2;
            // 13            float p1;
            intr.p2 = camIntr.Parameters[12];
            intr.p1 = camIntr.Parameters[13];

            // 14           float metric_radius;
            intr.maxRadius = camIntr.Parameters[14];

            EstimateFOV(intr);
        }


        // gets the given camera extrinsics
        private void GetCameraExtrinsics(Extrinsics camExtr, ref KinectInterop.CameraExtrinsics extr)
        {
            extr = new KinectInterop.CameraExtrinsics();

            extr.rotation = new float[camExtr.Rotation.Length];
            camExtr.Rotation.CopyTo(extr.rotation, 0);

            extr.translation = new float[camExtr.Translation.Length];
            camExtr.Translation.CopyTo(extr.translation, 0);
        }


        public override Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
        {
            if (sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
            {
                Vector3 depthSpacePos = UnprojectPoint(sensorData.depthCamIntr, depthPos, (float)depthVal);
                Vector3 colorSpacePos = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos);
                Vector2 colorPos = ProjectPoint(sensorData.colorCamIntr, colorSpacePos);

                return colorPos;
            }

            return Vector2.zero;
        }


        // unprojects plane point into the space
        protected override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
        {
            if (depth <= 0f)
                return Vector3.zero;

            System.Numerics.Vector2 fPixel = new System.Numerics.Vector2(pixel.x, pixel.y);
            System.Numerics.Vector3? fPoint = coordMapper.TransformTo3D(fPixel, depth, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType);
            Vector3 point = fPoint.HasValue ? new Vector3(fPoint.Value.X, fPoint.Value.Y, fPoint.Value.Z) : Vector3.zero;

            return point;
        }


        // projects space point onto a plane
        protected override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
        {
            if (point == Vector3.zero)
                return Vector2.zero;

            System.Numerics.Vector3 fPoint = new System.Numerics.Vector3(point.x, point.y, point.z);
            System.Numerics.Vector2? fPixel = coordMapper.TransformTo2D(fPoint, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType);
            Vector2 pixel = fPixel.HasValue ? new Vector2(fPixel.Value.X, fPixel.Value.Y) : Vector2.zero;

            return pixel;
        }


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

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


        //public override bool MapDepthFrameToColorData(KinectInterop.SensorData sensorData, ref byte[] vColorFrameData)
        //{
        //    if(coordMapperTransform == null)
        //    {
        //        coordMapperTransform = coordMapper.CreateTransformation();
        //    }

        //    if(vColorFrameData == null)
        //    {
        //        vColorFrameData = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * sensorData.colorImageStride];
        //    }

        //    return false;
        //}


        //public override bool MapColorFrameToDepthData(KinectInterop.SensorData sensorData, ref ushort[] vDepthFrameData)
        //{
        //    return false;
        //}


    }
}