#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>

const char *gst_cap = "rtspsrc location=rtsp://192.168.1.88/live/1 latency=100 !"
"  rtph264depay ! h264parse ! omxh264dec ! "
"  nvvidconv ! video/x-raw, format=BGRx ! "
"  videoconvert ! video/x-raw, format=BGR ! appsink max-buffers=1 drop=true";



int main()
{
    cv::VideoCapture cap(gst_cap, cv::CAP_GSTREAMER);
    if(!cap.isOpened()) 
    {
        std::cout<<"Failed to open camera."<<std::endl;
        return (-1);
    }
    
    unsigned int width  = cap.get(cv::CAP_PROP_FRAME_WIDTH); 
    unsigned int height = cap.get(cv::CAP_PROP_FRAME_HEIGHT); 
    unsigned int fps    = cap.get(cv::CAP_PROP_FPS);
    unsigned int pixels = width*height;
    //unsigned int fourcc = CAP_PROP_FOURCC('h', '2', '6', '4');
    std::cout <<" Frame size : "<<width<<" x "<<height<<", "<<pixels<<" Pixels "<<fps<<" FPS"<<std::endl;

    
    // cv::namedWindow("MyCameraPreview",cv::WINDOW_NORMAL);
	// cv::setWindowProperty("MyCameraPreview", cv::WND_PROP_FULLSCREEN, cv::WINDOW_FULLSCREEN );
    cv::namedWindow("MyCameraPreview", cv::WINDOW_FULLSCREEN);
    cv::Mat frame_in, frame_out;
    
    const char*  gst_writer = "appsrc ! video/x-raw, format=(string)BGR ! \
			videoconvert ! video/x-raw, format=(string)I420 ! \
 		        nvvidconv ! video/x-raw(memory:NVMM) ! \
			omxh264enc insert-sps-pps=1 ! h264parse ! rtph264pay config-interval=1 ! udpsink host=$IP_TO_STREAM_TO port=5000";
    cv::VideoWriter writer(gst_writer, cv::CAP_PROP_FOURCC, 30, cv::Size(width, height));

    while(1)
    {
        if (!writer.isOpened()) 
        {
            std::cout<<"Failed to open writer."<<std::endl;
            return (-2);
        }
        if (!cap.read(frame_in)) 
        {
            std::cout<<"Capture read error"<<std::endl;
            break;
        }
        else  
        {
            frame_in.copyTo(frame_out);
            //resize(frame_in, frame_in, cv::Size(1920, 1080));
            cv::imshow("MyCameraPreview",frame_in);
            writer.write(frame_out);
            // cv::waitKey(1) == 'q'; // let imshow draw 
        }
        if (char(cv::waitKey(1)) == 'q') break;	
    }

    cap.release();
    // writer.release();

    return 0;
}