File content as of revision 0:d2c78848026d:

/*
 * Copyright (c) 2014, Sumio Morioka
 * All rights reserved.
 *
 * This source code was originally written by Dr.Sumio Morioka for use in the Nov 2014 issue 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.
 *
 * - Distribution date of this code: Sep 24, 2014
 * - Author: Dr.Sumio Morioka (http://www002.upp.so-net.ne.jp/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 "mbed.h"
#include "TextLCD.h"
#include "LocalFileSystem.h"

#include "ov7670.h"

TextLCD lcd(p24, p26, p27, p28, p29, p30);
DigitalOut led1(LED1), led2(LED2), led3(LED3), led4(LED4);
PwmOut sv0(p21), sv1(p22), sv2(p23);

LocalFileSystem local("webfs");

OV7670 camera(
    p9,p10,			// SDA,SCL(I2C / SCCB)
    p5,p6,p7,		// VSYNC,HREF,WEN(FIFO)
    p20,p19,p18,p17,p16,p15,p14,p13,	// D7-D0
    p8,p11,p12);	// RRST,OE,RCLK

Timer tmr;

//#define QQVGA
#define QVGA
//#define VGA34

#ifdef QQVGA
#define SIZEX 160
#define SIZEY 120
#endif

#ifdef QVGA
#define SIZEX 320
#define SIZEY 240
#endif

#ifdef VGA34
#define SIZEX 480
#define SIZEY 360
#endif


#define	SHUTTER_THRESHOLD	3500	// # of skin color pixels


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

void rgb2hsv(
	unsigned char	rval,
	unsigned char	gval,
	unsigned char	bval,

	unsigned char	*hval,
	unsigned char	*sval,
	unsigned char	*vval
)
{
	unsigned char	max, min;
	int				ret;

	max	= (rval >= gval) ? rval : gval;
	max	= (bval >= max)  ? bval : max;

	min	= (rval <= gval) ? rval : gval;
	min	= (bval <= min)  ? bval : min;

	///////////////////////////////////////////
	// compute H
	///////////////////////////////////////////
	if (max == min)
		ret	= 0;	// (undef)
	else if (rval >= gval && rval >= bval)	// max: R
		ret	= ((gval - bval) * 60) / (max - min);
	else if (gval >= rval && gval >= bval)	// max: G
		ret	= ((bval - rval) * 60) / (max - min) + 120;
	else	// max: B
		ret	= ((rval - gval) * 60) / (max - min) + 240;

	ret	%= 360;
	if (ret < 0)
		ret	+= 360;

	*hval	= (ret * 255) / 360;

	///////////////////////////////////////////
	// compute S
	///////////////////////////////////////////
	if (max == 0)
		ret	= 0;	// (undef)
	else
		ret	= ((max - min) * 255) / max;

	*sval	= ret;

	///////////////////////////////////////////
	// compute V
	///////////////////////////////////////////
	*vval	= max;
}

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

unsigned char judge_skin_color(
	unsigned char	hval,
	unsigned char	sval,
	unsigned char	vval
)
{
	if (hval <= 70 && sval >= 25 && sval <= 70 && vval >= 180 && vval <= 250)	// <rgain,ggain,bgain> == <0x48,0x40,0x60>
		return (255);
	else
		return (0);
}

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

typedef	unsigned char (*kernel_func)(unsigned char *linebuf, int x, int y, int line_size, int kernel_size, void *param);	// kernel operation function

typedef struct _stream_filter
{
	// input pixel data
	unsigned char	d_in0;			// plane 0
	unsigned char	d_in1;			// plane 1
	unsigned char	d_in2;			// plane 2
	unsigned char	d_in_attr;		// data attribute (see the definitions below)

	// output pixel data
	unsigned char	d_out0;			// plane 0
	unsigned char	d_out1;			// plane 1
	unsigned char	d_out2;			// plane 2
	unsigned char	d_out_attr;		// data attribute (see the definitions below)

	// output data
	char			is_3_planes;	// 0: single plane, Others: 3 planes

	////////////////////////////////////////
	// internal variables (do not touch)
	////////////////////////////////////////
	char			state;			// internal 

	int				d_in_cnt;		// # of total input
	int				d_in_x;			// x-coordinate of input position on "line_buf" (not on entire screen)
	int				d_in_y;			// y-coordinate

	int				d_out_cnt;		// # of total output
	int				d_out_x;		// x-coordinate of center of kernel on "line_buf" (not on entire screen)
	int				d_out_y;		// y-coordinate

	int				line_size;		// width
	int				kernel_size;	// kernel size (MUST be odd)

	unsigned char	*line_buf0;		// line buffer for plane 0
	unsigned char	*line_buf1;		// plane 1
	unsigned char	*line_buf2;		// plane 2

	kernel_func		func;			// pointer to a kernel operation function
	void			*func_param;	// parameter to the above function (ex. pointer to coefficient matrix etc.)
} stream_filter;


#define	ATTR_1ST		0		// 1st pixel data
#define	ATTR_VALID		1		// valid pixel data (not 1st nor last)
#define	ATTR_LAST		2		// last pixel data
#define	ATTR_INVALID	3		// invalid pixel data

#define	STATE_PROCESS	0		// do not touch
#define	STATE_FLUSH		1		// do not touch

#ifdef DEBUG
FILE *fp_his;
#endif	// DEBUG


///////////////////--> kernel func
unsigned char comb_filter(unsigned char *linebuf, int x, int y, int line_size, int kernel_size, void *matrix_void)
{
	float	acc		= 0.0f;
	int		mat_idx	= 0;

	////////////////////////////////////////////// ---> customize here
	float	*matrix	= (float *)matrix_void;
	////////////////////////////////////////////// <--- customize here

#ifdef DEBUG
	fprintf(fp_his, "\tpixel: ");
#endif

	// scan kernel
	for (int ky = y - ((kernel_size - 1) / 2); ky <= y + (kernel_size - 1) / 2; ky++) {
		int		ly;
		// (rounding linebuf)
		if (ky < 0)
			ly	= ky + (kernel_size + 1);
		else if (ky >= kernel_size + 1)
			ly	= ky - (kernel_size + 1);
		else
			ly	= ky;

		for (int kx = x - ((kernel_size - 1) / 2); kx <= x + (kernel_size - 1) / 2; kx++, mat_idx++) {
			if (kx >= 0 && kx < line_size) {
				float	pixel;
				pixel	= (float)(*(linebuf + (ly * line_size) + kx));

				////////////////////////////////////////////// ---> customize here
#ifdef DEBUG
				fprintf(fp_his, "%02x ", *(linebuf + (ly * line_size) + kx));
#endif
				acc		+= pixel * (*(matrix + mat_idx));
				////////////////////////////////////////////// <--- customize here
			}
		}

#ifdef DEBUG
		fprintf(fp_his, ", ");
#endif
	}

#ifdef DEBUG
	fprintf(fp_his, "(ret %d)\n", (unsigned char)(acc + 0.5f));
#endif

	return ((unsigned char)(acc + 0.5f));
}
///////////////////<-- kernel func


void dump_stream_filter(FILE *fp, stream_filter *flt)	// for debug; dump contents of the structure data
{
	fprintf(fp, "d_in %02x ", flt->d_in0);
	if (flt->d_in_attr == ATTR_1ST)				fprintf(fp, "ATTR_1ST");
	else if (flt->d_in_attr == ATTR_VALID)		fprintf(fp, "ATTR_VALID");
	else if (flt->d_in_attr == ATTR_LAST)		fprintf(fp, "ATTR_LAST");
	else if (flt->d_in_attr == ATTR_INVALID)	fprintf(fp, "ATTR_INVALID");
	else										fprintf(fp, "illegal attribute");
	fprintf(fp, "\tcnt %d, x %d, y %d\n", flt->d_in_cnt, flt->d_in_x, flt->d_in_y);

	fprintf(fp, "d_out %02x ", flt->d_out0);
	if (flt->d_out_attr == ATTR_1ST)			fprintf(fp, "ATTR_1ST");
	else if (flt->d_out_attr == ATTR_VALID)		fprintf(fp, "ATTR_VALID");
	else if (flt->d_out_attr == ATTR_LAST)		fprintf(fp, "ATTR_LAST");
	else if (flt->d_out_attr == ATTR_INVALID)	fprintf(fp, "ATTR_INVALID");
	else										fprintf(fp, "illegal attribute");
	fprintf(fp, "\tcnt %d, x %d, y %d\n", flt->d_out_cnt, flt->d_out_x, flt->d_out_y);

	if (flt->state == STATE_PROCESS)			fprintf(fp, "state: STATE_PROCESS\n");
	else										fprintf(fp, "state: STATE_FLUSH\n");

	fprintf(fp, "\n");
	for (int y = 0; y < flt->kernel_size + 1; y++) {
		for (int x = 0; x < flt->line_size; x++) {
			fprintf(fp, "%02x ", *(flt->line_buf0 + (y * flt->line_size) + x) );
		}
		fprintf(fp, "\n");
	}

	if (flt->is_3_planes != 0) {
		fprintf(fp, "\n");
		for (int y = 0; y < flt->kernel_size + 1; y++) {
			for (int x = 0; x < flt->line_size; x++) {
				fprintf(fp, "%02x ", *(flt->line_buf1 + (y * flt->line_size) + x) );
			}
			fprintf(fp, "\n");
		}

		fprintf(fp, "\n");
		for (int y = 0; y < flt->kernel_size + 1; y++) {
			for (int x = 0; x < flt->line_size; x++) {
				fprintf(fp, "%02x ", *(flt->line_buf1 + (y * flt->line_size) + x) );
			}
			fprintf(fp, "\n");
		}
	}

	fprintf(fp, "====================================================\n");
	fflush(fp);
}


// reset data structure for reuse
void reset_stream_filter(stream_filter *flt)
{
	if (flt == (stream_filter *)NULL)
		return;

	flt->state			= STATE_PROCESS;

	flt->d_in_cnt		= 0;								// total # of data
	flt->d_in_x			= 0;								// linebuf pointer
	flt->d_in_y			= (flt->kernel_size - 1) / 2;		// linebuf pointer

	flt->d_out_cnt		= 0;								// total # of data
	flt->d_out_x		= 0;								// linebuf pointer
	flt->d_out_y		= (flt->kernel_size - 1) / 2;		// linebuf pointer

	// zero clear buffer
	if (flt->line_buf0 != (unsigned char *)NULL) {
		for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
			*(flt->line_buf0 + i)	= 0;
		}
	}

	if (flt->line_buf1 != (unsigned char *)NULL) {
		for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
			*(flt->line_buf1 + i)	= 0;
		}
	}

	if (flt->line_buf2 != (unsigned char *)NULL) {
		for (int i = 0; i < flt->line_size * (flt->kernel_size + 1); i++) {
			*(flt->line_buf2 + i)	= 0;
		}
	}
}


// allocate and initialize data structure
stream_filter *create_stream_filter(char is_3_planes, int line_size, int kernel_size, kernel_func func, void *func_param)
{
	stream_filter	*ret;

	// allocate memory
	if ((ret = (stream_filter *)malloc(sizeof (stream_filter))) == (stream_filter *)NULL)
		return ((stream_filter *)NULL);

	if ((ret->line_buf0 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
		free(ret);
		return ((stream_filter *)NULL);
	}

	if (is_3_planes != 0) {
		if ((ret->line_buf1 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
			free(ret->line_buf0);
			free(ret);
			return ((stream_filter *)NULL);
		}

		if ((ret->line_buf2 = (unsigned char *)malloc(sizeof (unsigned char) * line_size * (kernel_size + 1))) == (unsigned char *)NULL) {
			free(ret->line_buf0);
			free(ret->line_buf1);
			free(ret);
			return ((stream_filter *)NULL);
		}
	}
	else {
		ret->line_buf1	= (unsigned char *)NULL;
		ret->line_buf2	= (unsigned char *)NULL;
	}

	// init internal valiables
	ret->is_3_planes	= is_3_planes;
	ret->line_size		= line_size;
	ret->kernel_size	= kernel_size;

	reset_stream_filter(ret);

	// init kernel operation func
	ret->func		= func;
	ret->func_param	= func_param;

	return (ret);
}


// process 1 pixel; Set d_in & d_in_attr before calling. The result will be set to d_out & d_out_attr.
void apply_filter(stream_filter *flt)
{
	if (flt == (stream_filter *)NULL)
		return;

	if ((flt->state == STATE_PROCESS && flt->d_in_attr != ATTR_INVALID)
			|| (flt->state == STATE_FLUSH && flt->d_in_attr == ATTR_INVALID)) {
		/////////////////////////////////////////////
		// store d_in to linebuf
		/////////////////////////////////////////////
		if (flt->d_in_attr == ATTR_INVALID) {
			flt->d_in0	= 0;
			flt->d_in1	= 0;
			flt->d_in2	= 0;
		}

		*(flt->line_buf0 + (flt->d_in_y * flt->line_size) + flt->d_in_x)	= flt->d_in0;
		if (flt->is_3_planes != 0) {
			*(flt->line_buf1 + (flt->d_in_y * flt->line_size) + flt->d_in_x)	= flt->d_in1;
			*(flt->line_buf2 + (flt->d_in_y * flt->line_size) + flt->d_in_x)	= flt->d_in2;
		}

		// update input counter
		if (flt->state == STATE_PROCESS && flt->d_in_attr != ATTR_LAST)
			(flt->d_in_cnt)++;

		// update input pointer
		(flt->d_in_x)++;
		if (flt->d_in_x >= flt->line_size) {
			flt->d_in_x	= 0;
			(flt->d_in_y)++;
			if (flt->d_in_y >= flt->kernel_size + 1) {
				flt->d_in_y	= 0;
			}
		}

		/////////////////////////////////////////////
		// calc output (NOTE: do after updating input counter)
		/////////////////////////////////////////////
		if ((flt->state == STATE_PROCESS && flt->d_in_cnt > flt->line_size * (flt->kernel_size + 1) / 2)
				|| (flt->state == STATE_FLUSH && flt->d_in_cnt >= flt->d_out_cnt)) {	// normal operation

			// generate output (call kernel_operation func)
			flt->d_out0	= (flt->func)(flt->line_buf0, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
			if (flt->is_3_planes != 0) {
				flt->d_out1	= (flt->func)(flt->line_buf1, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
				flt->d_out2	= (flt->func)(flt->line_buf2, flt->d_out_x, flt->d_out_y, flt->line_size, flt->kernel_size, flt->func_param);
			}

			if (flt->d_out_cnt == 0)
				flt->d_out_attr	= ATTR_1ST;
			else if (flt->d_out_cnt == flt->d_in_cnt)
				flt->d_out_attr	= ATTR_LAST;
			else
				flt->d_out_attr	= ATTR_VALID;

			// update output counter
			(flt->d_out_cnt)++;

			// update output pointer
			(flt->d_out_x)++;
			if (flt->d_out_x >= flt->line_size) {
				flt->d_out_x	= 0;
				(flt->d_out_y)++;
				if (flt->d_out_y >= flt->kernel_size + 1) {
					flt->d_out_y	= 0;
				}
			}
		}
		else {	// initial operation (still filling buffer) or completion of data output
			// no output
			flt->d_out0		= 0;
			flt->d_out1		= 0;
			flt->d_out2		= 0;
			flt->d_out_attr	= ATTR_INVALID;
		}

		/////////////////////////////////////////////
		// change state
		/////////////////////////////////////////////
		if (flt->state == STATE_PROCESS && flt->d_in_attr == ATTR_LAST) {
			flt->state	= STATE_FLUSH;
		}
	}
}


// release data structure
void release_stream_filter(stream_filter *flt)
{
	if (flt == (stream_filter *)NULL)
		return;
	if (flt->line_buf0 != (unsigned char *)NULL)
		free(flt->line_buf0);
	free(flt);
}


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


//void cam_cap(void);
int cam_cap(char, stream_filter *);


int memfree(void)
{
	int	ret = 1;
	while (1) {
		char	*p	= (char *)malloc(ret);
		if (p == NULL)
			break;
		free(p);
		ret++;
	}
	return (ret - 1);
}


int main() 
{
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;

	sv0.period_us(20000);		// 20ms
	sv0.pulsewidth_us(5000);	// 5ms

	sv1.period_us(20000);		// 20ms
	sv1.pulsewidth_us(10000);	// 10ms

	sv2.period_us(20000);		// 20ms
	sv2.pulsewidth_us(15000);	// 15ms
	
	////////////////////////////////////////////////////////////////////////////
    camera.WriteReg(0x12, 0x80);			// com7; reset
    wait_ms(200);

	camera.InitDefaultReg();

    // negate vsync
    camera.WriteReg(0x15, 0x02);			// com10; negative vsync

#ifdef QQVGA
	camera.InitQQVGA();
#endif
#ifdef QVGA
	camera.InitQVGA();
#endif
#ifdef VGA34
	camera.InitVGA_3_4();
#endif

    // data format
    camera.WriteReg(0x12, 0x04 + 0);    // com7 RGB	(bit1...test pattern)
    camera.WriteReg(0x40, 0xD0);    // com15 RGB565
    camera.WriteReg(0x8c, 0x00);    // RGB444

	// turn off AWB
	unsigned char tmp = camera.ReadReg(0x13);	// COM8
	camera.WriteReg(0x13, tmp & 0xFD);			// COM8[1] = 0
	camera.WriteReg(0x02, 0x48);				// R gain
	camera.WriteReg(0x6A, 0x40);				// G gain
	camera.WriteReg(0x01, 0x60);				// B gain

    wait_ms(300);

	// discard some first frames
	for (int i = 0; i < 10; i++) {
	    camera.CaptureNext();   // sample start!
	    while(camera.CaptureDone() == false)
	        ;
	}

    //////////////////////////////////////////////////
	int	blur_kernel_size		= 7;

	float blur_matrix[] = {
		// Average filter
		// Change here in order to use a different filter
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f,
		0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f, 0.02f
	};

	int	prev_mem	= memfree();

	stream_filter *average_flt;
	average_flt	= create_stream_filter(0, SIZEX, blur_kernel_size, comb_filter, (void *)blur_matrix);	// 0: 1 plane

//	tmr.start();	// timer
//	cam_cap(0, average_flt);
//	tmr.stop();		// timer

	int	cur_mem	= memfree();

//	lcd.locate(0, 0);
//  lcd.printf("%dms", tmr.read_ms());

//	lcd.locate(0, 1);
//	lcd.printf("m %d, %d", prev_mem, cur_mem);

//char shot_flag = 0;

    //////////////////////////////////////////////////
    while (1) {
//		int	rgain, ggain, bgain; 
		int	skin_pixel_num;

		led1 = 1;
		reset_stream_filter(average_flt);

		tmr.reset();
		tmr.start();
		skin_pixel_num	= cam_cap(0, average_flt);
		tmr.stop();

		if (skin_pixel_num >= SHUTTER_THRESHOLD)	{	// threshold
			cam_cap(1, average_flt);	// shot

			lcd.locate(0, 0);
			lcd.printf("CAPTURED ");
			lcd.locate(0, 1);
			lcd.printf("val %d ", skin_pixel_num);

			while (1) {
				wait(1);
			}
		}
//		cur_mem	= memfree();

//		rgain	= camera.ReadReg(0x02);
//		ggain	= camera.ReadReg(0x6A);
//		bgain	= camera.ReadReg(0x01);

		lcd.locate(0, 0);
		lcd.printf("%dms ", tmr.read_ms());
		lcd.locate(0, 1);
		lcd.printf("val %d ", skin_pixel_num);
//		lcd.printf("%dms, M%d ", tmr.read_ms(), cur_mem);
//		lcd.printf("R%02x G%02x B%02x", rgain, ggain, bgain);

//		wait_ms(100);
   }

}


void write_bmp_header(FILE *fp_bmp)
{
    /////////////////////////
    // file header
    /////////////////////////
    fprintf(fp_bmp, "BM");
    int val = 14 + 40 + SIZEX * SIZEY * 3;   // file size
    fprintf(fp_bmp, "%c%c%c%c", val % 0x100, val / 0x100, val / 0x10000, val / 0x1000000);
    fprintf(fp_bmp, "%c%c%c%c%c%c%c%c", 0, 0, 0, 0, 0x36, 0, 0, 0);

    /////////////////////////
    // information header
    /////////////////////////
    fprintf(fp_bmp, "%c%c%c%c", 0x28, 0, 0, 0);  // header size
    fprintf(fp_bmp, "%c%c%c%c", SIZEX % 0x100, SIZEX / 0x100, SIZEX / 0x10000, SIZEX / 0x1000000);
    fprintf(fp_bmp, "%c%c%c%c", SIZEY % 0x100, SIZEY / 0x100, SIZEY / 0x10000, SIZEY / 0x1000000);
    fprintf(fp_bmp, "%c%c", 1, 0);               // # of plane
    fprintf(fp_bmp, "%c%c", 24, 0);              // bit count
    fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);     // compression
    val = SIZEX * SIZEY * 3;         // data size
    fprintf(fp_bmp, "%c%c%c%c", val % 0x100, val / 0x100, val / 0x10000, val / 0x1000000);
    fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
    fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
    fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
    fprintf(fp_bmp, "%c%c%c%c", 0, 0, 0, 0);
}


void write_bmp_data(FILE *fp_bmp, unsigned char b, unsigned char g, unsigned char r)
{
	fprintf(fp_bmp, "%c%c%c", b, g, r);
}


//void cam_cap(Arguments* input, Reply* output)
int cam_cap(char file_flag, stream_filter *average_flt)
{
	FILE *fp_bmp;
    unsigned int d1, d2;
    unsigned char sort[3];

	int	dx	= 0;
	int	dy	= 0;

	unsigned int	skin_pixel_num	= 0;
	unsigned int	skin_x			= 0;
	unsigned int	skin_y			= 0;


	////////////////////////////////////////////////////////////////////
	led2 = 0;
	led3 = 0;
	led4 = 0;

	if (file_flag != 0) {
	    fp_bmp	= fopen("/webfs/cam.bmp", "wb");
		write_bmp_header(fp_bmp);
	}

    camera.CaptureNext();   // sample start!

    while(camera.CaptureDone() == false)
        ;

    camera.ReadStart();     // reset pointer

	led2 = 1;

	/////////////////////////////////////////////
	// main loop
	/////////////////////////////////////////////
    for (int y = 0; y < SIZEY; y++) {    
        for (int x = 0; x < SIZEX; x++) {
            d1 = camera.ReadOneByte() ; // upper nibble is XXX , lower nibble is B
            d2 = camera.ReadOneByte() ; // upper nibble is G   , lower nibble is R

			/////////////////////////////////////////////
			// RGB565
			/////////////////////////////////////////////
			sort[0]	= ((d1 & 0xF8) >> 3) << 3;		// R
			sort[1] = ( ((d1 & 0x07) << 3) + ((d2 & 0xE0) >> 5) ) << 2;		// G
			sort[2]	= (d2 & 0x1F) << 3;				// B

			//////////////////////////////////////////////////////////////////////////
			// 1. get input
			//////////////////////////////////////////////////////////////////////////
			unsigned char	b0	= sort[2];
			unsigned char	g0	= sort[1];
			unsigned char	r0	= sort[0];

			if (file_flag != 0) {
				write_bmp_data(fp_bmp, sort[2], sort[1], sort[0]);		// B,G,R
			}
			else {
				//////////////////////////////////////////////////////////////////////////
				// 2. RGB2HSV
				//////////////////////////////////////////////////////////////////////////
				unsigned char	h1, s1, v1;
				rgb2hsv(r0, g0, b0, &h1, &s1, &v1);

				//////////////////////////////////////////////////////////////////////////
				// 3. judge
				//////////////////////////////////////////////////////////////////////////
				unsigned char	val2	= judge_skin_color(h1, s1, v1);

				//////////////////////////////////////////////////////////////////////////
				// 4. blur (average)
				//////////////////////////////////////////////////////////////////////////
				average_flt->d_in0	= val2;		// (1 plane mode; use d_in0 only)

				if (y == 0 && x == 0)
					average_flt->d_in_attr	= ATTR_1ST;
				else if (y == SIZEY - 1 && x == SIZEX - 1)
					average_flt->d_in_attr	= ATTR_LAST;
				else
					average_flt->d_in_attr	= ATTR_VALID;

				// apply filter
				apply_filter(average_flt);

				//////////////////////////////////////////////////////////////////////////
				// 5. limit
				//////////////////////////////////////////////////////////////////////////
				unsigned char	val3	= average_flt->d_out0;

				if (val3 < 220)
					val3	= 0;
				else {
					skin_pixel_num++;
					skin_x	+= dx;
					skin_y	+= dy;
				}

				//////////////////////////////////////////////////////////////////////////
				// 6. put output
				//////////////////////////////////////////////////////////////////////////
				if (average_flt->d_out_attr != ATTR_INVALID) {
					sort[2]	= val3 & b0;
					sort[1]	= val3 & g0;
					sort[0]	= val3 & r0;

					dx++;
					if (dx == SIZEX) {
						dx	= 0;
						dy++;
					}
				}
			}	// file_flag

        }
    }

	led3 = 1;

	if (file_flag != 0) {
		/////////////////////////////////////////////
		// flush filter
		/////////////////////////////////////////////
		average_flt->d_in_attr	= ATTR_INVALID;

		while (1) {
			apply_filter(average_flt);

			if (average_flt->d_out_attr == ATTR_INVALID)
				break;

			//////////////////////////////////////////////////////////////////////////
			// 5. limit
			//////////////////////////////////////////////////////////////////////////
			unsigned char	val3	= average_flt->d_out0;

			if (val3 < 220)
				val3	= 0;
			else {
				skin_pixel_num++;
				skin_x	+= dx;
				skin_y	+= dy;
			}

			//////////////////////////////////////////////////////////////////////////
			// 6. put output
			//////////////////////////////////////////////////////////////////////////
			sort[2]	= val3;
			sort[1]	= val3;
			sort[0]	= val3;

			dx++;
			if (dx == SIZEX) {
				dx	= 0;
				dy++;
			}
		}
	}

	led4 = 1;

	/////////////////////////////////////////////
	// end
	/////////////////////////////////////////////
    camera.ReadStop();

//	release_stream_filter(average_flt);

	if (skin_pixel_num != 0) {
		skin_x	/= skin_pixel_num;
		skin_y	/= skin_pixel_num;
	}
	else {
		skin_x	= SIZEX / 2;
		skin_y	= SIZEY / 2;
	}

	if (file_flag != 0) {
		fclose(fp_bmp);
	}

//	led2 = 0;
//	led3 = 0;
//	led4 = 0;

	return (skin_pixel_num);
}

// end of file