using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;

namespace hands_viewer.cs
{
    class HandsRecognition
    {
        byte[] LUT;
        private MainForm form;
        private bool _disconnected = false;
        //Queue containing depth image - for synchronization purposes
        private Queue<PXCMImage> m_images;
        private const int NumberOfFramesToDelay = 3;
        private int _framesCounter = 0;
        private float _maxRange;

        public HandsRecognition(MainForm form)
        {
            m_images = new Queue<PXCMImage>();
            this.form = form;
            LUT = Enumerable.Repeat((byte)0, 256).ToArray();
            LUT[255] = 1;
        }

        /* Checking if sensor device connect or not */
        private bool DisplayDeviceConnection(bool state)
        {
            if (state)
            {
                if (!_disconnected) form.UpdateStatus("Device Disconnected");
                _disconnected = true;
            }
            else
            {
                if (_disconnected) form.UpdateStatus("Device Reconnected");
                _disconnected = false;
            }
            return _disconnected;
        }

        /* Displaying current frame gestures */
        private void DisplayGesture(PXCMHandData handAnalysis)
        {
            PXCMHandData.GestureData gestureData;
            if (handAnalysis.QueryFiredGestureData(0, out gestureData) != pxcmStatus.PXCM_STATUS_NO_ERROR)
                return;

            PXCMHandData.IHand handData;
            if (handAnalysis.QueryHandDataById(gestureData.handId, out handData) != pxcmStatus.PXCM_STATUS_NO_ERROR)
                return;

            form.DisplayGestures(gestureData, (uint)handData.QueryBodySide());
        }

        /* Displaying Depth/Mask Images - for depth image only we use a delay of NumberOfFramesToDelay to sync image with tracking */
        private unsafe void DisplayPicture(PXCMImage depth, PXCMHandData handAnalysis)
        {
            if (depth == null)
                return;

            PXCMImage image = depth;

            //Mask Image
            if (form.GetLabelmapState())
            {

                Bitmap picture = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);


                for (int j = 0; j < handAnalysis.QueryNumberOfHands(); j++)
                {
                    int id;
                    PXCMImage.ImageData data;

                    handAnalysis.QueryHandId(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out id);
                    //Get hand by time of appearance
                    PXCMHandData.IHand handData;
                    handAnalysis.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, j, out handData);
                    if (handData != null &&
                        (handData.QuerySegmentationImage(out image) >= pxcmStatus.PXCM_STATUS_NO_ERROR))
                    {
                        if (image.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_Y8,
                            out data) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
                        {
                            Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);

                            BitmapData bitmapdata = picture.LockBits(rect, ImageLockMode.ReadWrite, picture.PixelFormat);
                            byte* numPtr = (byte*) bitmapdata.Scan0; //dst
                            byte* numPtr2 = (byte*) data.planes[0]; //row
                            int imagesize = image.info.width*image.info.height;
                            byte num2 = (byte) handData.QueryBodySide();

                            byte tmp = 0;
                            for (int i = 0; i < imagesize; i++, numPtr += 4, numPtr2++)
                            {
                                tmp = (byte) (LUT[numPtr2[0]]*num2*100);
                                numPtr[0] = (Byte) (tmp | numPtr[0]);
                                numPtr[1] = (Byte) (tmp | numPtr[1]);
                                numPtr[2] = (Byte) (tmp | numPtr[2]);
                                numPtr[3] = 0xff;
                            }

                            picture.UnlockBits(bitmapdata);
                            image.ReleaseAccess(data);
                        }
                    }
                }

                form.DisplayBitmap(picture);
                picture.Dispose();

            }
            //Depth Image
            else
            {
                //collecting 3 images inside a queue and displaying the oldest image
                PXCMImage.ImageInfo info;
                PXCMImage image2;

                info = image.QueryInfo();
                image2 = form.g_session.CreateImage(info);
                image2.CopyImage(image);
                m_images.Enqueue(image2);
                if (m_images.Count == NumberOfFramesToDelay)
                {
                    Bitmap picture = new Bitmap(image.info.width, image.info.height, PixelFormat.Format32bppRgb);
                    PXCMImage.ImageData data3;
                    PXCMImage image3 = m_images.Dequeue();
                    if (image3.AcquireAccess(PXCMImage.Access.ACCESS_READ, PXCMImage.PixelFormat.PIXEL_FORMAT_DEPTH, out data3) >= pxcmStatus.PXCM_STATUS_NO_ERROR)
                    {
                        float fMaxValue = _maxRange;
                        byte cVal;

                        Rectangle rect = new Rectangle(0, 0, image.info.width, image.info.height);
                        BitmapData bitmapdata = picture.LockBits(rect, ImageLockMode.ReadWrite, picture.PixelFormat);

                        byte* pDst = (byte*)bitmapdata.Scan0;
                        short* pSrc = (short*)data3.planes[0];
                        int size = image.info.width * image.info.height;

                        for (int i = 0; i < size; i++, pSrc++, pDst += 4)
                        {
                            cVal = (byte)((*pSrc) / fMaxValue * 255);
                            if (cVal != 0)
                                cVal = (byte)(255 - cVal);

                            pDst[0] = cVal;
                            pDst[1] = cVal;
                            pDst[2] = cVal;
                            pDst[3] = 255;
                        }

                        picture.UnlockBits(bitmapdata);
                        form.DisplayBitmap(picture);
                        image3.ReleaseAccess(data3);
                    }
                    image3.Dispose();
                    picture.Dispose();
                }
            }
        }



        /* Displaying current frames hand joints */
        private void DisplayJoints(PXCMHandData handOutput)
        {
            if (form.GetJointsState() || form.GetSkeletonState())
            {
                //Iterate hands
                PXCMHandData.JointData[][] nodes = new PXCMHandData.JointData[][] { new PXCMHandData.JointData[0x20], new PXCMHandData.JointData[0x20] };
                int numOfHands = handOutput.QueryNumberOfHands();
                for (int i = 0; i < numOfHands; i++)
                {
                    //Get hand by time of appearence
                    //PXCMHandAnalysis.HandData handData = new PXCMHandAnalysis.HandData();
                    PXCMHandData.IHand handData;
                    if (handOutput.QueryHandData(PXCMHandData.AccessOrderType.ACCESS_ORDER_BY_TIME, i, out handData) == pxcmStatus.PXCM_STATUS_NO_ERROR)
                    {
                        if (handData != null)
                        {
                            //Iterate Joints
                            for (int j = 0; j < 0x20; j++)
                            {
                                PXCMHandData.JointData jointData;
                                handData.QueryTrackedJoint((PXCMHandData.JointType)j, out jointData);
                                nodes[i][j] = jointData;
                            }
                        }
                    }

                }
                form.DisplayJoints(nodes, numOfHands);

            }
            else
            {
                form.DisplayJoints(null, 0);
            }
        }

        /* Displaying current frame alerts */
        private void DisplayAlerts(PXCMHandData handAnalysis)
        {
            bool isChanged = false;
            string sAlert = "Last Alert: ";
            for (int i = 0; i < handAnalysis.QueryFiredAlertsNumber(); i++)
            {
                PXCMHandData.AlertData alertData;
                if (handAnalysis.QueryFiredAlertData(i, out alertData) != pxcmStatus.PXCM_STATUS_NO_ERROR)
                    continue;

                //Displaying last alert - see PXCMHandAnalysis.AlertData.AlertType for all available alerts
                switch (alertData.label)
                {
                    case PXCMHandData.AlertType.ALERT_HAND_DETECTED:
                        {
                            
                            sAlert += "Hand Detected, ";
                            isChanged = true;
                            break;
                        }
                    case PXCMHandData.AlertType.ALERT_HAND_NOT_DETECTED:
                        {
                           
                            sAlert += "Hand Not Detected, ";
                            isChanged = true;
                            break;
                        }
                    case PXCMHandData.AlertType.ALERT_HAND_CALIBRATED:
                        {
                          
                            sAlert += "Hand Calibrated, ";
                            isChanged = true;
                            break;
                        }
                    case PXCMHandData.AlertType.ALERT_HAND_NOT_CALIBRATED:
                        {
                          
                            sAlert += "Hand Not Calibrated, ";
                            isChanged = true;
                            break;
                        }
                    case PXCMHandData.AlertType.ALERT_HAND_INSIDE_BORDERS:
                        {
                          
                            sAlert += "Hand Inside Border, ";
                            isChanged = true;
                            break;
                        }
                    case PXCMHandData.AlertType.ALERT_HAND_OUT_OF_BORDERS:
                        {
                           
                            sAlert += "Hand Out Of Borders, ";
                            isChanged = true;
                            break;
                        }
                }
            }
            if (isChanged == true)
            {
                form.UpdateStatus(sAlert);
            }
                
          
        }

        public static pxcmStatus OnNewFrame(Int32 mid, PXCMBase module, PXCMCapture.Sample sample)
        {
            return pxcmStatus.PXCM_STATUS_NO_ERROR;
        }



        /* Using PXCMSenseManager to handle data */
        public void SimplePipeline()
        {
            int framesToRecord = -1;
            bool flag = true;
            PXCMSenseManager instance = null;
            _disconnected = false;
            instance = form.g_session.CreateSenseManager();
            if (instance == null)
            {
                form.UpdateStatus("Failed creating SenseManager");
                return;
            }

            if (form.GetRecordState())
            {
                instance.captureManager.SetFileName(form.GetFileName(), true);
                PXCMCapture.DeviceInfo info;
                if (form.Devices.TryGetValue(form.GetCheckedDevice(), out info))
                {
                    instance.captureManager.FilterByDeviceInfo(info);
                }

                framesToRecord = form.GetNumberOfFramesToRecord();
            }
            else if (form.GetPlaybackState())
            {
                instance.captureManager.SetFileName(form.GetFileName(), false);
                instance.captureManager.SetRealtime(false);
            }
            else
            {
                PXCMCapture.DeviceInfo info;
                if (String.IsNullOrEmpty(form.GetCheckedDevice()))
                {
                    form.UpdateStatus("Device Failure");
                    return;
                }

                if (form.Devices.TryGetValue(form.GetCheckedDevice(), out info))
                {
                    instance.captureManager.FilterByDeviceInfo(info);
                }
            }
            /* Set Module */
            pxcmStatus status = instance.EnableHand(form.GetCheckedModule());
            PXCMHandModule handAnalysis = instance.QueryHand();

            if (status != pxcmStatus.PXCM_STATUS_NO_ERROR || handAnalysis == null)
            {
                form.UpdateStatus("Failed Loading Module");
            }

            PXCMSenseManager.Handler handler = new PXCMSenseManager.Handler();
            handler.onModuleProcessedFrame = new PXCMSenseManager.Handler.OnModuleProcessedFrameDelegate(OnNewFrame);

            FPSTimer timer = new FPSTimer(form);
            form.UpdateStatus("Init Started");
            if (handAnalysis != null && instance.Init(handler) == pxcmStatus.PXCM_STATUS_NO_ERROR)
            {

                PXCMHandConfiguration handConfiguration = handAnalysis.CreateActiveConfiguration();
                PXCMHandData handData = handAnalysis.CreateOutput();


                PXCMCapture.DeviceInfo dinfo;
                instance.QueryCaptureManager().QueryDevice().QueryDeviceInfo(out dinfo);
                if (dinfo != null && dinfo.model == PXCMCapture.DeviceModel.DEVICE_MODEL_IVCAM)
                {
                    instance.captureManager.device.SetDepthConfidenceThreshold(1);
                    instance.captureManager.device.SetMirrorMode(PXCMCapture.Device.MirrorMode.MIRROR_MODE_DISABLED);
                    instance.captureManager.device.SetIVCAMFilterOption(6);
                }

                _maxRange = instance.captureManager.device.QueryDepthSensorRange().max;


                if (handConfiguration != null)
                {
                    handConfiguration.EnableAllGestures();
                    handConfiguration.EnableAllAlerts();
                    handConfiguration.EnableSegmentationImage(true);

                    handConfiguration.ApplyChanges();
                    handConfiguration.Update();
                }

                form.UpdateStatus("Streaming");
                while (!form.stop && _framesCounter != framesToRecord)
                {
                    if (instance.AcquireFrame(true) < pxcmStatus.PXCM_STATUS_NO_ERROR)
                    {
                        break;
                    }

                    if (!DisplayDeviceConnection(!instance.IsConnected()))
                    {

                        if (handData != null)
                        {
                            handData.Update();
                        }

                        PXCMCapture.Sample sample = instance.QueryHandSample();
                        if (sample !=null && sample.depth != null)
                        {
                            DisplayPicture(sample.depth, handData);

                            if (handData != null)
                            {
                                DisplayJoints(handData);
                                DisplayGesture(handData);
                                DisplayAlerts(handData);
                            }
                            form.UpdatePanel();
                        }
                        timer.Tick();
                        if (form.GetRecordState())
                            form.setNumberOfFrames(++_framesCounter);
                    }
                    instance.ReleaseFrame();
                }

                // Clean Up
                if (handData != null) handData.Dispose();
                if (handConfiguration != null) handConfiguration.Dispose();
            }
            else
            {
                form.UpdateStatus("Init Failed");
                flag = false;
            }
            foreach (PXCMImage pxcmImage in m_images)
            {
                pxcmImage.Dispose();
            }
            instance.Close();
            instance.Dispose();
            if (flag)
            {
                form.UpdateStatus("Stopped");
            }
        }
    }
}
