#include "opencv2/opencv.hpp"
#include <iostream>
#include <vector>


using namespace cv;
using namespace std;
int main(int ac, char ** av)
{
  cv::Mat frame;
  Mat back;
  Mat frame_bg;
  cv::VideoCapture capture(0);
  cv::VideoWriter outputVideo;

  if (ac < 1)
    return 1;

  if (ac == 3){
      back = imread(av[2], 1);
      //namedWindow("lala");
      //imshow("lala", back);

  }
   capture.open(av[1]);
  if (!capture.isOpened())
  {
    std::cout << "capture device or file " << av[1] << " failed to open!" << std::endl;
    return 1;
  }


  cv::Ptr<cv::BackgroundSubtractor> bg1 = cv::createBackgroundSubtractorMOG2(100,20,false);
  //cv::Ptr<cv::BackgroundSubtractor> bg2 = cv::createBackgroundSubtractorMOG2(200,20,true);
  cv::Ptr<cv::BackgroundSubtractor> bg3 = cv::createBackgroundSubtractorMOG2(500,20,false);
  //cv::Ptr<cv::BackgroundSubtractor> bg4 = cv::createBackgroundSubtractorMOG2(250,16,true);

  vector<vector<Point>> contours1;
  vector<vector<Point>> contours2;
  vector<vector<Point>> contours3;
  vector<vector<Point>> contours4;

  cv::Size S = cv::Size((int) capture.get(CV_CAP_PROP_FRAME_WIDTH),
      (int) capture.get(CV_CAP_PROP_FRAME_HEIGHT));
  //int ex = static_cast<int>(capture.get(CV_CAP_PROP_FOURCC));
  //outputVideo.open("output.mpg", ex, capture.get(CV_CAP_PROP_FPS), S, true);
  outputVideo.open("output.mpg", CV_FOURCC('F','M','P','4'), capture.get(CV_CAP_PROP_FPS), S, true);
  namedWindow("Frame_damian");


  namedWindow("bq1");
 // namedWindow("bq2");
  namedWindow("bq3");
  //namedWindow("bq4");
  namedWindow("bq1-1");
  //namedWindow("bq2-1");
  namedWindow("bq3-1");
  //namedWindow("bq4-1");
   float prog = 0.0001;
     int time = 0;

        bg1->getBackgroundImage(back);
  // bg2->getBackgroundImage(back);
   bg3->getBackgroundImage(back);
  // bg4->getBackgroundImage(back);
  for (;;) {
    capture >> frame;
    imshow("Frame_damian", frame);

    outputVideo.write(frame);

       int erosion_size = 6;
       Mat element = getStructuringElement(cv::MORPH_CROSS,
              cv::Size(4 * erosion_size + 1, 2 * erosion_size + 1),
              cv::Point(erosion_size, erosion_size) );
       Mat erod;


    Mat fore;
    Mat dummy;
//if((time % 2 == 0) && time)
{
//     bg7->apply(frame, fore, -0.5);
//     bg7->getBackgroundImage(back);    //erode(fore,fore,Mat::ones(4,4,CV_8U));
    //dilate(fore,fore,Mat::ones(5,5,CV_8U));
//     imshow("bq7,250,20,false",fore);
//     time = 1;

//}else{
  //  bg7->apply(back, dummy, 1.0);
  //  bg7->getBackgroundImage(back);
  //  time++;
}




   // erode(frame_bg,fore,Mat::ones(6,6,CV_8U));
    //dilate(fore,fore,Mat::ones(9,9,CV_8U));

//     erode(frame_bg,fore,Mat::ones(5,5,CV_8U));
//     dilate(fore,fore,Mat::ones(4,4,CV_8U));


float m =4 ;
 float n = 5;
int T = 1;
vector<Vec4i> hierarchy;
Mat drawing = Mat::zeros( frame.size(), CV_8UC3 );
 Mat copy;
    bg1->apply(frame, frame_bg, prog);
    erode(frame_bg,fore,Mat::ones(m,m,CV_8U));
    dilate(fore, frame_bg,Mat::ones(n,n,CV_8U));
    copy = frame_bg;
    imshow("bq1-1", copy);
   // findContours(frame_bg, contours1, CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);
    findContours( frame_bg, contours1, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );
        for(int i = 1; i < (int)contours1.size();){
            cout<<contours1[i].size()<<endl;
        if((int)contours1[i].size()<20){
         contours1.erase(contours1.begin() + i);
        }else{
         i++;
        }
    }
      for(int i = 1; i < (int)contours1.size();i++){
            cout<<contours1[i].size()<<" Po"<<endl;
    }
    for( size_t i = 0; i< contours1.size(); i++ )
     {
       Scalar color = Scalar( 255, 255, 255);
       drawContours( drawing, contours1, (int)i, color, 2, 8, hierarchy, 0, Point() );
     }
     imshow("draw", drawing);
    //cout<<contours1.size()<<"      A"<<endl;

 Mat canny_output;
  vector<vector<Point> > contours;
 // vector<Vec4i> hierarchy;
  int thresh = 100;
  int max_thresh = 255;
  RNG rng(12345);


    Mat obraz;
   // for(int i = 0; i< contours1.size(); i++){
   //  Scalar color(255,255,255);
   //  drawContours(obraz,contours1, i, color, FILLED,8);
   // }
   //drawContours(obraz, contours1, -1, Scalar(0,0,255),4);
    //imshow("bq1", obraz);
    imshow("bq1", frame_bg);


    /*bg2->apply(frame, frame_bg, prog);
    erode(frame_bg,fore,Mat::ones(m,m,CV_8U));
    dilate(fore,frame_bg,Mat::ones(n,n,CV_8U));
    copy = frame_bg;
    imshow("bq2-2", copy);
    findContours(frame_bg, contours2, CV_RETR_EXTERNAL,CV_CHAIN_APPROX_NONE);
    cout<<contours2.size()<<"      B"<<endl;

    imshow("bq2", frame_bg);*/

    bg3->apply(frame, frame_bg, prog);
    erode(frame_bg,fore,Mat::ones(m,m,CV_8U));
    dilate(fore,frame_bg,Mat::ones(n,n,CV_8U));
    copy = frame_bg;
    imshow("bq3-2", copy);
    findContours(frame_bg, contours3, CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
  //  cout<<contours3.size()<<"      C"<<endl;
    imshow("bq3", frame_bg);

    /*
    bg4->apply(frame, frame_bg, prog);
    erode(frame_bg,fore,Mat::ones(m,m,CV_8U));
    dilate(fore,frame_bg,Mat::ones(n,n,CV_8U));
    copy = frame_bg;
    imshow("bq4-2", copy);
    findContours(frame_bg, contours4, CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE);
    cout<<contours4.size()<<"      D"<<endl;
    imshow("bq4", frame_bg);
    */

    drawContours(frame, contours1, -1, Scalar(0,0,255),2);
    char key = (char)cv::waitKey(20);
    switch (key)  {
      case 'w':
        imwrite("background.jpg", frame);
        break;
      case 27:
      case 'q':
        return 0;
        break;
    }

  }

  return 0;
}