/*
 * Copyright (c) 2020, Linaro Limited
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted (subject to the limitations in the
 * disclaimer below) provided that the following conditions are met:
 *
 *    * Redistributions of source code must retain the above copyright
 *      notice, this list of conditions and the following disclaimer.
 *
 *    * Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *      disclaimer in the documentation and/or other materials provided
 *      with the distribution.
 *
 *    * Neither the name of The Linux Foundation nor the names of its
 *      contributors may be used to endorse or promote products derived
 *      from this software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
 * GRANTED BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
 * HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
 * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <vector>
#include <string>
#include <iterator>
#include <unordered_map>
#include <algorithm>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <stdint.h>
#include <assert.h>

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

#include "DlSystem/DlVersion.hpp"
#include "DlSystem/DlError.hpp"
#include "DlSystem/RuntimeList.hpp"
#include "DlSystem/UserBufferMap.hpp"
#include "DlSystem/String.hpp"
#include "DlSystem/TensorShape.hpp"
#include "DlSystem/IUserBuffer.hpp"
#include "DlSystem/IUserBufferFactory.hpp"
#include "DlContainer/IDlContainer.hpp"
#include "SNPE/SNPE.hpp"
#include "SNPE/SNPEFactory.hpp"
#include "SNPE/SNPEBuilder.hpp"


/* MobileNet SSD Layers */
#define MODEL_DLC_NAME "/home/linaro/SNPEMobileNetCv/tensorflow_mobilenet_ssd.dlc"
#define IN_LAYER	"Preprocessor/sub:0"
#define OUT_LAYER1	"Postprocessor/BatchMultiClassNonMaxSuppression"
#define OUT_LAYER2	"add_6"
#define OUTPUT_BOXES   "Postprocessor/BatchMultiClassNonMaxSuppression_boxes"
#define OUTPUT_CLASSES "Postprocessor/BatchMultiClassNonMaxSuppression_classes"
#define OUTPUT_SCORES  "Postprocessor/BatchMultiClassNonMaxSuppression_scores"
#define OUTPUT_DET_CLASSES "detection_classes:0"
#define SSD_NUM_OUTPUTS	4
#define SSD_NUM_INPUTS	1

/* Post Processing Image size */
#define IMG_HEIGHT		300
#define IMG_WIDTH		300
#define IMG_DEPTH		3
#define BUFFER_SIZE	(IMG_WIDTH * IMG_HEIGHT * IMG_DEPTH)

/* Max detection Items */
#define MAX_ITEMS		20

/* Minimum Accuracy required in percentage */
#define MIN_ACCURACY		30

using namespace std;
using namespace cv;
using std::cout; using std::cerr; using std::endl;

struct ssd_layer_info {
	const char *name;
	int sizeBytes;
	std::vector<size_t> strides;
};

static struct ssd_layer_info ssd_output_info[SSD_NUM_OUTPUTS] {
	[0] = {
		.name = OUTPUT_BOXES,
		.sizeBytes = 1600,
		.strides = {1600, 16, 4},
	},
	[1] = {
		.name = OUTPUT_CLASSES,
		.sizeBytes = 1600,
		.strides = {400, 400, 4},
	},
	[2] = {
		.name = OUTPUT_SCORES,
		.sizeBytes = 1600,
		.strides = {400, 400, 4},
	},
	[3] = {
		.name = OUTPUT_DET_CLASSES,
		.sizeBytes = 400,
		.strides = {400, 400, 4},
	},
};

static struct ssd_layer_info ssd_input_info[SSD_NUM_OUTPUTS] {
	[0] = {
		.name = IN_LAYER,
		.sizeBytes = 1080000,
		.strides = {3600, 12, 4},
	},
};

struct SSDbox {
        float top;
        float left;
        float bottom;
        float right;
        uint32_t id;
        float score;
};
static struct SSDbox SSDBoxes[MAX_ITEMS];

std::vector<const char*> ssd_classes = {
		"person",
		"bicycle",
		"car",
		"motorcycle",
		"airplane",
		"bus",
		"train",
		"truck",
		"boat",
		"traffic light",
		"fire hydrant",
		"???",
		"stop sign",
		"parking meter",
		"bench",
		"bird",
		"cat",
		"dog",
		"horse",
		"sheep",
		"cow",
		"elephant",
		"bear",
		"zebra",
		"giraffe",
		"???",
		"backpack",
		"umbrella",
		"???",
		"???",
		"handbag",
		"tie",
		"suitcase",
		"frisbee",
		"skis",
		"snowboard",
		"sports ball",
		"kite",
		"baseball bat",
		"baseball glove",
		"skateboard",
		"surfboard",
		"tennis racket",
		"bottle",
		"???",
		"wine glass",
		"cup",
		"fork",
		"knife",
		"spoon",
		"bowl",
		"banana",
		"apple",
		"sandwich",
		"orange",
		"broccoli",
		"carrot",
		"hot dog",
		"pizza",
		"donut",
		"cake",
		"chair",
		"couch",
		"potted plant",
		"bed",
		"???",
		"dining table",
		"???",
		"???",
		"toilet",
		"???",
		"tv",
		"laptop",
		"mouse",
		"remote",
		"keyboard",
		"cell phone",
		"microwave",
		"oven",
		"toaster",
		"sink",
		"refrigerator",
		"???",
		"book",
		"clock",
		"vase",
		"scissors",
		"teddy bear",
		"hair drier",
		"toothbrush",
		"???",
		"???",
		"???",
		"???",
		"???",
		"???",
		"???",
		"???",
		"???",
};

static int uint8_to_float(uint8_t *data, uint8_t *fdata, size_t length)
{
	float *float_data = (float *)fdata;

	// Scale the uint8t data into floats
	for (int i = 0; i < length; i++)
		float_data[i] = (data[i] * (1.0) / 255) + 0.0;

	return 0;
}

void SSDshowOutput(uint8_t *uclasses, uint8_t *uscores, uint8_t *uboxes, Mat *frame)
{
	int i;
	float *mclasses = (float *)uclasses;
	float *scores = (float *)uscores;
	float *boxes = (float *)uboxes;
	char label[256];
	int fontface = cv::FONT_HERSHEY_PLAIN;
	double scale = 1.0;
	int thickness = 1;
	int baseline = 0;

	for ( i = 0; i < MAX_ITEMS; i++) {
		SSDBoxes[i].top = frame->rows * boxes[i * 4];
                SSDBoxes[i].left = frame->cols * boxes[i * 4 + 1];
                SSDBoxes[i].bottom = frame->rows * boxes[i * 4 + 2];
                SSDBoxes[i].right = frame->cols * boxes[i * 4 + 3];
                SSDBoxes[i].id = (uint32_t)mclasses[i];
                SSDBoxes[i].score = scores[i] * 100.00;

	}

	for ( i = 0; i < MAX_ITEMS; i++) {
#if DEBUG
             std::cout << i <<":\t" << SSDBoxes[i].score << ":\t" <<
                        ssd_classes[(SSDBoxes[i].id) ] <<":\t(" <<
                        (uint32_t) SSDBoxes[i].top << ", " <<
                        (uint32_t)SSDBoxes[i].left << ", " <<
                        (uint32_t) SSDBoxes[i].bottom << ", " <<
                        (uint32_t) SSDBoxes[i].right << ")"<< std::endl;
#endif
	     if (SSDBoxes[i].score < MIN_ACCURACY)
		     break;

	     sprintf(label, "%02.2f%%, %s", SSDBoxes[i].score,
		     ssd_classes[SSDBoxes[i].id]);
	     cv::rectangle(*frame, cv::Point((uint32_t) SSDBoxes[i].left,
					     (uint32_t)SSDBoxes[i].top),
				   cv::Point( (uint32_t) SSDBoxes[i].right,
					      (uint32_t) SSDBoxes[i].bottom),
				   cv::Scalar(0,255, 0), 1);

		cv::Size text = cv::getTextSize(label, fontface, scale,
						thickness, &baseline);
		cv::rectangle(*frame, cv::Point((uint32_t) SSDBoxes[i].left,
						(uint32_t)SSDBoxes[i].top) +
						  cv::Point(0, baseline),
				  cv::Point((uint32_t) SSDBoxes[i].left,
					    (uint32_t)SSDBoxes[i].top) +
					 cv::Point(text.width, -text.height),
				CV_RGB(0,0,0), cv::FILLED);
		cv::putText(*frame, label,
			    cv::Point((uint32_t) SSDBoxes[i].left,
				      (uint32_t)SSDBoxes[i].top),
			    cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(57,255,20),
			    0.1, LINE_8);
	}


}

void CreateBufferMap(zdl::DlSystem::UserBufferMap& bufferMap,
                          std::unordered_map<std::string, std::vector<uint8_t>>& applicationBuffers,
                          std::vector<std::unique_ptr<zdl::DlSystem::IUserBuffer>>& snpeUserBackedBuffers,
                          std::unique_ptr<zdl::SNPE::SNPE>& snpe,
			  struct ssd_layer_info *info, int size)
{
   int i;

   for (i = 0; i < size; i++) {

	std::unique_ptr<zdl::DlSystem::UserBufferEncoding> userBufferEncoding;
	userBufferEncoding = std::unique_ptr<zdl::DlSystem::UserBufferEncodingFloat>(new zdl::DlSystem::UserBufferEncodingFloat());

	applicationBuffers.emplace(info[i].name,
			   std::vector<uint8_t>(info[i].sizeBytes));
	zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
	snpeUserBackedBuffers.push_back(ubFactory.createUserBuffer(
					applicationBuffers.at(
						info[i].name).data(),
					info[i].sizeBytes,
					info[i].strides,
					userBufferEncoding.get()));
	if (snpeUserBackedBuffers.back() == nullptr)
	{
		std::cerr << "Error while creating user buffer." << std::endl;
	}

	bufferMap.add(info[i].name, snpeUserBackedBuffers.back().get());
   }
}

int main(int argc, char** argv)
{
	static zdl::DlSystem::Runtime_t runtime = zdl::DlSystem::Runtime_t::DSP;
	zdl::DlSystem::UserBufferMap inputMap, outputMap;
	static zdl::DlSystem::RuntimeList runtimeList;
	zdl::DlSystem::PlatformConfig platformConfig;
	static std::string dlc = MODEL_DLC_NAME;
	std::unique_ptr<zdl::SNPE::SNPE> snpe;
	zdl::DlSystem::StringList outList(512);
	Mat frame, frame1, cFrame;
	void *data, *output, *fdata;

	/* Pre Processing buffer */
	data = (void *)malloc(BUFFER_SIZE);
	if (!data)
		return -ENOMEM;

	/* Output Layers */
	outList.append(OUT_LAYER1);
	outList.append(OUT_LAYER2);

	if (!zdl::SNPE::SNPEFactory::isRuntimeAvailable(runtime)) {
		std::cerr << "DSP runtime not present!" << std::endl;
		return -EINVAL;
	}
	runtimeList.add(runtime);

	std::ifstream dlcFile(dlc);

	std::unique_ptr<zdl::DlContainer::IDlContainer> container =
		zdl::DlContainer::IDlContainer::open(zdl::DlSystem::String(dlc.c_str()));
	if (container == nullptr)
	{
		std::cerr << "Error while opening the container file." << std::endl;
		return -EINVAL;
	}
	zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
	snpe = snpeBuilder.setOutputLayers(outList)
		.setRuntimeProcessorOrder(runtimeList)
		.setUseUserSuppliedBuffers(true)
		.setPlatformConfig(platformConfig)
		.setInitCacheMode(false)
		.build();
	if (snpe == nullptr)
	{
		std::cerr << "Error building SNPE object." << std::endl;
		return -EINVAL;
	}



	std::vector <std::unique_ptr<zdl::DlSystem::IUserBuffer>> snpeUserBackedInputBuffers, snpeUserBackedOutputBuffers;
	std::unordered_map <std::string, std::vector<uint8_t>>	applicationOutputBuffers;
	std::unordered_map <std::string, std::vector<uint8_t>> applicationInputBuffers;

	CreateBufferMap(outputMap, applicationOutputBuffers,
			snpeUserBackedOutputBuffers, snpe,
			ssd_output_info, SSD_NUM_OUTPUTS);
	CreateBufferMap(inputMap, applicationInputBuffers,
			snpeUserBackedInputBuffers, snpe,
			ssd_input_info, SSD_NUM_INPUTS);


	Mat rgb_mat(IMG_HEIGHT, IMG_WIDTH, CV_8UC3, data);

	VideoCapture capture(2);
	if (!capture.isOpened())
	{
		cerr << "ERROR: Can't initialize camera capture" << endl;
		return 1;
	}

	for (;;)
	{
		capture >> frame;
		if (frame.empty())
		{
			cerr << "ERROR: Can't grab camera frame." << endl;
			break;
		}
		/* resize to 300 x 300 */
		cv::resize(frame, frame1, cv::Size(IMG_HEIGHT, IMG_WIDTH));
		cv::cvtColor(frame1, rgb_mat, cv::COLOR_BGR2RGB);

		uint8_to_float((uint8_t *)data,
			       applicationInputBuffers.at(IN_LAYER).data(),
			       BUFFER_SIZE);

		if (!snpe->execute(inputMap, outputMap)) {
			std::cerr << "Error while executing the network."
				<< std::endl;
			break;
		}

		SSDshowOutput(applicationOutputBuffers.at(OUTPUT_CLASSES).data(),
			      applicationOutputBuffers.at(OUTPUT_SCORES).data(),
			      applicationOutputBuffers.at(OUTPUT_BOXES).data(),
			      &frame);

		imshow("SPNE-MoblieNetSSD-Demo", frame);
		waitKey(1);
	}
	snpe.reset();
	free(data);
	return 0;
}