#include "Argus/Argus.h"
#include <Argus/Ext/DolWdrSensorMode.h>
#include <Argus/Ext/PwlWdrSensorMode.h>

#include <EGLStream/EGLStream.h>
#include <EGLStream/NV/ImageNativeBuffer.h>

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


#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>

#include <vpi/Image.h>
#include <vpi/Status.h>
#include <vpi/Stream.h>
#include <vpi/algo/ConvertImageFormat.h>
#include <vpi/algo/PerspectiveWarp.h>
#include <vpi/algo/Rescale.h>
#include <vpi/EGLInterop.h>
#include <vpi/NvBufferInterop.h>
#include <vpi/Event.h>


using namespace Argus;

#define VPI_JOB(msg,cmd)	\
		do                                                        \
		{                                                         \
			VPIStatus status = (cmd);                             \
			if (status != VPI_SUCCESS)                            \
			{                                                     \
				char buffer[VPI_MAX_STATUS_MESSAGE_LENGTH];       \
				vpiGetLastStatusMessage(buffer, sizeof(buffer));  \
				std::ostringstream ss;                            \
				ss << msg << " -- ";							  \
				ss << vpiStatusGetName(status) << ": " << buffer; \
				throw std::runtime_error(ss.str());               \
			} else {                                              \
				std::cout << msg << ": OK!\n";					  \
			}													  \
		} while (0)

#define EXIT_IF_NULL(val,msg)   \
		{if (!val) {printf("%s\n",msg); return EXIT_FAILURE;}}
#define EXIT_IF_NOT_OK(val,msg) \
		{if (val!=Argus::STATUS_OK) {printf("%s\n",msg); return EXIT_FAILURE;}}



int main() {
	// Set up some constants:
	const uint16_t w = 1920, h = 1080;
	const VPIPerspectiveTransform xform =
	{
			{ 0.5386, 0.1419, -74  },
			{ -.4399, 0.8662, 291.5},
			{-0.0005, 0.0003,   1  }
	};

	//Initialize/setup Argus:
	Argus::UniqueObj<Argus::CameraProvider> cameraProvider(Argus::CameraProvider::create());

	Argus::ICameraProvider *iCameraProvider =
			Argus::interface_cast<Argus::ICameraProvider>(cameraProvider);

	printf("Argus Version: %s\n", iCameraProvider->getVersion().c_str());

	std::vector<CameraDevice*> devices;
	iCameraProvider->getCameraDevices(&devices);
	uint16_t i=0;
	for (auto& device: devices) {
		InterfaceID interface;
		device->getInterface(interface);
		printf("%u == %s\n", i++, interface.getName());
	}
	CameraDevice* device = devices[0];
	Argus::ICameraProperties *iCameraProperties =
			Argus::interface_cast<Argus::ICameraProperties>(device);

	std::vector<Argus::SensorMode*> sensorModes;
	Argus::Status status = iCameraProperties->getAllSensorModes(&sensorModes);

	Argus::SensorMode* sensorMode = sensorModes[3];
	Argus::ISensorMode *iSensorMode =
			Argus::interface_cast<Argus::ISensorMode>(sensorMode);

	if (!iSensorMode)
	{
		printf("Failed to get sensor mode interface\n");
		return EXIT_FAILURE;
	}

	Argus::UniqueObj<Argus::CaptureSession> captureSession(
			iCameraProvider->createCaptureSession(device, &status));
	EXIT_IF_NOT_OK(status, "Failed to create capture session");


	Argus::ICaptureSession *iSession =
			Argus::interface_cast<Argus::ICaptureSession>(captureSession);
	EXIT_IF_NULL(iSession, "Cannot get Capture Session Interface");

	Argus::UniqueObj<Argus::OutputStreamSettings> streamSettings(
			iSession->createOutputStreamSettings(Argus::STREAM_TYPE_EGL));

	Argus::IEGLOutputStreamSettings *iEGLStreamSettings =
			Argus::interface_cast<Argus::IEGLOutputStreamSettings>(streamSettings);
	EXIT_IF_NULL(iEGLStreamSettings, "Cannot get IEGLOutputStreamSettings Interface");

	iEGLStreamSettings->setPixelFormat(Argus::PIXEL_FMT_YCbCr_420_888);
	iEGLStreamSettings->setResolution(iSensorMode->getResolution());
	iEGLStreamSettings->setMetadataEnable(true);


	Argus::UniqueObj<Argus::OutputStream> stream(
			iSession->createOutputStream(streamSettings.get()));
	EXIT_IF_NULL(stream, "Failed to create EGLOutputStream");

	Argus::UniqueObj<EGLStream::FrameConsumer> consumer(
			EGLStream::FrameConsumer::create(stream.get()));

	EGLStream::IFrameConsumer *iFrameConsumer =
			Argus::interface_cast<EGLStream::IFrameConsumer>(consumer);
	EXIT_IF_NULL(iFrameConsumer, "Failed to initialize Consumer");

	Request *request = iSession->createRequest();
	IRequest *iRequest = interface_cast<IRequest>(request);
	ISourceSettings *iSourceSettings = interface_cast<ISourceSettings>(request);
	iSourceSettings->setFrameDurationRange(Range<uint64_t>(33333334));

	EXIT_IF_NULL(iSourceSettings, "Failed to get source settings request interface");
	iSourceSettings->setSensorMode(sensorMode);

	iRequest->enableOutputStream(stream.get());

	VPIStream vpiStream;
	VPI_JOB("Creating VPI stream", vpiStreamCreate(VPI_BACKEND_CUDA, &vpiStream));

	i = 0;
	int m_dmabuf = -1;
	IEGLOutputStream *iEGLOutputStream = interface_cast<IEGLOutputStream>(stream);

	VPIImage vpiInputImage;
	VPIImage vpiScaledImage;
	VPIImage vpiWarpedImage;
	VPIImage vpiBGR8Image;

	VPIEvent evScale, evConvert, evWarp, evDone;

	bool firstCapture = true;
	uint16_t frameNr = 0;

	VPI_JOB("Creating output scaled image", vpiImageCreate(w, h, VPI_IMAGE_FORMAT_NV12_ER, 0, &vpiScaledImage));
	VPI_JOB("Creating output warped image", vpiImageCreate(w, h, VPI_IMAGE_FORMAT_NV12_ER, 0, &vpiWarpedImage));
	VPI_JOB("Creating output image (BGR8)", vpiImageCreate(w, h, VPI_IMAGE_FORMAT_BGR8, 0, &vpiBGR8Image));


	VPIPayload warp;
	VPI_JOB("Creating perspective warp payload", vpiCreatePerspectiveWarp(VPI_BACKEND_CUDA, &warp));

	std::vector<EventType> eventTypes;
	eventTypes.push_back(EVENT_TYPE_CAPTURE_COMPLETE);
	eventTypes.push_back(EVENT_TYPE_ERROR);
	eventTypes.push_back(EVENT_TYPE_CAPTURE_STARTED);
	IEventProvider *iEventProvider = interface_cast<IEventProvider>(captureSession);
	UniqueObj<EventQueue> queue(iEventProvider->createEventQueue(eventTypes));
	IEventQueue *iQueue = interface_cast<IEventQueue>(queue);

	EXIT_IF_NOT_OK(iSession->repeat(request), "Submit repeat request failed");


	for (uint16_t i=0; i<200; i++) {

		iEventProvider->waitForEvents(queue.get(), -1);
		if (iQueue->getSize() == 0) {
			printf("NO EVENTS IN QUEUE\n");
			return -1;
		}

		const Event* event = iQueue->getEvent(iQueue->getSize() -1);
		const IEvent* iEvent = interface_cast<const IEvent>(event);
		if (iEvent->getEventType() == EVENT_TYPE_CAPTURE_COMPLETE) {

			Argus::UniqueObj<EGLStream::Frame> frame(iFrameConsumer->acquireFrame(1000000, &status));

			printf("acquireFrame status=%d\n", status);
			if (status != 0) continue;

			EGLStream::IFrame *iFrame = Argus::interface_cast<EGLStream::IFrame>(frame);

			printf("Time: Sensor=%ld\n", iFrame->getTime());

			EGLStream::NV::IImageNativeBuffer *iNativeBuffer = interface_cast<EGLStream::NV::IImageNativeBuffer>(iFrame->getImage());
			if (!iNativeBuffer) {
				printf("IImageNativeBuffer not supported by Image.\n");
			}

			if (m_dmabuf == -1) {
				m_dmabuf = iNativeBuffer->createNvBuffer(
						iEGLOutputStream->getResolution(),
						NvBufferColorFormat_NV12_ER,
						NvBufferLayout_Pitch,
						EGLStream::NV::ROTATION_0,
						&status);

				if (m_dmabuf == -1) {
					printf("Failed to create NvBuffer\n");
					continue;
				}
			}
			else if (iNativeBuffer->copyToNvBuffer(m_dmabuf) != STATUS_OK)
			{
				printf("Failed to copy frame to NvBuffer.\n");
				continue;
			}
			printf("dmabuf=%d\n", m_dmabuf);


			printf("NvBuffer filled, proceeding to VPI portion...\n");
			vpiEventCreate(0, &evScale);
			vpiEventCreate(0, &evConvert);
			vpiEventCreate(0, &evWarp);
			vpiEventCreate(0, &evDone);


			if (firstCapture) {
				VPI_JOB("Creating wrapper", vpiImageCreateNvBufferWrapper(m_dmabuf, nullptr, VPI_BACKEND_CUDA | VPI_EXCLUSIVE_STREAM_ACCESS, &vpiInputImage));
				firstCapture=false;
			}

			VPI_JOB("Marking rescale", vpiEventRecord(evScale, vpiStream));
			VPI_JOB("Submitting rescale", vpiSubmitRescale(vpiStream, VPI_BACKEND_CUDA,
					vpiInputImage, vpiScaledImage, VPI_INTERP_LINEAR, VPI_BORDER_ZERO, 0));


			VPI_JOB("Marking warp", vpiEventRecord(evWarp, vpiStream));
			VPI_JOB("Submitting perspective warp", vpiSubmitPerspectiveWarp(vpiStream, 0, warp, vpiScaledImage, xform ,vpiWarpedImage, VPI_INTERP_LINEAR, VPI_BORDER_ZERO, 0));


			VPI_JOB("Marking convert", vpiEventRecord(evConvert, vpiStream));

			VPI_JOB("Converting to BGR8", vpiSubmitConvertImageFormat(vpiStream, VPI_BACKEND_CUDA, vpiWarpedImage, vpiBGR8Image, nullptr));
			VPI_JOB("Marking done", vpiEventRecord(evDone, vpiStream));

			VPI_JOB("Sync stream", vpiStreamSync(vpiStream));


			VPIImageData outData;
			VPI_JOB("Locking output data", vpiImageLock(vpiBGR8Image, VPI_LOCK_READ, &outData));


			cv::Mat cvOut(outData.planes[0].height, outData.planes[0].width, CV_8UC3,
					outData.planes[0].data, outData.planes[0].pitchBytes);

			cv::imshow("Out", cvOut);
			cv::waitKey(1);

			VPI_JOB("Unlocking output data", vpiImageUnlock(vpiBGR8Image));


			float tscale, tconvert, twarp;
			VPI_JOB("Getting scale time", vpiEventElapsedTimeMillis(evScale, evWarp, &tscale));
			VPI_JOB("Getting warp time", vpiEventElapsedTimeMillis(evWarp, evConvert, &twarp));
			VPI_JOB("Getting convert time", vpiEventElapsedTimeMillis(evConvert, evDone, &tconvert));
			frameNr++;

			printf("TIMINGS: scale: %fms\twarp:%fms\tconvert: %fms\n", tscale, twarp, tconvert);

			vpiEventDestroy(evScale);
			vpiEventDestroy(evConvert);
			vpiEventDestroy(evWarp);
			vpiEventDestroy(evDone);
		}
	}
	printf("Destroying images\n");

	vpiImageDestroy(vpiInputImage);
	vpiImageDestroy(vpiScaledImage);
	vpiImageDestroy(vpiBGR8Image);
	vpiImageDestroy(vpiWarpedImage);


	iSession->stopRepeat();
	iSession->waitForIdle();

	request->destroy();

	printf("Done!\n");



	cameraProvider.reset();

}