/*
 * Copyright (c) 2015-2016, 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 "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		// camera frame size
#define	DEFAULT_HEIGHT		240		// camera frame size

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

#define	THREAD_NUM			4
//#define	SPECIFY_CPUCORE

#define	TIMEOUT_VAL			30000	// (unit: ms) socket time out

//#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_in, t1_in, t0_out, t1_out;
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;


typedef struct _thread_arg {
	int				thread_id;

	RASPIVID_STATE	*rasp_state;

	pthread_t		th;
	volatile char	th_inrdy;		// NOTE: use volatile
	volatile char	th_outrdy;		// NOTE: use volatile
	pthread_mutex_t	th_rdyflag;

	cv::Mat			th_in;
	cv::Mat			th_out;

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// input/output data to/from imag processing
// ********************************************************** << CQ
} THREAD_ARG;

THREAD_ARG	th_arg_G[THREAD_NUM];

static int	next_in_thread_G	= 0;
static int	next_out_thread_G	= 0;


// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// definitions, global variables, sub-functions, etc.
// ********************************************************** << CQ


////////////////////////////////////////////////////////////////////////////////
// socket (NODE)
////////////////////////////////////////////////////////////////////////////////
int		socket_fd;				// global

static char		node_hostname[256];
static int		node_port;

void dispip(FILE *fp, char *ipaddr)
{
	char disp_ipaddr[4];
	bcopy(ipaddr,disp_ipaddr,4);
	fprintf(fp, "%u.%u.%u.%u",
				(u_char)disp_ipaddr[0],
				(u_char)disp_ipaddr[1],
				(u_char)disp_ipaddr[2],
				(u_char)disp_ipaddr[3]
			);
}


int init_node_socket(char *hostname, int port)
{
	u_short				sock;
	struct sockaddr_in	addr;
	struct hostent		*node_host;

	// socket
	if ((sock = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
//		fprintf(stderr, "NODE: failed socket()\n");
		return -1;
	}

	// get IP addr of server
	if ((node_host = gethostbyname(hostname)) == NULL) {
//		fprintf(stderr, "NODE: failed gethostbyname(%s)\n", hostname);
		return -1;
	}

	// connect
	bzero((char *)(&addr), sizeof (addr));	// init structure
	addr.sin_family	= AF_INET;
	addr.sin_port	= htons(port);
//	inet_aton(ip_addr, &addr.sin_addr);
	bcopy(node_host->h_addr, (char *)(&addr.sin_addr), node_host->h_length);

	if (connect(sock, (const struct sockaddr *)(&addr), sizeof (addr)) == -1) {
//		fprintf(stderr, "NODE: failed connect()\n");
		return -1;
	}

//	// set to NON-BLOCKING mode
//	u_long	tmp	= 1;
//	ioctl(sock, FIONBIO, &tmp);

//	fprintf(stderr, "NODE: socket initialized [");
//	dispip(stderr, node_host->h_addr);
//	fprintf(stderr, ":%d]\n", port);

	return (int)sock;
}


////////////////////////////////////////////////////////////////////////////////
// thread func
////////////////////////////////////////////////////////////////////////////////
void *thread_func(void *arg)
{
	int						id;
	THREAD_ARG				*th_arg;
	IMG_PROCESSING_PARAM	img_param;

	th_arg	= (THREAD_ARG *)arg;
	id		= th_arg->thread_id;

	pthread_mutex_lock(&(th_arg->th_rdyflag));
	th_arg->th_outrdy	= 0;
	th_arg->th_inrdy	= 1;		// notify thread is generated
	pthread_mutex_unlock(&(th_arg->th_rdyflag));

	img_param.srcImage	= &(th_arg->th_in);
	img_param.dstImage	= &(th_arg->th_out);

	fprintf(stderr, "thread %d start\n", id);

	while (1) {
		//	inrdy	outrdy
		//	1		0			1.Waiting input
		//	0		0			2.Image processing
		//	0		1			3.Waiting output
		//	0		0			4.Done

		// wait input (status <1,0>)
		while (th_arg->th_inrdy == 1) {
			usleep(100);
			continue;
		}

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// copy other input data to image processing (from th_arg to img_param)
// ********************************************************** << CQ

		// process image (status <0, 0>)
		opencv_func_run(&img_param);

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// 
// copy other output data of image processing (from img_param to th_arg)
// ********************************************************** << CQ

		pthread_mutex_lock(&(th_arg->th_rdyflag));
		th_arg->th_outrdy	= 1;
		pthread_mutex_unlock(&(th_arg->th_rdyflag));

		// wait output (status <0,1>)
		while (th_arg->th_outrdy == 1) {
			usleep(100);
			continue;
		}

		pthread_mutex_lock(&(th_arg->th_rdyflag));
		th_arg->th_inrdy	= 1;
		pthread_mutex_unlock(&(th_arg->th_rdyflag));
	}
}


////////////////////////////////////////////////////////////////////////////////
// callback of video frame capture
////////////////////////////////////////////////////////////////////////////////
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_in	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);

			if (t1_in - t0_in >= fps_intval) {		// finish waiting
				if ((th_arg_G[next_in_thread_G]).th_inrdy == 1) { // check thread input
					int	w, h;

					mmal_buffer_header_mem_lock(buffer);
					w	= pData->pstate->width;
					h	= pData->pstate->height;

#ifdef IMG_COLOR
					memcpy(((th_arg_G[next_in_thread_G]).th_in).data, buffer->data, w * h * 3);	// set image
#endif	// IMG_COLOR
#ifdef IMG_GRAY
					memcpy(((th_arg_G[next_in_thread_G]).th_in).data, buffer->data, w * h);	// set image
#endif	// IMG_GRAY

					mmal_buffer_header_mem_unlock(buffer);

					pthread_mutex_lock(&(th_arg_G[next_in_thread_G]).th_rdyflag);
					(th_arg_G[next_in_thread_G]).th_inrdy	= 0;	// change this flag after setting image
					pthread_mutex_unlock(&(th_arg_G[next_in_thread_G]).th_rdyflag);

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

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// copy other information to input thread (to a member of th_arg_G[next_in_thread_G])
// ********************************************************** << CQ

					next_in_thread_G++;
					if (next_in_thread_G >= THREAD_NUM) {
						next_in_thread_G	= 0;
					}

					clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
					t1_in	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);
					t0_in	= t1_in;
				}
				// else ... discard this frame
			}
			// 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				outImage;

	int		i;

	//////////////////////////////////////////////////////////
	// socket (NODE)
	//////////////////////////////////////////////////////////
	if (argc < 3) {
		fprintf(stderr, "usage: command_name server_hostname server_port\n");
		exit(1);
	}

	strcpy(node_hostname, argv[1]);
	node_port	= atoi(argv[2]);
	socket_fd	= -1;

	//////////////////////////////////////////////////////////
	// 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));
// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
	// see RaspiCamControl.c/h
// ********************************************************** << CQ

	opencv_func_init((void *)NULL);

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

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

	// init thread parameters
	for (i = 0; i < THREAD_NUM; i++) {
		(th_arg_G[i]).thread_id		= i;
		(th_arg_G[i]).rasp_state	= &(state);

		pthread_mutex_init(&((th_arg_G[i]).th_rdyflag), NULL);
		(th_arg_G[i]).th_inrdy	= 0;	// NOTE: set to 0 prior to generating thread
		(th_arg_G[i]).th_outrdy	= 0;

#ifdef IMG_COLOR
		((th_arg_G[i]).th_in).create(cv::Size(state.width, state.height),  CV_8UC3);
		((th_arg_G[i]).th_out).create(cv::Size(state.width, state.height),  CV_8UC3);
#endif
#ifdef IMG_GRAY
		((th_arg_G[i]).th_in).create(cv::Size(state.width, state.height),  CV_8UC1);
		((th_arg_G[i]).th_out).create(cv::Size(state.width, state.height),  CV_8UC1);
#endif
	}

	// create thread
	pthread_attr_t	attr;
	cpu_set_t		cpus;
    pthread_attr_init(&attr);

	for (i = 0; i < THREAD_NUM; i++) {
		CPU_ZERO(&cpus);
		CPU_SET(i % 4, &cpus);
		pthread_attr_setaffinity_np(&attr, sizeof(cpu_set_t), &cpus);

#ifdef SPECIFY_CPUCORE
		pthread_create(&((th_arg_G[i]).th), &attr, thread_func, (void *)(&(th_arg_G[i])));
#else
		pthread_create(&((th_arg_G[i]).th), NULL, thread_func, (void *)(&(th_arg_G[i])));
#endif
	}

	// confirm all threads are generated
	while (1) {
		int	break_flag;
		break_flag	= 1;
		for (i = 0; i < THREAD_NUM; i++) {
			if ((th_arg_G[i]).th_inrdy == 0) {
				break_flag	= 0;
				break;
			}
		}
		if (break_flag == 1)
			break;
	}

	fprintf(stderr, "all thread ready\n");

	//////////////////////////////////////////////////////////
	// start capture
	//////////////////////////////////////////////////////////
	clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
	t0_in	= t0_out	= (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) {
		// output
		if ((th_arg_G[next_out_thread_G]).th_outrdy == 1) {
#ifdef IMG_COLOR
			memcpy(outImage.data, ((th_arg_G[next_out_thread_G]).th_out).data, state.width * state.height * 3);
#endif
#ifdef IMG_GRAY
			memcpy(outImage.data, ((th_arg_G[next_out_thread_G]).th_out).data, state.width * state.height);
#endif

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// copy other information from output thread (from a member of th_arg_G[next_out_thread_G])
// ********************************************************** << CQ

			pthread_mutex_lock(&((th_arg_G[next_out_thread_G]).th_rdyflag));
			(th_arg_G[next_out_thread_G]).th_outrdy	= 0;			// change this flag after getting image
			pthread_mutex_unlock(&((th_arg_G[next_out_thread_G]).th_rdyflag));

			next_out_thread_G++;
			if (next_out_thread_G >= THREAD_NUM) {
				next_out_thread_G	= 0;
			}

// ********************************************************** >> CQ
// >> ADD YOUR ORIGINAL CODE HERE
// use image processing result (outImage)
// ********************************************************** << CQ

//			cv::imshow(WINNAME, outImage);
//			cv::waitKey(1);

			//////////////////////////////////////////////////////////
			// socket (NODE)
			//////////////////////////////////////////////////////////
			ssize_t	rlen;
			char	rbuf;

			if (socket_fd >= 0) {
//				rlen	= recv(socket_fd, &rbuf, 1, MSG_DONTWAIT);	// NOTE: non-blocking
				rlen	= read(socket_fd, &rbuf, 1);	// NOTE: blocking
			}

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

			if (socket_fd >= 0 && t1_out - t0_out > (long)TIMEOUT_VAL) {
				fprintf(stderr, "%d: time out caused by no request from host.\n", t1_out);
				close(socket_fd);
				socket_fd	= -1;
//				exit(1);
			}

			if (socket_fd < 0) {
				socket_fd	= init_node_socket(node_hostname, node_port);
				if (socket_fd >= 0) {
					clock_gettime(CLOCK_MONOTONIC_RAW, &tspec);
					t0_out	= (tspec.tv_nsec / 1000000) + (tspec.tv_sec * 1000);

					fprintf(stderr, "%d: connected to server (%d)\n", t0_out, socket_fd);
				}
				else {
					fprintf(stderr, "cannot connect to server\n");
					exit(1);
				}

				rlen	= 0;
			}

			if (socket_fd >= 0 && rlen >= 1) {	// if there is a request from HUB
				// jpeg compression
				std::vector<uchar>	encbuf;
				std::vector<int>	enc_param = std::vector<int>(2);
				enc_param[0]	= CV_IMWRITE_JPEG_QUALITY;
				enc_param[1]	= DEFAULT_JPG_QUALITY;		// 0-100, default(95)
				imencode(".jpg", outImage, encbuf, enc_param);

				// write to socket
				int		len	= encbuf.size();
				int		sum;
				char	*buf;
				buf	= (char *)malloc(sizeof (char) * (len + 12));	// +12 ... header [data_size, x_size, y_size, checksum]

				*(buf + 0)	= (len >> 24) & 0xFF;
				*(buf + 1)	= (len >> 16) & 0xFF;
				*(buf + 2)	= (len >> 8)  & 0xFF;
				*(buf + 3)	= len & 0xFF;

				*(buf + 4)	= (state.width >> 8) & 0xFF;
				*(buf + 5)	= state.width & 0xFF;

				*(buf + 6)	= (state.height >> 8) & 0xFF;
				*(buf + 7)	= state.height & 0xFF;

				sum	= 0;
				for (int i = 0; i < len; i++) {
					char	ch;
					ch				= (char)(encbuf.at(i));
					*(buf + i + 12)	= ch;
					sum				+= ch;
				}

				*(buf + 8)	= (sum >> 24) & 0xFF;
				*(buf + 9)	= (sum >> 16) & 0xFF;
				*(buf + 10)	= (sum >> 8)  & 0xFF;
				*(buf + 11)	= sum & 0xFF;

//				send(socket_fd, buf, len + 12, MSG_DONTWAIT);		// NOTE: non-blocking
				write(socket_fd, buf, len + 12);		// NOTE: blocking
				free(buf);

//fprintf(stderr, "%d: %dbytes (%d,%d)\n", t1_out, len, state.width, state.height);

				t0_out	= t1_out;
			}
			// else ... discard frame

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

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

		usleep(100);		// 0.1ms
	}

	//////////////////////////////////////////////////////////
	// 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((void *)NULL);
	outImage.release();

	return 0;
}

// end of file
