using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using OpenNI;
using Kinect_viewer.Projection;
using NUnit.Framework;
using USBHIDDRIVER;

namespace Kinect_viewer
{
    public delegate void SetTextDeleg(Bitmap text);

    public partial class Form1 : Form
    {
        //Threads for depth and image projection
        private Thread imageThread;
        private Thread depthThread;
        // Bitmap for drawing images
        private Bitmap imageBitmap;
        private Bitmap depthBitmap;
        //Panels whitch draw bitmap on their background       
        private Panel2 panelImage;
        private Panel2 panelDepth;
        //Projection classes
        private Image_projection imageProjection;
        private Depth_projection depthProjection;
        //false if threads are running
        private bool stoppedThreadsAll = false;
       // private int[] hhh= new int[100000];
        
        #region Fields
        /// <summary>
        /// A context is a workspace where the application builds an OpenNI production graph.
        /// By way of analogy, this is just like a disk partition in which users build their folder trees.
        /// The context object is the entry point to OpenNI and holds the complete state of an application using OpenNI.
        /// This includes the application's entire production graph.
        /// </summary>
        private Context context;
        /// <summary>
        /// The ScriptNode object loads an XML script from a file or string, and then runs the XML script to build a production graph.
        /// This approach is useful for developers who wish to manipulate the ScriptNode directly.
        /// </summary>
        private ScriptNode scriptNode;
        /// <summary>
        ///  A DepthGenerator node generates a depth map. Each pixel value of a depth map represents a distance from the sensor.
        /// </summary>
        private DepthGenerator generatorDepth;
        /// <summary>
        /// An Image Generator node is a Map Generator that outputs image maps.
        /// It supports all Map Generator functions, and adds additional functions.
        /// Image maps may arrive in different pixel formats.
        /// </summary>
        private ImageGenerator generatorImage;
        /// <summary>
        /// Bitmap with deph data
        /// </summary>
        private Bitmap bitmapDepth;
        private Bitmap bitmapImage;
        /// <summary>
        /// Array data histogram for drawing depth data of the Kinect. The data in histogram means the value of color on the position.
        /// Note: histogram[1000]=255 => 1 meter = color value 255
        /// </summary>
        private int[] histogram;
        /// <summary>
        /// Path for default configuration of OpenNI. 
        /// If you want to use the library for using OpenNI library you must to have got installed OpenNI library.
        /// </summary>
        private readonly string SAMPLE_XML_FILE = @"C:\Program Files\OpenNI\Data\SamplesConfig.xml";
        /// <summary>
        /// If true -> process running ... it reads data from depth generator
        /// </summary>
        private bool shouldReadDepth=false;
        /// <summary>
        /// If true -> process running ... it reads data from image generator
        /// </summary>
        private bool shouldReadImage=false;
        /// <summary>
        /// Locking object
        /// </summary>
        public static object lockObject = new object();
        private DepthMetaData depthMD;
        private ImageMetaData imageMD;
        /// <summary>
        /// Data with the first histogram data.
        /// </summary>
        private int[] dataHistogram = new int[10000];
        /// <summary>
        /// Datas array for graph.
        /// </summary>
        private int[] rawHistogram = new int[280];
        
        #endregion
        #region Properties
        /// <summary>
        /// Gets or Sets state of the cycle in the thread of child classes.
        /// </summary>
        public bool ShouldRun
        {
            get { return shouldReadDepth; }
            set { shouldReadDepth = value; }
        }
        public Bitmap ImageBitmap
        {
            get
            {
                    return bitmapDepth;
            }
            set
            {
                    bitmapDepth = value;
            }
        }
        #endregion
        

        /// <summary>
        /// Constructor of the main form
        /// </summary>
        public Form1()
        {
            InitializeComponent();
            #region Problem
            imageProjection = new Image_projection(this);
            depthProjection = new Depth_projection(this);
            //New panel2 situated on the same location as panel1
            //Note: User can change panel2´s position by changing position of panel1 on the main form
            panelImage = new Panel2(this.panel1, this.groupBox1);
            //Event for redrawing background
            panelImage.Paint += new PaintEventHandler(p_Paint);
            panelDepth = new Panel2(this.panel2, this.groupBox1);
            panelDepth.Paint += new PaintEventHandler(p_Paint); 
            #endregion
            //Creating new context with the license saved in XML file in instalation folder of OpenNi
            this.context = Context.CreateFromXmlFile(SAMPLE_XML_FILE, out scriptNode);
            this.generatorDepth = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
            this.generatorImage = context.FindExistingNode(NodeType.Image) as ImageGenerator;
            //Here is problem of IR generator class ... new IRgenerator returns allways null ...
            // chceking if generators are not null
            if (this.generatorDepth == null)
            {
                throw new Exception("Depth generator must have a depth node!");
            }
            else if (this.generatorImage == null)
            {
                throw new Exception("Image generator must have a image node!");
            }
            //Note: New instance of IR generator returns allways null
            //else if (generatorIR == null)
            //{
            //throw new Exception("IR generator must have a IR node!");
            //}
            MapOutputMode imageMode = this.generatorImage.MapOutputMode;
            MapOutputMode mapMode = this.generatorDepth.MapOutputMode;
            bitmapDepth = new Bitmap((int)imageMode.XRes, (int)imageMode.YRes, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            bitmapImage = new Bitmap((int)imageMode.XRes, (int)imageMode.YRes, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
            //Creating new histogram ... array of 10 000 points -> 1 point == 1 mm
            this.histogram = new int[this.generatorDepth.DeviceMaxDepth];
            
        }
        /// <summary>
        /// Paint event of the Panel2 object
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void p_Paint(object sender, PaintEventArgs e)
        {
            lock (this)
            {
                Bitmap bit=null;
                int width = 0;
                int heigth = 0;
                {
                    if (sender == panelDepth)
                    {
                        bit = ImageBitmap;
                        //bit = bitmap;
                        width = panelDepth.Width;
                        heigth = panelDepth.Height;
                    }
                    else
                    {
                        bit = bitmapImage;
                        width = panelImage.Width;
                        heigth = panelImage.Height;
                    }
                    e.Graphics.DrawImage(bit,
                                        0,
                                        0,
                                        width,
                                        heigth);
                    
                }
            }
        }
        /// <summary>
        /// If the main form´s closing, stop all helping threads a close the aplication.
        /// </summary>
        /// <param name="e"></param>
        protected override void OnClosing(CancelEventArgs e)
        {
            imageProjection.ShouldRun = false;
            depthProjection.ShouldRun = false;
            this.depthThread.Abort();
            this.depthThread.Join();
            base.OnClosing(e);
        }
        /// <summary>
        /// Check if escape key was pressed
        /// </summary>
        /// <param name="e"></param>
        protected override void OnKeyPress(KeyPressEventArgs e)
        {
            if (e.KeyChar == 27)
            {
                this.Close();
            }
            base.OnKeyPress(e);
        }
        /// <summary>
        /// Sets data for drawing images
        /// </summary>
        /// <param name="bitt"></param>
        public void SetImageToDraw(Bitmap bitt)
        {
            lock (this)
            {
                bitmapDepth = bitt;
                panelImage.Invalidate(); 
            }
        }
        /// <summary>
        /// Sets data for drawing depth images
        /// </summary>
        /// <param name="bitt"></param>
        public void SetDepthDataToDraw(Bitmap bitt)
        {
            lock (this)
            {
                depthBitmap = bitt;
                panelDepth.Invalidate(); 
            }
        }
        
        //MENUStrip EVENTS--------
        /// <summary>
        /// Starts drawing images from generators
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void startToolStripMenuItem_Click(object sender, EventArgs e)
        {
                this.shouldReadDepth = true;
                //this.depthThread = new Thread(depthProjection.ReaderThread);
                this.depthThread = new Thread(ThreadDepth);
                this.depthThread.Priority = ThreadPriority.AboveNormal;
                this.depthThread.Name = "Depth data thread";
                this.depthThread.Start();
                this.shouldReadImage = true;
                this.imageThread = new Thread(this.ThreadImage);
                this.imageThread.Priority = ThreadPriority.Lowest;
                this.imageThread.Name = "Image data threat";
                this.imageThread.Start();
            stoppedThreadsAll = false;
            this.timer1.Enabled = true;
        }
        /// <summary>
        /// Stop drawing images from generators
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void stopToolStripMenuItem_Click(object sender, EventArgs e)
        {
            StopAllThreads();          
        }
        /// <summary>
        /// Close the main form
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void closeToolStripMenuItem_Click(object sender, EventArgs e)
        {//Check if all threads are stopped
            if (!stoppedThreadsAll)
            {
                StopAllThreads();
            }
            this.Close();
        }
        /// <summary>
        /// Methods stops all threads whitch´s used
        /// </summary>
        private void StopAllThreads()
        {
            //First image thread
            if (shouldReadImage == true)
            {
               // imageProjection.ShouldRun = false;
                shouldReadImage = false;
                imageThread.Abort();
                imageThread.Join();
            }
            //Next thread - depth tread
            if (shouldReadDepth == true)
            {
                //depthProjection.ShouldRun = false;
                shouldReadDepth = false;
                depthThread.Abort();
                depthThread.Join();
            }
            stoppedThreadsAll = true;
            this.timer1.Enabled = false;
        }

        //public void SetHist(int[] hist)
        //{
        //    lock (this)
        //    {
        //        hhh = hist; 
        //    }
        //}
        /// <summary>
        /// Method writes histogram data to the graph. It is generated by timer.
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void timer1_Tick(object sender, EventArgs e)
        {
            
            lock (lockObject)
            {
                //This converts all data of histogram to the graph. 
                //max value of histogram
                int maxvalue = 0;
                // 10 000 / 270 => 37
                int pocet = 0;
                int index=0;
                
                //Remove all data from array
                for (int i = 0; i < rawHistogram.Length; i++)
                {
                    rawHistogram[i] = 0;
                }

                for (int i = 0; i < dataHistogram.Length; i++)
                {//Check max value in range of 37 numbers
                    if (maxvalue<dataHistogram[i])
                    {
                        maxvalue = dataHistogram[i];
                    }
                    if (pocet==37)
                    {
                        rawHistogram[index++] =10- maxvalue / 25;
                        pocet = 0;
                        maxvalue = 0;
                    }
                    pocet++;
                }
                //novyHistogram[0] = histogram2[0] / 25;
                //novyHistogram[1] = histogram2[37] / 25;
                //novyHistogram[2] = histogram2[90] / 25;
                //novyHistogram[3] = histogram2[1000] / 25;
                //novyHistogram[4] = histogram2[1500] / 25;
                if (rawHistogram[0]==0&& rawHistogram[10]==0)
                {
                    this.graph1.Histogram = rawHistogram;  
                }
            }
        }
        #region Reading threads
        /// <summary>
        /// This reads all depth data a crates histogram
        /// </summary>
        public unsafe void ThreadDepth()
        {
            depthMD = new DepthMetaData();
            while (this.ShouldRun)
            {
                try
                {
                    this.context.WaitOneUpdateAll(generatorDepth);
                }
                catch (Exception e)
                {
                    throw e;
                }
                this.generatorDepth.GetMetaData(depthMD);
                CreateHistogramData(depthMD);
                lock (this)
                {
                    Rectangle rect = new Rectangle(0, 0, this.ImageBitmap.Width, this.ImageBitmap.Height);
                    BitmapData data = this.ImageBitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

                    //byte* pSrc = (byte*)image.ImageMapPtr.ToPointer();
                    ushort* pDepth = (ushort*)this.generatorDepth.DepthMapPtr.ToPointer();
                    for (int y = 0; y < depthMD.YRes; ++y)
                    {//Scan0 vrátí hodnotu prvního řádku a stride postupně načte všechny řádky
                        byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
                        // byte* pDest2 = (byte*)imageData.Scan0.ToPointer() + y * imageData.Stride;
                        for (int x = 0; x < depthMD.XRes; ++x, ++pDepth, pDest += 3)
                        {//Setting projection color
                            byte pixel = (byte)this.histogram[*pDepth];
                            pDest[0] = 0;//B
                            pDest[1] = pixel;//G
                            pDest[2] = pixel;//R
                        }
                    }
                    this.ImageBitmap.UnlockBits(data);
                }
                //Překreslení           
                this.panelDepth.Invalidate();
                Thread.Sleep(100);
            }
        }
        /// <summary>
        /// Creates histogram data and converting this to the values up to 256
        /// </summary>
        /// <param name="depthMD"></param>
        private unsafe void CreateHistogramData(DepthMetaData depthMD)
        {
            //Clear this histogram.
            for (int i = 0; i < this.histogram.Length; ++i)
            {
                this.histogram[i] = 0;
                //  this.hist[i] = 0;
                this.dataHistogram[i] = 0;
            }

            ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();

            int points = 0;
            for (int y = 0; y < depthMD.YRes; ++y)
            {
                for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
                {
                    ushort depthVal = *pDepth;
                    if (depthVal != 0)
                    {
                        this.histogram[depthVal]++;
                        //
                        // this.hist[depthVal]++;
                        //
                        points++;
                    }
                }
            }
            //((Form1)form).SetHist(histogram);
            for (int i = 1; i < this.histogram.Length; i++)
            {
                this.histogram[i] += this.histogram[i - 1];
            }
            dataHistogram = histogram;
            if (points > 0)
            {
                for (int i = 1; i < this.histogram.Length; i++)
                {
                    this.histogram[i] = (int)(256 * (1.0f - (this.histogram[i] / (float)points)));
                }
            }
        }
        /// <summary>
        /// Thread for reading RGB images from Kinect
        /// </summary>
        public unsafe void ThreadImage()
        {
            imageMD = new ImageMetaData();
            while (this.shouldReadImage)
            {
                try
                {
                    this.context.WaitOneUpdateAll(generatorImage);
                }
                catch (Exception e)
                {
                    throw e;
                }
                this.generatorImage.GetMetaData(imageMD);
lock (this)
{
    Rectangle imageRect = new Rectangle(0, 0, bitmapImage.Width, bitmapImage.Height);
    BitmapData imageData = bitmapImage.LockBits(imageRect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
    //Pointer pointing on data from ImageGenerator class
    byte* pSrc = (byte*)generatorImage.ImageMapPtr.ToPointer();
    for (int y = 0; y < imageMD.YRes; ++y)
    {//Scan0 vrátí hodnotu prvního řádku a stride postupně načte všechny řádky      
        byte* pDest2 = (byte*)imageData.Scan0.ToPointer() + y * imageData.Stride;
        for (int x = 0; x < imageMD.XRes; ++x, pDest2 += 3, pSrc += 3)
        {//Setting projection color
            //Image 
            pDest2[0] = pSrc[2];
            pDest2[1] = pSrc[1];
            pDest2[2] = pSrc[0];
        }
    }
    this.bitmapImage.UnlockBits(imageData);
}
                this.panelImage.Invalidate();
                Thread.Sleep(100);
            }
        } 
        #endregion        
    }
}