/*
 * Copyright (c) 2019, Sumio Morioka
 * All rights reserved.
 *
 * This source code was originally written by Dr.Sumio Morioka for use in the 2019 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 <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

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

#include "interface/mmal/mmal.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.hpp"

// ********************************************************** >> CQ
// >> CHANGE FOLLOWING DEFS IF NECESSARY

#define	DEFAULT_WIDTH		288
#define	DEFAULT_HEIGHT		216

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

#define	DISP_FPS
// ********************************************************** << 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.

void gpio_export(int pin)
{
	int		hd_gpio;
	char	wbuf[5];
	if ((hd_gpio = open("/sys/class/gpio/export", O_WRONLY)) < 0) {
		fprintf(stderr, "cannot open gpio_export (forget to sudo?)\n");
		return;
	}
	sprintf(wbuf, "%d", pin);
	write(hd_gpio, wbuf, strlen(wbuf));
	close(hd_gpio);
}


void gpio_setdir(int pin, int dir)
{
	int		hd_gpio;
	char	wbuf[100];
	sprintf(wbuf, "/sys/class/gpio/gpio%d/direction", pin);
	if ((hd_gpio = open(wbuf, O_WRONLY)) < 0) {
		fprintf(stderr, "cannot open %s (forget to sudo?)\n", wbuf);
		return;
	}
	if (dir == 0) {
		sprintf(wbuf, "out");
	}
	else {
		sprintf(wbuf, "in");
	}
	write(hd_gpio, wbuf, strlen(wbuf));
	close(hd_gpio);
}


void gpio_out(int pin, int val)
{
	int		hd_gpio;
	char	wbuf[100];
	sprintf(wbuf, "/sys/class/gpio/gpio%d/value", pin);
	if ((hd_gpio = open(wbuf, O_WRONLY)) < 0) {
		fprintf(stderr, "cannot open %s (forget to sudo?)\n", wbuf);
		return;
	}
	if (val == 0) {
		sprintf(wbuf, "0");
	}
	else {
		sprintf(wbuf, "1");
	}
	write(hd_gpio, wbuf, strlen(wbuf));
	close(hd_gpio);
}


// camera switcher
#define	CAM_LEFT	0
#define	CAM_RIGHT	1

volatile char	twincam_sw	= CAM_LEFT;
volatile int	twincam_ctr	= 0;
volatile int	first_capture	= 1;

void init_twincam(void)
{
	gpio_export(4);
	gpio_export(17);
	gpio_export(18);

	gpio_setdir(4,  0);		// OUT
	gpio_setdir(17, 0);		// OUT
	gpio_setdir(18, 0);		// OUT
}


void set_twincam_L(void)
{
	twincam_sw	= CAM_LEFT;
	twincam_ctr	= 0;

	gpio_out(4,  0);
	gpio_out(17, 0);
	gpio_out(18, 1);
}


void set_twincam_R(void)
{
	twincam_sw	= CAM_RIGHT;
	twincam_ctr	= 0;

	gpio_out(4,  0);
	gpio_out(17, 1);
	gpio_out(18, 0);
}


///////////////////////////////////////////////////////////////////////////////////////////////////

#define PI			3.14159265358979
#define rad2deg(x)	(((x) / PI) * 180.0)
#define deg2rad(x)	(((x) / 180.0) * PI)

#define	L_CTR_X		140.0
#define	L_CTR_Y		97.0
#define	L_COEF		1.0

#define	R_CTR_X		155.0
#define	R_CTR_Y		101.0
#define	R_COEF		1.0

#define	DIST_L_R	0.1			// unit: m

#define	MATCH_THRESHOLD		8.0

#define	SEARCH_STEP			0.1			// pixels
#define	SEARCH_MAX			2.0			// pixels
#define	SEARCH_THRESHOLD	1.0			// meter

#define	SHORT_RANGE			1.0
#define	MIDDLE_RANGE		3.0
#define	LONG_RANGE			10.0

#define	OUTNUM_SHORT_RANGE	3
#define	OUTNUM_MIDDLE_RANGE	3
#define	OUTNUM_LONG_RANGE	2

#define	L_DISP_X_OFFSET		(-35)
#define	R_DISP_X_OFFSET		(2)


void distance_calc_agl(
	double	d,			// L-R distance
	double	cx_l,		// L:center
	double	cy_l,
	double	x_l,		// L:pos
	double	y_l,
	double	ratio_l,	// L:coef
	double	cx_r,		// R:center
	double	cy_r,
	double	x_r,		// R:pos
	double	y_r,
	double	ratio_r,	// R:coef
	double	*d_l,		// L:result
	double	*d_r		// R:result
)
{
	double	dx_l, dy_l;
	double	dx_r, dy_r;

	// 左レンズ
	dx_l	= x_l  - cx_l;
	dy_l	= cy_l - y_l;

	// 右レンズ
	dx_r	= x_r  - cx_r;
	dy_r	= cy_r - y_r;

	////////////////////////////////////////////////////////////////////////////////////
	// θrot_+y計算
	////////////////////////////////////////////////////////////////////////////////////
	double	r_l, theta_l_deg, theta_l;
	double	r_r, theta_r_deg, theta_r;

	// 左レンズ
	r_l		= sqrt(dx_l * dx_l + dy_l * dy_l);
	if (ratio_l != 0.0)
		theta_l_deg	= r_l / ratio_l;
	else
		theta_l_deg	= 0.0;
	theta_l	= deg2rad(theta_l_deg);

	// 右レンズ
	r_r		= sqrt(dx_r * dx_r + dy_r * dy_r);
	if (ratio_r	!= 0.0)
		theta_r_deg	= r_r / ratio_r;
	else
		theta_r_deg	= 0.0;
	theta_r	= deg2rad(theta_r_deg);

	////////////////////////////////////////////////////////////////////////////////////
	// φ計算
	////////////////////////////////////////////////////////////////////////////////////
	double	phi_l, phi_r;

	// 左レンズ
	phi_l	= atan2(dy_l, dx_l);

	// 右レンズ
	phi_r	= atan2(dy_r, dx_r);

	////////////////////////////////////////////////////////////////////////////////////%
	// 距離計算
	////////////////////////////////////////////////////////////////////////////////////%
	double	divider;

	divider	= sin(theta_l) * cos(phi_l) * cos(theta_r) - sin(theta_r) * cos(phi_r) * cos(theta_l);

	if (divider != 0.0) {
		*d_l	= (cos(theta_r) / divider) * d;
		*d_r	= (cos(theta_l) / divider) * d;
	}
	else {
		*d_l	= -1.0;
		*d_r	= -1.0;
	}
}


void search_distance_calc_agl(
	double	d,			// L-R distance
	double	cx_l,		// L:center
	double	cy_l,
	double	x_l,		// L:pos
	double	y_l,
	double	ratio_l,	// L:coef
	double	cx_r,		// R:center
	double	cy_r,
	double	x_r,		// R:pos
	double	y_r,
	double	ratio_r,	// R:coef
	double	*d_l,		// L:result
	double	*d_r		// R:result
)
{
	double	diff_l, diff2_l;
	double	new_l, new_r;
	double	prev_l, prev_r;

	double	idx;
	int		initflag;

	initflag	= 1;

#define	SEARCH_THRESHOLD	1.0			// meter
	for (idx = 0.0; idx < SEARCH_MAX; idx += SEARCH_STEP) {

		distance_calc_agl(
					d,			// L-R distance
					cx_l,		// L:center
					cy_l,
					x_l + idx,	// *** L:pos
					y_l,
					ratio_l,	// L:coef
					cx_r,		// R:center
					cy_r,
					x_r - idx,	// *** R:pos
					y_r,
					ratio_r,	// R:coef
					&new_l,		// L:result
					&new_r		// R:result
		);

		if (initflag == 1) {
			initflag	= 0;
			*d_l		= new_l;
			*d_r		= new_r;
			diff2_l		= SEARCH_THRESHOLD * 10000.0;	// 10000.0 ... margin
			diff_l		= SEARCH_THRESHOLD * 10000.0;
		}
		else {
			if (new_l <= 0.0 || new_r <= 0.0) {
				new_l	= SEARCH_THRESHOLD * (-10000.0);
				new_r	= SEARCH_THRESHOLD * (-10000.0);
				diff2_l	= SEARCH_THRESHOLD * 10000.0;
				diff_l	= SEARCH_THRESHOLD * 10000.0;
				prev_l	= new_l;
			}
			else {	// normal case
				diff2_l	= diff_l;
				diff_l	= prev_l - new_l;
				prev_l	= new_l;

				if (diff2_l < SEARCH_THRESHOLD && diff_l < SEARCH_THRESHOLD) {
					*d_l	= new_l;
					*d_r	= new_r;
					break;
				}
			}
		}
	}
}


int orthant(int cx, int cy, int x, int y)
{
	if (x >= cx && y >= cy)
		return 1;
	else if (x < cx && y >= cy)
		return 2;
	else if (x < cx && y < cy)
		return 3;
	else
		return 4;
}


void match_img(cv::Mat *l_img, cv::Mat *r_img, cv::Mat *dst_img)
{
	cv::Mat	img1 = *l_img;
	cv::Mat	img2 = *r_img;

	cv::hconcat(img1, img2, *dst_img);

	// 特徴点抽出
	auto akaze	= cv::AKAZE::create();
	std::vector<cv::KeyPoint>	keypoint1, keypoint2;
	akaze->detect(img1, keypoint1);
	akaze->detect(img2, keypoint2);

	// 特徴記述
	cv::Mat	descriptor1, descriptor2;
	akaze->compute(img1, keypoint1, descriptor1);
	akaze->compute(img2, keypoint2, descriptor2);

	// マッチング
	cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce");

	std::vector<cv::DMatch> match, match_use;
	matcher->match(descriptor1, descriptor2, match);

	double	min_dist	= -1.0;
	for (size_t i = 0; i < match.size(); i++) {
		cv::DMatch forward	= match[i];
		if (min_dist < 0 || forward.distance < min_dist)
			min_dist	= forward.distance;
	}

	int	cnt_short	= 0;
	int	cnt_mid		= 0;
	int	cnt_long	= 0;

	int	clr	= 1;

	for (size_t i = 0; i < match.size(); i++) {
		cv::DMatch forward	= match[i];

		if (forward.distance < min_dist * MATCH_THRESHOLD) {
			int			idx1, idx2;
			cv::Point2f	coor1, coor2;
			int			x1, y1, x2, y2;
			double		l1, l2;

			// 対応点の座標を求める
			idx1	= (int)(forward.queryIdx);
			coor1	= (keypoint1[idx1]).pt;
			x1		= (int)coor1.x;
			y1		= (int)coor1.y;

			idx2	= (int)(forward.trainIdx);
			coor2	= (keypoint2[idx2]).pt;
			x2		= (int)coor2.x;
			y2		= (int)coor2.y;

			search_distance_calc_agl(
					DIST_L_R,
					L_CTR_X, L_CTR_Y, (double)x1, (double)y1, L_COEF,
					R_CTR_X, R_CTR_Y, (double)x2, (double)y2, R_COEF,
					&l1, &l2
				);

			int	orthant1	= orthant(L_CTR_X, L_CTR_Y, x1, y1);
			int	orthant2	= orthant(R_CTR_X, R_CTR_Y, x2, y2);

			// 対応点のうち距離が正常なものを登録
			if (l1 > 0.0 && l2 > 0.0
					&& (   (l2 <= l1 && l1 < DIST_L_R + l2)
						|| (l1 <= l2 && l2 < DIST_L_R + l1) )
					&& (   (orthant1 == 1 && orthant2 != 3)
						|| (orthant1 == 2 && orthant2 != 4)
						|| (orthant1 == 3 && orthant2 != 1)
						|| (orthant1 == 4 && orthant2 != 2) )
				) {

				int	push_back	= 0;
				if (l1 <= SHORT_RANGE && cnt_short < OUTNUM_SHORT_RANGE) {
					cnt_short++;
					push_back	= 1;
				}
				else if (SHORT_RANGE < l1 && l1 <= LONG_RANGE && cnt_mid < OUTNUM_MIDDLE_RANGE) {
					cnt_mid++;
					push_back	= 1;
				}
				else if (LONG_RANGE < l1 && cnt_long < OUTNUM_LONG_RANGE) {
					cnt_long++;
					push_back	= 1;
				}

				if (push_back == 1) {
					char	strbuf[1024];
					int		r, g, b;

					r	= ((clr & 0x01) == 0) ? 0 : 255;
					g	= ((clr & 0x02) == 0) ? 0 : 255;
					b	= ((clr & 0x04) == 0) ? 0 : 255;
					clr++;
					if (clr >= 8) {
						clr	= 1;
					}

					match.push_back(forward);
//					fprintf(stderr, "1[x=%d, y=%d] l=%f <==> 2[x=%d, y=%d] l=%f\n", x1, y1, l1, x2, y2, l2);

					// note: 16 == CV_AA

					sprintf(strbuf, "%.2f", l1);
					cv::putText(*dst_img, strbuf, cv::Point(x1 + L_DISP_X_OFFSET, y1), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(0,0,0), 2, 16);
					cv::putText(*dst_img, strbuf, cv::Point(x1 + L_DISP_X_OFFSET, y1), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(b,g,r), 1, 16);

					sprintf(strbuf, "%.2f", l2);
					cv::putText(*dst_img, strbuf, cv::Point(x2 + R_DISP_X_OFFSET + img2.cols, y2), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(0,0,0), 2, 16);
					cv::putText(*dst_img, strbuf, cv::Point(x2 + R_DISP_X_OFFSET + img2.cols, y2), cv::FONT_HERSHEY_DUPLEX, 0.5, cv::Scalar(b,g,r), 1, 16);

					cv::line(*dst_img, cv::Point(x1, y1), cv::Point(x2 + img2.cols, y2), cv::Scalar(b, g, r), 1, 16);
				}
			}
		}
	}

//	cv::drawMatches(img1, keypoint1, img2, keypoint2, match, *dst_img);
}


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


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

static volatile char	update_result	= 0;


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)

				if (twincam_ctr >= 2) {
					if (twincam_sw == CAM_LEFT) {
						set_twincam_R();
						cv::cvtColor(*(img_param_G.dstImage), *(img_param_G.L_image), cv::COLOR_RGB2BGR);
//						(*(img_param_G.dstImage)).copyTo(*(img_param_G.L_image));
					}
					else {
						set_twincam_L();
						cv::cvtColor(*(img_param_G.dstImage), *(img_param_G.R_image), cv::COLOR_RGB2BGR);
//						(*(img_param_G.dstImage)).copyTo(*(img_param_G.R_image));

						if (first_capture == 1) {
							first_capture	= 0;
							cv::imwrite("rpi_left.png",  *(img_param_G.L_image));
							cv::imwrite("rpi_right.png", *(img_param_G.R_image));
						}

						cv::Mat		dst;
						match_img(img_param_G.L_image, img_param_G.R_image, &dst);
						cv::resize(dst, *(img_param_G.D_image), (*(img_param_G.D_image)).size(), 0, 0, 1);	// 1...INTER_LINEAR

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

						update_result	= 1;
					}
				}
				else {
					twincam_ctr++;
				}

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

				if (update_result == 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;
					update_result	= 0;
				}
			}
			// 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

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

	//////////////////////////////////////////////////////////
	// init camera
	//////////////////////////////////////////////////////////
	init_twincam();
	set_twincam_L();
	usleep(100000);		// 100ms

	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

	cv::Mat		L_image, R_image, D_image;

	L_image.create(cv::Size(state.width, state.height), CV_8UC1);
	R_image.create(cv::Size(state.width, state.height), CV_8UC1);
	D_image.create(cv::Size(state.width * 4, state.height * 2), CV_8UC1);

	img_param_G.L_image	= &(L_image);
	img_param_G.R_image	= &(R_image);
	img_param_G.D_image	= &(D_image);

// ********************************************************** << 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
