#include <iostream>
#include <opencv2/opencv.hpp>
#include "wiringPi.h"
#include "raspicam_cv.h"
#define CAM_ID 0
#define PIN_0 0
using namespace std;
using namespace cv;
using namespace raspicam;

int main()
{
    cout << "Testovani kamery ..." << endl;
    VideoCapture cam_web(CAM_ID);   // Web kamera
    RaspiCam_Cv cam_rpi;            // RPi kamera
    // Zmena rozliseni u RPi kamera na 640x480 px
    cam_rpi.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    cam_rpi.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
    cam_rpi.open();
    // V pripade, ze kamera nebyla pipojena zobraz hlaseni
    // a ukonci program. Potom je potreba zmenit cislo kamery CAM_ID
    if (!cam_web.isOpened())
    {
        cout << "Web kamera nebyla pripojena." << endl;
        return -1;
    }
    if (!cam_rpi.isOpened())
    {
        cout << "RPi kamera nebyla pripojena." << endl;
        return -2;
    }

    wiringPiSetup() ;          // Setup pro GPIO piny
    pinMode(PIN_0, INPUT) ;    // Nastaveni pinu 0 jako vstupniho
    pullUpDnControl(PIN_0, PUD_UP); // Pull up
    // Promenna do, kterych se ukladaji snimky z kamer
    Mat img_web;
    Mat img_rpi;

    while(1)                    // Nekonen smyka
    {
        if (digitalRead(PIN_0) == LOW)     // Pri stisknuti tlacitka
        {
            cam_web >> img_web;             // Porizeni snimku z web kamery
            cam_rpi.grab();                 // Porizeni snimku z RPi kamery
            cam_rpi.retrieve(img_rpi);
            imshow("web_kam",img_web);        // Zobrazen snimku
            imshow("rpi_kam",img_rpi);        // Zobrazen snimku
            // Ukonen programu pi stisknut libovolne klvesy
        }
        if(waitKey(10) >= 0)
        {
          cout << "Ukoncuji program."  << endl;
          break;
        }
    }
    cam_web.release();              // Uvolneni kamery
    cam_rpi.release();              // Uvolneni kamery
    return 0;
}
