using Iisu;
using System;
using Emgu.CV;
using Emgu.CV.Structure;

namespace BBIWARG.Input.InputProviding
{

    /// <summary>
    /// InputProvider provides the raw depth and confidence data through an event.
    /// </summary>
    public class InputProviderIisu : IInputProvider
    {
        /// <summary>
        /// data handle for the raw confidence data
        /// </summary>
        protected IDataHandle<Iisu.Data.IImageData> confidenceImage;

        /// <summary>
        /// data handle for the raw depth data
        /// </summary>
        protected IDataHandle<Iisu.Data.IImageData> depthImage;

        /// <summary>
        /// iisu device from which the data is read
        /// </summary>
        protected IDevice device;

        /// <summary>
        /// parameter handle for the horizontal field of view angle
        /// </summary>
        protected IParameterHandle<float> fieldOfViewHorizontal;

        /// <summary>
        /// parameter handle for the vertical field of view angle
        /// </summary>
        protected IParameterHandle<float> fieldOfViewVertical;

        /// <summary>
        /// parameter handle for the frame rate
        /// </summary>
        protected IParameterHandle<float> frameRate;

        /// <summary>
        /// iisu handle
        /// </summary>
        protected IHandle handle;

        /// <summary>
        /// parameter handle for the image height
        /// </summary>
        protected IParameterHandle<int> height;

        /// <summary>
        /// parameter handle for the image width
        /// </summary>
        protected IParameterHandle<int> width;

        /// <summary>
        /// the id of the current frame
        /// </summary>
        public virtual int CurrentFrameID { get { return device.FrameId; } }

        /// <summary>
        /// the horizontal field of view angle
        /// </summary>
        public float FieldOfViewHorizontal { get { return fieldOfViewHorizontal.Value; } }

        /// <summary>
        /// the vertical field of view angle
        /// </summary>
        public float FieldOfViewVertical { get { return fieldOfViewVertical.Value; } }

        /// <summary>
        /// the height of all images
        /// </summary>
        public int ImageHeight { get { return height.Value; } }

        /// <summary>
        /// the width of all images
        /// </summary>
        public int ImageWidth { get { return width.Value; } }

        /// <summary>
        /// true iff the input source provides data
        /// </summary>
        public bool IsActive { get; private set; }

        /// <summary>
        /// event that the device started
        /// </summary>
        public event DeviceStartedEventHandler DeviceStartedEvent;

        /// <summary>
        /// event that a new frame is available
        /// </summary>
        public event NewFrameEventHandler NewFrameEvent;

        /// <summary>
        /// Constructs an InputProvider.
        /// </summary>
        public InputProviderIisu()
        {
            IsActive = false;
        }

        /// <summary>
        /// Initializes to device and data handles.
        /// </summary>
        public void initialize()
        {
            createDevice();
            registerHandles();
        }

        /// <summary>
        /// Starts the device.
        /// </summary>
        public void start()
        {
            device.Start();
            IsActive = true;

            if (DeviceStartedEvent != null)
                DeviceStartedEvent(this, new EventArgs());

            run();
        }

        /// <summary>
        /// Stops the device.
        /// </summary>
        public void stop()
        {
            IsActive = false;
            device.Stop(true);
        }

        /// <summary>
        /// Creates an iisu device which provides the data.
        /// </summary>
        protected void createDevice()
        {
            handle = Iisu.Iisu.Context.CreateHandle();
            IDeviceConfiguration conf = createDeviceConfiguration();
            device = handle.InitializeDevice(conf);
        }

        /// <summary>
        /// Returns an iisu device configuration.
        /// </summary>
        /// <returns>iisu device configuration</returns>
        protected virtual IDeviceConfiguration createDeviceConfiguration()
        {
            IDeviceConfiguration conf = handle.CreateDeviceConfiguration();
            conf.IsAsynchronous = false;
            return conf;
        }

        /// <summary>
        /// Gets the next frame from the device.
        /// </summary>
        protected virtual void nextFrame()
        {
            device.UpdateFrame(true);
            provideNewFrame();
            device.ReleaseFrame();
        }

        /// <summary>
        /// Triggers the new frame event.
        /// </summary>
        protected void provideNewFrame()
        {
            if (NewFrameEvent != null)
            {
                Image<Gray, UInt16> rawDepthImage = new Image<Gray, UInt16>(ImageWidth, ImageHeight, ImageWidth * 2, depthImage.Value.Raw);
                Image<Gray, UInt16> confidenceImage2 = new Image<Gray, UInt16>(ImageWidth, ImageHeight, ImageWidth * 2, confidenceImage.Value.Raw);

                Image<Gray, byte> confidenceMask = confidenceImage2.ThresholdBinary(new Gray(Parameters.ConfidenceImageMinThreshold), new Gray(1)).Convert<Gray, byte>();

                rawDepthImage = rawDepthImage.Or((1 - confidenceMask).Convert<Gray, UInt16>().Mul(UInt16.MaxValue));

                NewFrameEvent(this, new NewFrameEventArgs(CurrentFrameID, rawDepthImage));
            }
        }

        /// <summary>
        /// Registers all parameter and data handles.
        /// </summary>
        protected virtual void registerHandles()
        {
            width = device.RegisterParameterHandle<int>("SOURCE.CAMERA.DEPTH.Width");
            height = device.RegisterParameterHandle<int>("SOURCE.CAMERA.DEPTH.Height");
            fieldOfViewHorizontal = device.RegisterParameterHandle<float>("SOURCE.CAMERA.DEPTH.HFOV");
            fieldOfViewVertical = device.RegisterParameterHandle<float>("SOURCE.CAMERA.DEPTH.VFOV");
            frameRate = device.RegisterParameterHandle<float>("SOURCE.FrameRate");

            depthImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.DEPTH.Image");
            confidenceImage = device.RegisterDataHandle<Iisu.Data.IImageData>("SOURCE.CAMERA.CONFIDENCE.Image");
        }

        /// <summary>
        /// Provides the main loop for reading data from the device.
        /// </summary>
        protected virtual void run()
        {
            while (IsActive)
                nextFrame();
        }
    }
}