context: /*Automated Rodent Tracker - A program to automatically track rodents
  Copyright(C) 2015 Brett Michael Hewitt
  
  This program is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with this program.If not, see<http://www.gnu.org/licenses/>.*/
  
  using System;
  using System.Collections.Generic;
  using System.Drawing;
  using System.Linq;
  using System.Text;
  using System.Threading.Tasks;
  using System.Windows;
  using AutomatedRodentTracker.Model;
  using AutomatedRodentTracker.Model.Resolver;
  using Emgu.CV;
  using Emgu.CV.CvEnum;
  using Emgu.CV.Structure;
  using Emgu.CV.Util;
  using AutomatedRodentTracker.ModelInterface.Motion.BackgroundSubtraction;
  using AutomatedRodentTracker.ModelInterface.Motion.MotionBackground;
  using AutomatedRodentTracker.ModelInterface.Video;
  using AutomatedRodentTracker.Services.ImageToBitmapSource;
  using Point = System.Drawing.Point;
  
  namespace AutomatedRodentTracker.Model.Motion.MotionBackground
  {
      internal class MotionBackground : ModelObjectBase, IMotionBackground
      {
          private int m_MotionLength;
          private IVideo m_Video;
          private double m_CannyThreshold1 = 120;
          private double m_CannyThreshold2 = 180;
  
          public IVideo Video
          {
              get
              {
                  return m_Video;
              }
              set
              {
                  if (Equals(m_Video, value))
                  {
                      return;
                  }
  
                  m_Video = value;
  
                  MarkAsDirty();
              }
          }
  
          public int MotionLength
          {
              get
              {
                  return m_MotionLength;
                  ;
              }
              set
              {
                  if (Equals(m_MotionLength, value))
                  {
                      return;
                  }
  
                  m_MotionLength = value;
  
                  MarkAsDirty();
              }
          }
  
          public void GenerateMotionBackground(double threshold, out Image<Gray, Byte> binaryBackground, Rectangle roi, int startFrame = 0)
          {
              if (Video == null || Video.FrameCount - startFrame <= MotionLength)
              {
                  throw new Exception("Video can not be null and motion length must be over " + MotionLength);
              }
  
              Mat[] motionImages = new Mat[MotionLength];
  
              IMotionBackgroundSubtraction motion = ModelResolver.Resolve<IMotionBackgroundSubtraction>();
              motion.UpdateDetector(Video.FrameCount, 16, false);
  
              Video.SetFrame(startFrame);
              for (int i = 0; i < MotionLength; i++)
              {
                  motionImages[i] = Video.GetFrameMat();
  
                  if (roi.IsEmpty)
                  {
                      motion.ProcessFrame(motionImages[i]);
                  }
                  else
                  {
                      using (Image<Gray, Byte> tempImage = motionImages[i].ToImage<Gray, Byte>(true))
                      using (Image<Gray, Byte> roiImage = tempImage.GetSubRect(roi))
                      {
                          motion.ProcessFrame(roiImage);
                      }
                  }
              }
              
              Image<Gray, Byte> original = motionImages[MotionLength - 1].ToImage<Gray, Byte>(true);
              original.ROI = roi;
              
  
              //Image<Gray, Byte> tempOriginal = original.CopyBlank();
              //double otsuThreshold = CvInvoke.Threshold(original, tempOriginal, 20, 255, ThresholdType.Otsu | ThresholdType.Binary);
              Image<Gray, Byte> originBinary = original.ThresholdBinary(new Gray(threshold), new Gray(255));
  
              using (Mat motionMat = motion.ForegroundMask)
              using (Image<Gray, Byte> motionImage = motionMat.ToImage<Gray, Byte>())
              using (Image<Gray, Byte> filteredImage = motionImage.SmoothMedian(3))
              {
                  //motionImage.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\011 - Final.png");
                  //Hough lines
                  using (Mat canneyEdges = new Mat())
                  {
                      CvInvoke.Canny(filteredImage, canneyEdges, m_CannyThreshold1, m_CannyThreshold2);
                      //filteredImage.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\010 - Final.png");
                      //canneyEdges.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\09 - Final.png");
                      LineSegment2D[] lines = CvInvoke.HoughLinesP(canneyEdges,
                     1, //Distance resolution in pixel-related units
                     Math.PI / 45, //Angle resolution measured in radians.
                     20, //threshold
                     5, //min Line width
                     40); //gap between lines
  
                      using (VectorOfVectorOfPoint apxContour = new VectorOfVectorOfPoint())
                      {
                          using (Image<Gray, Byte> test1 = new Image<Gray, byte>(original.Width, original.Height, new Gray(0)))
                          using (Image<Gray, Byte> test3 = test1.Clone())
                          {
                              foreach (var line in lines)
                              {
                                  test1.Draw(line, new Gray(255), 1);
                              }
                              //test1.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\08 - Final.png");
                              CvInvoke.FindContours(test1, apxContour, null, RetrType.External, ChainApproxMethod.ChainApproxSimple);
  
                              List<Point> pointsF = new List<Point>();
  
                              for (int i = 0; i < apxContour.Size; i++)
                              {
                                  CvInvoke.DrawContours(test3, apxContour, i, new MCvScalar(255), 4);
                                  pointsF.AddRange(apxContour[i].ToArray());
                              }
                              //test3.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\07 - Final.png");
                              PointF[] finalPoints = pointsF.Select(x => new PointF(x.X, x.Y)).ToArray();
                              PointF[] convexHull = CvInvoke.ConvexHull(finalPoints, true);
                              Point[] finalfinalPoints = convexHull.Select(x => new Point((int)x.X, (int)x.Y)).ToArray();
  
                              test3.Draw(finalfinalPoints, new Gray(255), 4);
                              //using (Image<Bgr, Byte> temp = motionImages[0].ToImage<Bgr, Byte>())
                              ////using (Image<Gray, Byte> temp2 = new Image<Gray, byte>(temp.Size))
                              //{
                              //    //temp2.SetValue(0);
                              //    //temp2.Draw(finalfinalPoints, new Gray(255), -1);
  
                              //    using (Image<Gray, Byte> temp3 = originBinary.Clone())
                              //    {
                              //        temp3.Draw(finalfinalPoints, new Gray(255), -1);
                              //        temp3.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\06c-Final.png");
                              //    }
                              //using (Image<Bgr, Byte> temp = original.Convert<Bgr, Byte>())
                              //{
                              //    temp.Draw(finalfinalPoints, new Bgr(Color.Red), 3);
                              //    temp.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\06d-Final.png");
                              //}
                              //}
                              //test3.Save(@"C:\Users\10488835\Desktop\PhD\Papers\Software Practise and Experience\Latex\images\BackgroundSub\06 - Final.png");
                              byte[,,] imageData = originBinary.Data;
  
                              Point[] currentContour = convexHull.Select(x => new Point((int)x.X, (int)x.Y)).ToArray();
                              List<Point> currentContourPoints = new List<Point>(currentContour);
  
                              List<List<Point>> allPoints = new List<List<Point>>();
                              allPoints.Add(new List<Point>());
                              allPoints[0].Add(currentContourPoints[0]);
                              int allPointsIndex = 0;
                              bool blackFlag = false;
  
                              for (int j = 1; j < currentContourPoints.Count; j++)
                              {
                                  Point firstPoint = currentContourPoints[j - 1];
                                  Point secondPoint = currentContourPoints[j];
  
                                  //bool go2 = firstPoint.X == 441 && firstPoint.Y == 356 && secondPoint.X == 794 && secondPoint.Y == 771;
  
                                  List<Point> pixelPoints = GetBresenhamLine(firstPoint, secondPoint);
  
                                  for (int i = 0; i < pixelPoints.Count; i++)
                                  {
                                      Point pixel = pixelPoints[i];
                                      Byte currentPixel = imageData[pixel.Y, pixel.X, 0];
  
                                      if (currentPixel == 0)
                                      {
                                          //It's black
                                          if (!blackFlag && j != 0 && i != 0)
                                          {
                                              allPoints[allPointsIndex].Add(new Point(pixel.X, pixel.Y));
                                              allPoints.Add(new List<Point>());
                                              allPointsIndex++;
                                          }
  
                                          blackFlag = true;
                                      }
                                      else
                                      {
                                          if (blackFlag)
                                          {
                                              //We've gone from black to white
                                              allPoints[allPointsIndex].Add(new Point(pixel.X, pixel.Y));
                                              blackFlag = false;
                                          }
                                      }
                                  }
  
                                  if (!blackFlag)
                                  {
                                      allPoints[allPointsIndex].Add(secondPoint);
                                  }
                              }
  
                              //First contour dealt with
                              List<List<Point>> tempList = new List<List<Point>>();
                              for (int j = 0; j < allPoints.Count; j++)
                              {
                                  List<Point> list = allPoints[j];
                                  if (list.Count > 1)
                                  {
                                      tempList.Add(list);
                                  }
                              }
  
                              List<List<Point>> finalWhitePoints = new List<List<Point>>();
                              for (int j = 0; j < tempList.Count; j++)
                              {
  
gt:                                 List<Point> list = tempList[j];