/// KinectCam
/// MIG module for XBox One Kinect
/// Jacobo Amselem 2018
using System.Drawing;
using System.Drawing.Imaging;
using System;
using System.Collections.Generic;
using Microsoft.Kinect;
using MIG.Config;
using System.IO;
using System.Windows;
using System.Windows.Media;
using System.Windows.Media.Imaging;

namespace MIG.Interfaces.HGKinect
{

    public class KinectCam : MigInterface
    {

        #region MigInterface API commands and events

        public enum Commands
        {
            Control_On,
            Control_Off,
            SetControl,
            GetImage,
            SetOutputFormat
        }

        private String outputFormat = "RGB";

        #endregion

        #region Private fields

        private bool isConnected;
        private List<InterfaceModule> modules;

        #endregion

        #region Lifecycle

        public KinectCam()
        {

            modules = new List<InterfaceModule>();

            var module_1 = new InterfaceModule();
            module_1.Domain = this.GetDomain();
            module_1.Description = "Kinect Camera";
            module_1.Address = "100";
            module_1.ModuleType = ModuleTypes.Sensor;
            // add them to the modules list
            modules.Add(module_1);
            
            OnInterfacePropertyChanged(this.GetDomain(), module_1.Address, "Kinect OFF", "Status.Level", 0);
            OnInterfacePropertyChanged(this.GetDomain(), module_1.Address, "Output Format", "Output.Format", this.outputFormat);
        }

        public void Dispose()
        {
            Disconnect();
        }

        #endregion

        #region MIG Interface members

        public event InterfaceModulesChangedEventHandler InterfaceModulesChanged;
        public event InterfacePropertyChangedEventHandler InterfacePropertyChanged;

        public bool IsEnabled { get; set; }

        public List<Option> Options { get; set; }

        public void OnSetOption(Option option)
        {
            if (IsEnabled)
                Connect();
        }

        public List<InterfaceModule> GetModules()
        {
            return modules;
        }

        /// <summary>
        /// Gets a value indicating whether the interface/controller device is connected or not.
        /// </summary>
        /// <value>
        /// <c>true</c> if it is connected; otherwise, <c>false</c>.
        /// </value>
        public bool IsConnected
        {
            get { return isConnected; }
        }

        /// <summary>
        /// Returns true if the device has been found in the system
        /// </summary>
        /// <returns></returns>
        public bool IsDevicePresent()
        {
            return true;
        }

        public bool Connect()
        {
            if (!isConnected)
            {
                isConnected = true;
            }
            OnInterfaceModulesChanged(this.GetDomain());
            return true;
        }

        public void Disconnect()
        {
            if (isConnected)
            {
                isConnected = false;
            }
        }

        private KinectSensor kinectSensor = null;

        /// <summary>
        /// Reader for color frames
        /// </summary>
        private ColorFrameReader colorFrameReader = null;


        /// <summary>
        /// Reader for infrared frames
        /// </summary>
        private InfraredFrameReader infraredFrameReader = null;


        /// <summary>
        /// Bitmap to display
        /// </summary>
        //private   WriteableBitmap colorBitmap = null;
        private Bitmap outputBitmap = null;


        private void InitializeKinectSensor()
        {
            CloseKinectSensor();

            // get the kinectSensor object
            this.kinectSensor = KinectSensor.GetDefault();

            this.OpenColorFrameReader();
            
            // open the sensor
            this.kinectSensor.Open();

        }

        private void OpenColorFrameReader()
        {
            // open the reader for the color frames
            this.colorFrameReader = this.kinectSensor.ColorFrameSource.OpenReader();

            // wire handler for frame arrival
            this.colorFrameReader.FrameArrived += this.Reader_ColorFrameArrived;
        }

        private void OpenInfraredFrameReader()
        {
            // open the reader for the infrared frames
            this.infraredFrameReader = this.kinectSensor.InfraredFrameSource.OpenReader();

            // wire handler for frame arrival
            this.infraredFrameReader.FrameArrived += this.Reader_InfraredFrameArrived;
        }


        private void CloseKinectSensor()
        {
            CloseColorFrameReader();
            CloseInfraredFrameReader();

            if (this.kinectSensor != null)
            {
                this.kinectSensor.Close();
                this.kinectSensor = null;
            }
        }

        private void CloseColorFrameReader()
        {
            if (this.colorFrameReader != null)
            {
                this.colorFrameReader.FrameArrived -= this.Reader_ColorFrameArrived;
                // ColorFrameReder is IDisposable
                this.colorFrameReader.Dispose();
                this.colorFrameReader = null;
            }
        }

        private void CloseInfraredFrameReader()
        {
            if (this.infraredFrameReader != null)
            {
                // infraredFrameReder is IDisposable
                this.infraredFrameReader.Dispose();
                this.infraredFrameReader = null;
            }
        }


        /// <summary>
        /// Handles the color frame data arriving from the sensor
        /// </summary>
        /// <param name="sender">object sending the event</param>
        /// <param name="e">event arguments</param>
        private void Reader_ColorFrameArrived(object sender, ColorFrameArrivedEventArgs e)
        {
            // ColorFrame is IDisposable
            using (ColorFrame colorFrame = e.FrameReference.AcquireFrame())
            {
                if (colorFrame != null)
                {
                    outputBitmap = GetBitmap(ToBitmap(colorFrame));
                }
            }
        }

        private void Reader_InfraredFrameArrived(object sender, InfraredFrameArrivedEventArgs e)
        {
            // infraredFrame is IDisposable
            using (InfraredFrame infraredFrame = e.FrameReference.AcquireFrame())
            {
                if (infraredFrame != null)
                {
                    outputBitmap = IRFrameToBitmap(infraredFrame);
                }
            }
        }

        private BitmapSource ToBitmap(ColorFrame frame)
        {
            int width = frame.FrameDescription.Width;
            int height = frame.FrameDescription.Height;
            var format = PixelFormats.Bgr32;

            byte[] pixels = new byte[width * height * ((PixelFormats.Bgr32.BitsPerPixel + 7) / 8)];

            if (frame.RawColorImageFormat == ColorImageFormat.Bgra)
            {
                frame.CopyRawFrameDataToArray(pixels);
            }
            else
            {
                frame.CopyConvertedFrameDataToArray(pixels, ColorImageFormat.Bgra);
            }

            int stride = width * format.BitsPerPixel / 8;

            return BitmapSource.Create(width, height, 96, 96, format, null, pixels, stride);

        }

        /// <summary>
        /// Maximum value (as a float) that can be returned by the InfraredFrame
        /// </summary>
        private const float InfraredSourceValueMaximum = (float)ushort.MaxValue;

        /// <summary>
        /// The value by which the infrared source data will be scaled
        /// </summary>
        private const float InfraredSourceScale = 0.75f;

        /// <summary>
        /// Smallest value to display when the infrared data is normalized
        /// </summary>
        private const float InfraredOutputValueMinimum = 0.01f;

        /// <summary>
        /// Largest value to display when the infrared data is normalized
        /// </summary>
        private const float InfraredOutputValueMaximum = 1.0f;

    
        private Bitmap IRFrameToBitmap(InfraredFrame frame)
        {
            int width = frame.FrameDescription.Width;
            int height = frame.FrameDescription.Height;
            System.Drawing.Imaging.PixelFormat format = System.Drawing.Imaging.PixelFormat.Format32bppRgb;

            ushort[] infraredData = new ushort[frame.FrameDescription.LengthInPixels];
            byte[] pixelData = new byte[frame.FrameDescription.LengthInPixels * 4];

            frame.CopyFrameDataToArray(infraredData);

            for (int infraredIndex = 0; infraredIndex < infraredData.Length; infraredIndex++)
            {
                ushort ir = infraredData[infraredIndex];

                var pixel = Math.Min(InfraredOutputValueMaximum, (((float)ir / InfraredSourceValueMaximum * InfraredSourceScale) * (1.0f - InfraredOutputValueMinimum)) + InfraredOutputValueMinimum);

                var intensity = (byte) ((pixel * 255) + 0.5);

                //byte intensity = (byte)(ir >> 8);

                pixelData[infraredIndex * 4] = intensity; // Blue
                pixelData[infraredIndex * 4 + 1] = intensity; // Green   
                pixelData[infraredIndex * 4 + 2] = intensity; // Red
                pixelData[infraredIndex * 4 + 3] = 255;
            }

            var bitmap = new Bitmap(width, height, format);
            var bitmapdata = bitmap.LockBits(
                new System.Drawing.Rectangle(0, 0, width, height),
                ImageLockMode.WriteOnly,
                bitmap.PixelFormat
            );
            IntPtr ptr = bitmapdata.Scan0;

            System.Runtime.InteropServices.Marshal.Copy(pixelData, 0, ptr, pixelData.Length);
            bitmap.UnlockBits(bitmapdata);

            return bitmap;
        }

        Bitmap GetBitmap(BitmapSource source)
        {
            Bitmap bmp = new Bitmap(
              source.PixelWidth,
              source.PixelHeight,
              System.Drawing.Imaging.PixelFormat.Format32bppPArgb);
            BitmapData data = bmp.LockBits(
              new Rectangle(System.Drawing.Point.Empty, bmp.Size),
              ImageLockMode.WriteOnly,
              System.Drawing.Imaging.PixelFormat.Format32bppPArgb);
            source.CopyPixels(
              Int32Rect.Empty,
              data.Scan0,
              data.Height * data.Stride,
              data.Stride);
            bmp.UnlockBits(data);
            return bmp;
        }

        public object InterfaceControl(MigInterfaceCommand request)
        {
            var response = new ResponseText("OK"); //default success value

            Commands command;
            Enum.TryParse<Commands>(request.Command.Replace(".", "_"), out command);
            
            var module = modules.Find(m => m.Address.Equals(request.Address));

            if (module != null)
            {
                if (command == Commands.SetControl)
                    command = (request.OptionsString.Split('/')[0] == "1") ? Commands.Control_On : Commands.Control_Off;

                switch (command)
                {
                    case Commands.SetOutputFormat:
                        this.CloseColorFrameReader();
                        this.CloseInfraredFrameReader();

                        outputFormat = request.OptionsString.Split('/')[0];

                        switch (outputFormat)
                        {
                            case "Infrared":
                                this.OpenInfraredFrameReader();
                                break;

                            default:
                                this.OpenColorFrameReader();
                                break;
                        }
                            
                        OnInterfacePropertyChanged(this.GetDomain(), request.Address, "Output Format", "Output.Format", this.outputFormat);
                        break;

                    case Commands.Control_On:
                        this.InitializeKinectSensor();
                        //OnInterfacePropertyChanged(this.GetDomain(), request.Address, "Widget", "Widget.DisplayModule", "homegenie/generic/camerainput");
                        OnInterfacePropertyChanged(this.GetDomain(), request.Address, "Kinect ON", "Status.Level", 1);
                        break;

                    case Commands.Control_Off:
                        this.CloseKinectSensor();
                        OnInterfacePropertyChanged(this.GetDomain(), request.Address, "Kinect OFF", "Status.Level", 0);
                        break;

                    case Commands.GetImage:
                        return ImageToByte((kinectSensor != null) ? outputBitmap : new Bitmap(1, 1)); 
                        break;
                }
            }
            else
            {
                response = new ResponseText("ERROR: invalid module address");
            }

            return response;
        }

        private static byte[] ImageToByte(Image img)
        {
            using (var stream = new MemoryStream())
            {
                img.Save(stream, System.Drawing.Imaging.ImageFormat.Jpeg);
                return stream.ToArray();
            }
        }

        #endregion



        protected virtual void OnInterfaceModulesChanged(string domain)
        {
            if (InterfaceModulesChanged != null)
            {
                var args = new InterfaceModulesChangedEventArgs(domain);
                InterfaceModulesChanged(this, args);
            }
        }

        protected virtual void OnInterfacePropertyChanged(string domain, string source, string description, string propertyPath, object propertyValue)
        {
            if (InterfacePropertyChanged != null)
            {
                var args = new InterfacePropertyChangedEventArgs(domain, source, description, propertyPath, propertyValue);
                InterfacePropertyChanged(this, args);
            }
        }

    }


}