/*
 * Copyright (c) 2015-2017, Sumio Morioka
 * All rights reserved.
 *
 * This source code was originally written by Dr.Sumio Morioka for use in the 2015-2016 issues of 
 * "the Interface magazine", published by CQ publishing Co.Ltd in Japan (http://www.cqpub.co.jp).
 * The author has no responsibility on any results caused by using this code.
 *
 * - Author: Dr.Sumio Morioka (http://www002.upp.so-net.ne.jp/morioka, FB:Sumio Morioka)
 *
 *
 * IMPORTANT NOTICE:
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted 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 copyright holder nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * 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 <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <libwebsockets.h>
#include <linux/i2c-dev.h>
#include <math.h>
#include <memory.h>
#include <pthread.h>
#include <semaphore.h>
#include <signal.h>
#include <sysexits.h>
#include <syslog.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <termios.h>
#include <time.h>
#include <unistd.h>

#include "cv.h"
#include "highgui.h"
#include <opencv2/opencv.hpp>		// added; OpenCV 3.1
#include <opencv2/tracking.hpp>		// added; OpenCV 3.1

#include "bcm_host.h"
#include "interface/vcos/vcos.h"

#include "interface/mmal/mmal.h"
//#include "interface/mmal/mmal_logging.h"
#include "interface/mmal/mmal_buffer.h"
#include "interface/mmal/util/mmal_util.h"
#include "interface/mmal/util/mmal_util_params.h"
#include "interface/mmal/util/mmal_default_components.h"
#include "interface/mmal/util/mmal_connection.h"

#include "RaspiPreview.h"
#include "RaspiCLI.h"
//#include "RaspiCamControl.h"
#include "RaspiCamControl.hpp"

// ********************************************************** >> CQ
// >> CHANGE FOLLOWING DEFS IF NECESSARY
#define	DEFAULT_WIDTH		320		// use a multiple of 320 (640, 1280)
#define	DEFAULT_HEIGHT		240		// use a multiple of 240 (480, 960)

#define	DEFAULT_FPS_INTVAL	1		// (unit: ms) flame wait interval
#define	DEFAULT_JPG_QUALITY	95

#define	DISP_FPS

//#define CASCADE_PATH "/usr/share/opencv/haarcascades/haarcascade_frontalface_default.xml"
#define CASCADE_PATH "/usr/local/share/OpenCV/haarcascades/haarcascade_frontalface_default.xml"
// ********************************************************** << CQ

#define	DEV_I2C		"/dev/i2c-1"
#define	DEV_MEM		"/dev/mem"
#define	DEV_SERIAL	"/dev/ttyAMA0"
#define	SERIAL_RATE	B9600

#define MEM_PAGESIZ	4096
#define MEM_BLKSIZ	4096

#define PERIPHERAL_BASEADR	0x20000000
#define GPIO_BASEADR		(PERIPHERAL_BASEADR + 0x200000)
#define PWM_BASEADR			(PERIPHERAL_BASEADR + 0x20C000)
#define CLOCK_BASEADR		(PERIPHERAL_BASEADR + 0x101000)

#define	PWM_CTL		0
#define	PWM_RNG1	4
#define	PWM_DAT1	5
#define	PWMCLK_CNTL	40
#define	PWMCLK_DIV	41

volatile unsigned int *ioadr_gpio;
volatile unsigned int *ioadr_clk;
volatile unsigned int *ioadr_pwm;

static long			fps_intval;

struct timespec		tspec;
long				t0, t1;
int					tdiff;
long				tall	= 0;
int					fnum	= 0;

#include "opencv_func.h"


// Standard port setting for the camera component
#define MMAL_CAMERA_PREVIEW_PORT	0
#define MMAL_CAMERA_VIDEO_PORT		1
#define MMAL_CAMERA_CAPTURE_PORT	2

// Video format information
#define VIDEO_FRAME_RATE_NUM	30
#define VIDEO_FRAME_RATE_DEN	1

/// Video render needs at least 2 buffers.
#define VIDEO_OUTPUT_BUFFERS_NUM	3

int mmal_status_to_int(MMAL_STATUS_T status);

// Structure containing all state information for the current run
typedef struct {
   int	width;
   int	height;
   int	framerate;

   RASPICAM_CAMERA_PARAMETERS	camera_parameters;	// Camera setup parameters
   MMAL_COMPONENT_T				*camera_component;	// Pointer to the camera component
   MMAL_POOL_T					*video_pool;		// Pointer to the pool of buffers used by encoder output port
} RASPIVID_STATE;

// Struct used to pass information in encoder port userdata to callback
typedef struct {
	VCOS_SEMAPHORE_T	complete_semaphore;	// semaphore which is posted when we reach end of frame (indicates end of capture or fault)
	RASPIVID_STATE		*pstate;           	// pointer to our state in case required in callback
} PORT_USERDATA;


// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// definitions, global variables, sub-functions, etc.
int						find_face_flag_G;
cv::Ptr<cv::Tracker>	tracker_G;
cv::Rect2d				roi_G;
// ********************************************************** << CQ


////////////////////////////////////////////////////////////////////////////////
// callback of video frame capture
////////////////////////////////////////////////////////////////////////////////
IMG_PROCESSING_PARAM	img_param_G;

static void video_buffer_callback(MMAL_PORT_T *port, MMAL_BUFFER_HEADER_T *buffer)
{
	MMAL_BUFFER_HEADER_T	*new_buffer;
	PORT_USERDATA			*pData;
	char					allocate_flag	= 0;

	pData	= (PORT_USERDATA *)(port->userdata);
	if (pData) {
		if (buffer->length) {
			// check interval time
			clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
			t1	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);

			if (t1 - t0 >= fps_intval) {		// finish waiting
				int	w, h;

				mmal_buffer_header_mem_lock(buffer);
				w	= pData->pstate->width;
				h	= pData->pstate->height;
#ifdef IMG_COLOR
				memcpy((*(img_param_G.srcImage)).data, buffer->data, w * h * 3);	// set image
#endif	// IMG_COLOR
#ifdef IMG_GRAY
				memcpy((*(img_param_G.srcImage)).data, buffer->data, w * h);	// set image
#endif	// IMG_GRAY
				mmal_buffer_header_mem_unlock(buffer);

				// release buffer back to the pool
				mmal_buffer_header_release(buffer);
				if (port->is_enabled) {
					MMAL_STATUS_T	status;
					new_buffer	= mmal_queue_get(pData->pstate->video_pool->queue);
					if (new_buffer)
						status = mmal_port_send_buffer(port, new_buffer);
				}
				allocate_flag	= 1;

				opencv_func_run(&img_param_G);

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// copy other information to input thread (if necessary)
// ********************************************************** << CQ

				cv::imshow(WINNAME, *(img_param_G.dstImage));
				cv::waitKey(1);

#ifdef	DISP_FPS
				clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
				t1	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);
				tdiff	= (int)(t1 - t0);
				tall	+= tdiff;
				fnum++;

				if (tdiff > 0 && tall > 0) {
					fprintf(stderr, "%d fps (%d ms)     \r", (1000 * fnum) / tall, tall / fnum);
				}
#endif	// DISP_FPS

				t0	= t1;
			}
			// else "discard this frame"
		}
	}

	if (allocate_flag == 0) {
		// release buffer back to the pool
		mmal_buffer_header_release(buffer);

		if (port->is_enabled) {
			MMAL_STATUS_T	status;
			new_buffer	= mmal_queue_get(pData->pstate->video_pool->queue);
			if (new_buffer)
				status = mmal_port_send_buffer(port, new_buffer);
		}
	}
}


////////////////////////////////////////////////////////////////////////////////
// main
////////////////////////////////////////////////////////////////////////////////
int main(int argc, const char **argv)
{
	RASPIVID_STATE		state;
	MMAL_STATUS_T		status;
	MMAL_PORT_T			*camera_video_port = NULL;
	PORT_USERDATA		callback_data;

	MMAL_COMPONENT_T	*camera = 0;
	MMAL_ES_FORMAT_T	*format;
	MMAL_PORT_T			*video_port = NULL;
	MMAL_POOL_T			*pool;
	MMAL_PARAMETER_CAMERA_CONFIG_T	cam_config;

	cv::Mat				inImage, outImage;

	int		i;

	//////////////////////////////////////////////////////////
	// init variables
	//////////////////////////////////////////////////////////
	memset(&state, 0, sizeof(RASPIVID_STATE));

	state.width 		= DEFAULT_WIDTH;
	state.height 		= DEFAULT_HEIGHT;
	state.framerate 	= VIDEO_FRAME_RATE_NUM;

	fps_intval	= DEFAULT_FPS_INTVAL;

	// Set up the camera_parameters to default
	raspicamcontrol_set_defaults(&(state.camera_parameters));

	opencv_func_init(state.width, state.height);

#ifdef IMG_COLOR
	inImage.create(cv::Size(state.width, state.height), CV_8UC3);
	outImage.create(cv::Size(state.width, state.height), CV_8UC3);
#endif
#ifdef IMG_GRAY
	inImage.create(cv::Size(state.width, state.height), CV_8UC1);
	outImage.create(cv::Size(state.width, state.height), CV_8UC1);
#endif

	cv::CascadeClassifier	cascade;
	cascade.load(CASCADE_PATH);

	img_param_G.srcImage	= &(inImage);
	img_param_G.dstImage	= &(outImage);
	img_param_G.width		= state.width;
	img_param_G.height		= state.height;
	img_param_G.cascade		= &(cascade);

	//////////////////////////////////////////////////////////
	// init camera
	//////////////////////////////////////////////////////////
	bcm_host_init();

	// Create the component
	status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera);
	video_port = camera->output[MMAL_CAMERA_VIDEO_PORT];

	//  set up the camera configuration
	(cam_config.hdr).id				= MMAL_PARAMETER_CAMERA_CONFIG;
	(cam_config.hdr).size			= sizeof(cam_config);
	cam_config.max_stills_w			= state.width;
	cam_config.max_stills_h			= state.height;
	cam_config.stills_yuv422		= 0;
	cam_config.one_shot_stills 		= 0;
	cam_config.max_preview_video_w	= state.width;
	cam_config.max_preview_video_h	= state.height;
	cam_config.num_preview_video_frames	= 3;
	cam_config.stills_capture_circular_buffer_height	= 0;
	cam_config.fast_preview_resume	= 0;
	cam_config.use_stc_timestamp	= MMAL_PARAM_TIMESTAMP_MODE_RESET_STC;

	mmal_port_parameter_set(camera->control, &(cam_config.hdr));

	// Set the encode format on the video  port
	format	= video_port->format;
#ifdef IMG_COLOR
	format->encoding_variant		= MMAL_ENCODING_RGB24;
	format->encoding				= MMAL_ENCODING_RGB24;
#endif
#ifdef IMG_GRAY
	format->encoding_variant		= MMAL_ENCODING_I420;
	format->encoding				= MMAL_ENCODING_I420;
#endif
	format->es->video.width			= state.width;
	format->es->video.height		= state.height;
	format->es->video.crop.x		= 0;
	format->es->video.crop.y		= 0;
	format->es->video.crop.width	= state.width;
	format->es->video.crop.height	= state.height;
	format->es->video.frame_rate.num = state.framerate;
	format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN;

	status = mmal_port_format_commit(video_port);
	status = mmal_port_enable(video_port, video_buffer_callback);

	// Ensure there are enough buffers to avoid dropping frames
	if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) {
		video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM;
	}

	// create pool of message on video port
	video_port->buffer_size		= video_port->buffer_size_recommended;
	video_port->buffer_num		= video_port->buffer_num_recommended;
	pool				= mmal_port_pool_create(video_port, video_port->buffer_num, video_port->buffer_size);
	state.video_pool	= pool;

	// Enable component
	status	= mmal_component_enable(camera);
	raspicamcontrol_set_all_parameters(camera, &(state.camera_parameters));
	state.camera_component	= camera;

	camera_video_port		= state.camera_component->output[MMAL_CAMERA_VIDEO_PORT];
	callback_data.pstate	= &state;

	// assign data to use for callback
	camera_video_port->userdata = (struct MMAL_PORT_USERDATA_T *)&callback_data;

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// preparation of image processing, prior to starting capture

	find_face_flag_G	= 0;
//	tracker_G	= cv::Tracker::create("KCF");			// Opencv 3.1 required
//	tracker_G	= cv::Tracker::create("BOOSTING");		// Opencv 3.1 required
//	tracker_G	= cv::Tracker::create("MIL");			// Opencv 3.1 required
//	tracker_G	= cv::Tracker::create("TLD");			// Opencv 3.1 required
//	tracker_G	= cv::Tracker::create("MEDIANFLOW");	// Opencv 3.1 required

// ********************************************************** << CQ

	//////////////////////////////////////////////////////////
	// start capture
	//////////////////////////////////////////////////////////
	clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
	t0	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);

    // start capture
	mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1);

	// Send all the buffers to the video port
	int		num = mmal_queue_length(state.video_pool->queue);
	int		q;

	for (q = 0; q < num; q++) {
		MMAL_BUFFER_HEADER_T *buffer;
		buffer	= mmal_queue_get(state.video_pool->queue);
		mmal_port_send_buffer(camera_video_port, buffer);
	}

	while (1) {
		usleep(15000);		// 15ms
	}

	//////////////////////////////////////////////////////////
	// end of process
	//////////////////////////////////////////////////////////
	vcos_semaphore_delete(&callback_data.complete_semaphore);

	mmal_status_to_int(status);

	// Disable all our ports that are not handled by connections
	if (state.camera_component) {
		mmal_component_disable(state.camera_component);
	}

	//	destroy_camera_component(&state);
	if (state.camera_component) {
		mmal_component_destroy(state.camera_component);
		state.camera_component = NULL;
	}

	opencv_func_stop();
	outImage.release();

	return 0;
}

// end of file
