//>>> _using
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using SharpDX;
using SharpDX.Direct3D11;
using SharpDX.Windows;
//<<< _using
using Microsoft.Kinect;
using SharpDX.Direct3D;
using SharpDX.DXGI;
using System.Dynamic;
using System.Runtime.InteropServices;

namespace Framefield.Core.ID1d1f72d6_f327_4a73_8452_075339602fdc
{
    public class Class_KinectResource : FXImageFunction, Framefield.Core.OperatorPartTraits.ITimeAccessor
    {
        //>>> _outputids
        private enum OutputId
        {
            Image = 0,
            Skeletons = 1,
            Available = 2
        }
        //<<< _outputids

        //>>> _inputids
        private enum InputId
        {
            Code = 0,
            Running = 1,
            DisplayMode = 2,
            ResetTrigger = 3,
            Value = 4
        }
        //<<< _inputids
 

        public Class_KinectResource() 
        {
            _currentStatus = KinectStatus.Undefined;
            KinectSensor sensor = KinectSensor.KinectSensors.FirstOrDefault(s => s.Status == KinectStatus.Connected);
            InitializeSensor(sensor);
            KinectSensor.KinectSensors.StatusChanged += HandleKinectStatusChanged;
        }

        public override void Dispose()
        {
            FinalizeSensor(_sensor);
            Utilities.DisposeObj(ref _colorResource);
            Utilities.DisposeObj(ref _colorSRV);
            Utilities.DisposeObj(ref _depthResource);
            Utilities.DisposeObj(ref _depthSRV);
            KinectSensor.KinectSensors.StatusChanged -= HandleKinectStatusChanged;
            base.Dispose();
        }

        public override OperatorPartContext Eval(OperatorPartContext context, List<OperatorPart> inputs, int outputIdx)
        {
            //>>> _params
            var Code = inputs[(int)InputId.Code].Eval(context).Text;
            var Running = inputs[(int)InputId.Running].Eval(context).Value;
            var DisplayMode = (int) inputs[(int)InputId.DisplayMode].Eval(context).Value;
            var ResetTrigger = inputs[(int)InputId.ResetTrigger].Eval(context).Value;
            var Value = inputs[(int)InputId.Value].Eval(context).Value;
            //<<< _params

            if (_displayMode != DisplayMode)
            {
                _displayMode = DisplayMode;
                InitializeSensor(_sensor);
            }

            if (_sensor == null)
            {
                context.Image = null;
                context.Value = 0.0f;
                context.Dynamic = null;
                return context;
            }

            bool triggerUpFlankDetected = _lastStoreTrigger < 0.5 && ResetTrigger > 0.5;
            if (triggerUpFlankDetected)
            {
                InitializeSensor(_sensor);
            }
            _lastStoreTrigger = ResetTrigger;

            if (Running > 0.5 && !_sensor.IsRunning)
            {
                try
                {
                    _sensor.Start();
                    //_sensor.ElevationAngle = 15;
                }
                catch(Exception e)
                {
                    Logger.Error(this,"error starting kinect sensor {0}", e);
                }
            }

            if (Running < 0.5 && _sensor.IsRunning)
            {
                try
                {
                    _sensor.Stop();
                }
                catch(Exception)
                {
                    Logger.Error(this,"error stopping kinect sensor");
                }
            }

            switch (outputIdx)
            {
                case (int)OutputId.Image: context = DoImageEvaluation(context); break;
                case (int)OutputId.Skeletons: context = DoSkeletonEvaluation(context); break;
                case (int)OutputId.Available: context.Value = _sensor.IsRunning ? 1.0f : 0.0f; break;
            }
            return context;
        }

        OperatorPartContext DoImageEvaluation(OperatorPartContext context)
        {
            if (_colorSRV == null || _depthSRV == null)
                return context;

            Changed = true;

            return PrepareAndEvalOnChange(context, () =>
            {
                ClearRenderTarget(context, new Color4(0, 0, 0, 0));

                var depthStencilDescription = new DepthStencilStateDescription
                                                  {
                                                      IsDepthEnabled = true,
                                                      DepthWriteMask = DepthWriteMask.All,
                                                      DepthComparison = Comparison.Always
                                                  };
                using (var depthStencilState = new DepthStencilState(context.D3DDevice, depthStencilDescription))
                //>>> _setup__
                {
                    _effect.GetVariableByName("RenderTargetSize").AsVector().Set(new Vector2(_usedViewport.Width, _usedViewport.Height));
                    //<<< _setup__
                    _effect.GetVariableByName("txImage").AsShaderResource().SetResource(_colorSRV); 
                    _effect.GetVariableByName("txDepth").AsShaderResource().SetResource(_depthSRV); 

                    context.DepthStencilState = depthStencilState;

                    Render(context);

                    _imageDataFetched = true;
                //>>> _cleanup
                }
                //<<< _cleanup
            });
        }

        OperatorPartContext DoSkeletonEvaluation(OperatorPartContext context)
        {
            dynamic obj = new ExpandoObject();
            obj.TrackedSkeletons = new List<Skeleton>();

            if (_skeletonData != null)
            {
                foreach (Skeleton skeleton in _skeletonData)
                {
                    if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                    {
                        obj.TrackedSkeletons.Add(skeleton);
                    }
                }
            }
            context.Dynamic = obj;
            return context;
        }

        void HandleSensorFrameReady(object sender, AllFramesReadyEventArgs args)
        {
            if (_requestStopHandlingFrames)
                return;
            _processingFrame = true;
            try {
                if (D3DDevice.Device == null)
                    return;

                UpdateTextures(args);
            }
            catch (Exception e)
            {
                Logger.Error(this, "error receiving new frame: {0}", e.Message);
            }
            _processingFrame = false;
        }

        void UpdateTextures(AllFramesReadyEventArgs args)
        {
            if (_sensor == null ||
                !_sensor.ColorStream.IsEnabled ||
                !_sensor.DepthStream.IsEnabled ||
                !_sensor.SkeletonStream.IsEnabled)
                return;

            var desc = ResourceManager.GetTextureDescription(BindFlags.ShaderResource, Format.R8G8B8A8_UNorm, _sensor.ColorStream.FrameWidth, _sensor.ColorStream.FrameHeight);
            desc.Usage = ResourceUsage.Dynamic;
            desc.CpuAccessFlags = CpuAccessFlags.Write;
            if (ResourceManager.ValidateResource(ref _colorResource, OperatorPart, D3DDevice.Device, desc))
            {
                Utilities.DisposeObj(ref _colorSRV);
                _colorSRV = new ShaderResourceView(D3DDevice.Device, _colorResource.Texture);
            }

            desc.Format = Format.R16_UInt;
            if (ResourceManager.ValidateResource(ref _depthResource, OperatorPart, D3DDevice.Device, desc))
            {
                Utilities.DisposeObj(ref _depthSRV);

                var shaderDesc = new ShaderResourceViewDescription
                                     {
                                         Format = Format.R16_UInt,
                                         Dimension = ShaderResourceViewDimension.Texture2D,
                                         Texture2D = {MipLevels = 1}
                                     };
                _depthSRV = new ShaderResourceView(D3DDevice.Device, _depthResource.Texture, shaderDesc);
            }

            if (_imageDataFetched)
            {
                using (DepthImageFrame depthFrame = args.OpenDepthImageFrame())
                using (ColorImageFrame colorFrame = args.OpenColorImageFrame())
                {
                    if (colorFrame != null && depthFrame != null)
                    {
                        if (_depthPixels == null || _depthPixels.Length != depthFrame.PixelDataLength ||
                            _colorPixelData == null || _colorPixelData.Length != colorFrame.PixelDataLength)
                        {
                            //note: initializing the mapper earlier will not work properly
                            _coordinateMapper = new CoordinateMapper(_sensor);
                            _colorPixelData = new byte[colorFrame.PixelDataLength];
                            _colorPoints = new ColorImagePoint[depthFrame.PixelDataLength];
                            _depthPixels = new DepthImagePixel[depthFrame.PixelDataLength];
                            _mappedColorPixels = new int[depthFrame.PixelDataLength];
                        }
                        colorFrame.CopyPixelDataTo(_colorPixelData);

                        if (_displayMode == 0)
                        {
                            //infrared texture
                            int colorIndex = 0;
                            for (int i = 0; i < colorFrame.PixelDataLength; i += 2, ++colorIndex)
                            {
                                ushort s = (ushort)((_colorPixelData[i + 1] << 8) + _colorPixelData[i]);
                                byte b = (byte)(s >> 8); 
                                _mappedColorPixels[colorIndex] = (b << 16) | (b << 8) | b;
                            }
                        }
                        else if (_displayMode == 1)  
                        {
                            //color texture
                            int colorIndex = 0;
                            for (int i = 0; i < colorFrame.PixelDataLength; i += 4, ++colorIndex)
                            {
                                _mappedColorPixels[colorIndex] = (_colorPixelData[i] << 16) | (_colorPixelData[i + 1] << 8) | _colorPixelData[i + 2];
                            }

                            
                            depthFrame.CopyDepthImagePixelDataTo(_depthPixels);
                            _coordinateMapper.MapDepthFrameToColorFrame(depthFrame.Format, _depthPixels, colorFrame.Format, _colorPoints);

                            /*int colorIndex = 0;
                            for (int depthIndex = 0; depthIndex < _depthPixels.Length; ++depthIndex)
                            {
                                ColorImagePoint colorPoint = _colorPoints[depthIndex];
                                colorIndex = (colorPoint.X + colorPoint.Y * colorFrame.Width)*4;
    
                                if (colorPoint.X >= 0 && colorPoint.X < colorFrame.Width &&
                                    colorPoint.Y >= 0 && colorPoint.Y < colorFrame.Height && _depthPixels[depthIndex].IsKnownDepth)
                                {
                                    _mappedColorPixels[depthIndex] = (_colorPixelData[colorIndex] << 16) | (_colorPixelData[colorIndex + 1] << 8) | _colorPixelData[colorIndex + 2];
                                }
                                else
                                {
                                    _mappedColorPixels[depthIndex] = 0x00000000;
                                }
                            }
                            */
                        }

                        DataStream colorTextureStream;
                        D3DDevice.Device.ImmediateContext.MapSubresource(_colorResource.Texture, 0, 0, MapMode.WriteDiscard, SharpDX.Direct3D11.MapFlags.None, out colorTextureStream);
                        using (colorTextureStream)
                        {
                            Marshal.Copy(_mappedColorPixels, 0, colorTextureStream.DataPointer, _mappedColorPixels.Length);
                        }
                        D3DDevice.Device.ImmediateContext.UnmapSubresource(_colorResource.Texture, 0);

                        
                        DataStream depthTextureStream;
                        D3DDevice.Device.ImmediateContext.MapSubresource(_depthResource.Texture, 0, 0, MapMode.WriteDiscard, SharpDX.Direct3D11.MapFlags.None, out depthTextureStream);
                        using (depthTextureStream)
                        {
                            depthFrame.CopyPixelDataTo(depthTextureStream.DataPointer, depthFrame.PixelDataLength);
                        }
                        D3DDevice.Device.ImmediateContext.UnmapSubresource(_depthResource.Texture, 0);
                    }
    
                    _imageDataFetched = false;
                }
            }

            using (var skeletonFrame = args.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    if (_skeletonData == null || _skeletonData.Length != skeletonFrame.SkeletonArrayLength)
                        _skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];

                    skeletonFrame.CopySkeletonDataTo(_skeletonData);
                }
            }
        }

        void HandleKinectStatusChanged(object s, StatusChangedEventArgs args)
        {
            if (_currentStatus != KinectStatus.Connected && args.Status == KinectStatus.Connected)
            {
                InitializeSensor(args.Sensor);
            }
            else if (_currentStatus == KinectStatus.Connected && args.Status != KinectStatus.Connected)
            {
                FinalizeSensor(args.Sensor);
            }
        }

        void InitializeSensor(KinectSensor sensor)
        {
            FinalizeSensor(_sensor);

            if (sensor == null || sensor.Status != KinectStatus.Connected)
                return;

            _sensor = sensor;
            _currentStatus = sensor.Status;
            _imageDataFetched = true;

            switch (_displayMode)
            {
                case 0: _sensor.ColorStream.Enable(ColorImageFormat.InfraredResolution640x480Fps30); break;
                case 1: _sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30); break;
            }
            _sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
            _sensor.DepthStream.Range = DepthRange.Default;

            TransformSmoothParameters smoothingParam = new TransformSmoothParameters
                                                       {
                                                           //default params
                                                           //Smoothing = 0.5f,
                                                           //Correction = 0.5f,
                                                           //Prediction = 0.5f,
                                                           //JitterRadius = 0.05f,
                                                           //MaxDeviationRadius = 0.04f

                                                           //good
                                                           /*Smoothing = 0.5f,
                                                           Correction = 0.1f,
                                                           Prediction = 0.5f,
                                                           JitterRadius = 0.1f,
                                                           MaxDeviationRadius = 0.1f*/

                                                           //extreme smoothing
                                                           Smoothing = 0.7f,
                                                           Correction = 0.3f,
                                                           Prediction = 1.0f,
                                                           JitterRadius = 1.0f,
                                                           MaxDeviationRadius = 1.0f
                                                       };
            _sensor.SkeletonStream.Enable(smoothingParam);
            _sensor.SkeletonStream.TrackingMode = SkeletonTrackingMode.Default;
            _sensor.SkeletonStream.EnableTrackingInNearRange = false;

            _sensor.AllFramesReady += HandleSensorFrameReady;
            Logger.Info(this, "kinect connection established ({0})", _sensor.DeviceConnectionId);
        }
        
        void FinalizeSensor(KinectSensor sensor)
        {
            if (_sensor == null || _sensor != sensor)
                return;
            
            _requestStopHandlingFrames = true;
            _sensor.ColorStream.Disable();
            _sensor.DepthStream.Disable();
            _sensor.SkeletonStream.Disable();
            _sensor.Stop();
            _sensor.AllFramesReady -= HandleSensorFrameReady;
            while (_processingFrame);
            _requestStopHandlingFrames = false;

            Logger.Info(this,"kinect connection closed ({0}, reason: {1})", _sensor.DeviceConnectionId, _sensor.Status);
            _sensor = null;
            _currentStatus = KinectStatus.Undefined;
            _depthPixels = null;
            _coordinateMapper = null;
            _colorPixelData = null;
            _colorPoints = null;
            _depthPixels = null;
            _mappedColorPixels = null;
        }

        float _lastStoreTrigger;
        
        KinectSensor _sensor;
        KinectStatus _currentStatus;
        int _displayMode = 0;
        bool _requestStopHandlingFrames = false;
        bool _processingFrame = false;

        CoordinateMapper _coordinateMapper;
        Resource _colorResource;
        ShaderResourceView _colorSRV;
        Resource _depthResource;
        ShaderResourceView _depthSRV;
        byte[] _colorPixelData;
        int[] _mappedColorPixels;
        DepthImagePixel[] _depthPixels;
        ColorImagePoint[] _colorPoints;
        bool _imageDataFetched = true;

        Skeleton[] _skeletonData;
    }

}
