#include "SDL2/SDL.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <SerialPort.h>

#include <string>
#include <fstream>
#include <ios>
#include <iostream>
#include <sstream>
#include <stdio.h>
#include <time.h>
#include <chrono>


std::string gstreamer_pipeline (int capture_width, int capture_height, int display_width, int display_height, int framerate, int flip_method) {
    return "nvarguscamerasrc ! video/x-raw(memory:NVMM), width=(int)" + std::to_string(capture_width) + ", height=(int)" +
           std::to_string(capture_height) + ", format=(string)NV12, framerate=(fraction)" + std::to_string(framerate) +
           "/1 ! nvvidconv flip-method=" + std::to_string(flip_method) + " ! video/x-raw, width=(int)" + std::to_string(display_width) + ", height=(int)" +
           std::to_string(display_height) + ", format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR ! appsink";
}


SDL_Joystick *joystick;
SDL_Event event;

using namespace std;
using namespace cv;
using namespace std::chrono;


int Deadzone = 0;
int coordX = 0;
int coordY = 0;
int FPS, FPScount;

steady_clock::time_point t1;
steady_clock::time_point t2;


std::string data;

cv::Scalar HUD_COLOR(0,255,0); // Green
cv::Scalar BG_COLOR(0,0,0); // Green


long mapW(long x, long in_min, long in_max, long out_min, long out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int main (){
	
	
/*SerialPort serial_port("/dev/ttyACM0");
  try {
    serial_port.Open(SerialPort::BAUD_230400,
    SerialPort::CHAR_SIZE_8,
    SerialPort::PARITY_NONE,
    SerialPort::STOP_BITS_1,
    SerialPort::FLOW_CONTROL_NONE);
  }
  catch (SerialPort::OpenFailed E) {
    cout << "Error opening the serial port" << endl;
    return 1;
  }
*/


	int capture_width = 1280 ;
    	int capture_height = 720 ;
    	int display_width = 1024;
    	int display_height = 576;
    	int framerate = 60 ;
    	int flip_method = 0 ;

    	std::string pipeline = gstreamer_pipeline(capture_width,
        capture_height,
        display_width,
        display_height,
        framerate,
        flip_method);




	if((SDL_Init(SDL_INIT_JOYSTICK)==-1)) {
        printf("Could not initialize SDL: %s.\n", SDL_GetError());
        exit(-1);
    	}
    	printf("SDL initialized.\n");


	printf("%i joysticks were found.\n\n", SDL_NumJoysticks());
   	printf("The names of the joysticks are:\n");

    	for(int i=0; i < SDL_NumJoysticks(); i++ ){
        printf("    %s\n", SDL_JoystickName(SDL_JoystickOpen(i)));
	}

    	SDL_JoystickEventState(SDL_ENABLE);
    	joystick = SDL_JoystickOpen(0);

	printf("Number of Axes: 	%i\n", SDL_JoystickNumAxes(joystick));
	printf("Number of Hats: 	%i\n", SDL_JoystickNumHats(joystick));
	printf("Number of Buttons: 	%i\n", SDL_JoystickNumButtons(joystick));


	//cv::VideoCapture camera(0);
	cv::VideoCapture camera(pipeline, cv::CAP_GSTREAMER);
	if(!camera.isOpened()) {
	std::cout<<"Failed to open camera."<<std::endl;
	return (-1);
    }
	camera.set(CAP_PROP_BUFFERSIZE, 1);
    /*if (!camera.open(0)) {
         printf("ERROR: Could not open camera");
        return 1;
    }*/


	cv::namedWindow("Webcam", WINDOW_AUTOSIZE);
    	cv::Mat frame;	
	
	cv::Point ZERO(0,0);
	cv::Point CENTER( frame.cols / 2, frame.rows / 2 );
	cv::Point RET_OFFSET(- 28,9);

	while (1) {
		
		
		/*//-- Wait for the received data
		SerialPort::DataBuffer buffer;

		try {
		serial_port.Read(buffer,20,500);
		}
		catch (SerialPort::ReadTimeout E) {
		cout << "TIMEOUT!";
		return 1;
		}

		for(int ii=0; ii < buffer.size(); ii++) {
		cout << buffer[ii];
		}*/
		
		t2 = steady_clock::now();
		duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
		
		if( time_span.count() >= 1){
			FPS = FPScount;
			FPScount = 0;
			t1 = steady_clock::now();
			
		}
	SDL_PollEvent(&event);
	//cv::Mat frame(512, 512, CV_8UC3, cv::Scalar(0));
	
	coordY = SDL_JoystickGetAxis(joystick, 1);
	coordX = SDL_JoystickGetAxis(joystick, 0);
	int coordXmapped = mapW(coordX, -32768, 32767, 0, frame.cols);
	int coordYmapped = mapW(coordY, -32768, 32767, 0, frame.rows);
	
	cv::Point AIM(coordXmapped,coordYmapped);
	
	if (!camera.read(frame)) {
		std::cout<<"Capture read error"<<std::endl;
		break;
	}
	
	
	
	
	//camera.grab();
	//camera >> frame;
	std::string showX = "X coord: " + std::to_string(coordXmapped);
	std::string showY = "Y coord: " + std::to_string(coordYmapped);
	std::string showFPS = "FPS: " + std::to_string(FPS);
	
	
	cv::rectangle(frame, cv::Point(0, 0), cv::Point(120, 60),BG_COLOR, -1);
	cv::putText(frame, showX,cv::Point(10, 10),cv::FONT_HERSHEY_PLAIN ,0.8,HUD_COLOR,1);
	cv::putText(frame, showY,cv::Point(10, 30),cv::FONT_HERSHEY_PLAIN ,0.8,HUD_COLOR,1);
	cv::putText(frame, showFPS,cv::Point(10, 50),cv::FONT_HERSHEY_PLAIN ,0.8,HUD_COLOR,1);
	
	cv::putText(frame, ">|<",AIM + RET_OFFSET,cv::FONT_HERSHEY_DUPLEX,1.0,HUD_COLOR,2);
	//cv::circle( frame, cv::Point( frame.cols / 2, frame.rows / 2 ), 32.0, cv::Scalar( 0, 255, 0 ), 1, 8 );
	//cv::circle( frame, ZERO, 32.0, cv::Scalar( 0, 255, 0 ), 1, 8 );
	//cv::line(frame, cv::Point(frame.cols / 2, 0), cv::Point(frame.cols / 2, frame.rows), HUD_COLOR, 1);
	//cv::line(frame, cv::Point(0, frame.rows / 2), cv::Point(frame.cols, frame.rows / 2), HUD_COLOR, 1);

    cv::imshow("Webcam", frame);
	FPScount++;

	if( waitKey(10) == 27 ) break; // stop capturing by pressing ESC


   }


	return 0;
    	camera.release();
    	cv::destroyAllWindows() ;
	//serial_port.Close();

}