#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> bg3 = cv::createBackgroundSubtractorMOG2(500,20,false);

  vector<vector<Point> > contours1;
  vector<vector<Point> > contours3;

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


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

   bg1->getBackgroundImage(back);
   bg3->getBackgroundImage(back);
  for (;;) {
    capture >> frame;
    imshow("Frame_damian", frame);
    Mat frame2 = 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;

float m =4 ;
 float n = 7;
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, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE, Point(0, 0) );
        for(int i = 1; i < (int)contours1.size();){
            cout<<contours1[i].size()<<endl;
        if((int)contours1[i].size()<60){
         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);
     vector<Point> > wektorŚrodka =
    //cout<<contours1.size()<<"      A"<<endl;
    for( size_t i = 0; i< contours1.size(); i++ ){
        int minx= 100000, miny= 100000, maxx= 0, maxy = 0;
        for( size_t j = 0; j< contours1[i].size(); j++ ){

            if(contours1[i][j].x > maxx){
            // cout<<"ello";
                maxx = contours1[i][j].x;
            }
             if(contours1[i][j].x < minx){
            // cout<<"ello";
                minx = contours1[i][j].x;
            }
             if(contours1[i][j].y > maxy){
            // cout<<"ello";
                maxy = contours1[i][j].y;
            }
             if(contours1[i][j].y < miny){
            // cout<<"ello";
                miny = contours1[i][j].y;
            }
        }
        Scalar red = Scalar( 0, 0,255);
        Point pointTopLeft = Point(minx, maxy);
            Point pointBottonRight = Point(maxx, miny);
        cout << "Punkt " << i << "(" << minx << ","<< miny<<endl;
        rectangle(frame2, pointTopLeft, pointBottonRight, red, 4);
        imshow("frame2", frame2);
    }



 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);



    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);



    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;
}