#include "OCVCapture.h"

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <unistd.h>

using namespace cv;
using namespace std;

int main(int argc, char** argv)
{
	OCVCapture capture;
    capture.deviceID("/dev/video1");
	capture.setDesiredSize(640,480);
	capture.setFramerate(60);
	capture.setPixelFormat("MJPEG");
	//capture.setVerbose(true);
	capture.open();

	if (!capture.isOpen())
	{
		cerr << "Failed to open camera" << endl;
		return -1;
	}

	cout << "Capture " << capture.width() << " x " << capture.height();
	cout << " pixels at " << capture.frameRate() << " fps" << endl;

	// The first several frames tend to come out black.
	for (int i = 0; i < 20; ++i)
	{
		capture.grab();
		usleep(1000);
	}

	namedWindow("RGB", CV_WINDOW_AUTOSIZE);

	Mat rgb;

	bool done = false;
	bool paused = false;

    int i=0;
	while (!done)
	{
	i++;
		if (!paused)
			done = !capture.grab();

		if (!done)
		{
			capture.rgb(rgb);
			//imshow("RGB", rgb);

			if (i % 100 == 0){
                char buf[50];
                sprintf(buf,"test%3d.jpg",i);
                imwrite(buf,rgb);
			}

			cout << "." << flush;
		}

		if (!done)
		{
			int key = waitKey(10);
			switch (key)
			{
				case 27:
					done = true;
					break;
				case ' ':
					paused = !paused;
					break;
			}
		}
	}

	capture.close();

	return 0;
}