bool BaseV4L2VideoCapture::SetSharpness(int32_t value) {
  //return v4l2_s_ctrl(V4L2_CID_SHARPNESS, value);
  if(_pVideoCaptureFiltersCapability->VerifySharpness(value)){
    if(v4l2_s_ctrl(V4L2_CID_SHARPNESS, value)){
      _pVideoCaptureFilters->sharpness = GetSharpness();
      return true;
    }
    else
      return false;
  }
  else
    return false;
}
bool BaseV4L2VideoCapture::InitVideoCaptureFilters() {
  if(_pVideoCaptureFilters == NULL)
    _pVideoCaptureFilters = new VideoCaptureFilters();

  _pVideoCaptureFilters->brightness = GetBrightness();
  _pVideoCaptureFilters->contrast = GetContrast();
  _pVideoCaptureFilters->hue = GetHUE();
  _pVideoCaptureFilters->saturation = GetSaturation();
  _pVideoCaptureFilters->sharpness = GetSharpness();
  _pVideoCaptureFilters->gamma = GetGamma();
  _pVideoCaptureFilters->backlightCompensation = GetBacklightCompensation();

  // Below is for debug sake
  //FATAL("brightness : %d\n contrast : %d\n hue : %d\n saturation : %d\n sharpness : %d\n gamma : %d\n backlightCompensation :%d", _pVideoCaptureFilters->brightness , _pVideoCaptureFilters->contrast, _pVideoCaptureFilters->hue, _pVideoCaptureFilters->saturation , _pVideoCaptureFilters->sharpness, _pVideoCaptureFilters->gamma, _pVideoCaptureFilters->backlightCompensation);
  return true;
}
int main(int argc, char *argv[ ])
{
	char SETTINGS_FILE[]="settings.txt";

	RASPIVID_CONFIG * config = (RASPIVID_CONFIG*)malloc(sizeof(RASPIVID_CONFIG));
	
	int full_width=2592;
	int full_height=1944;
	config->width=full_width;
	config->height=full_height;
	config->bitrate=0;	// zero: leave as default
	config->framerate=15;
	config->monochrome=1;

    RaspiCamCvCapture * capture = (RaspiCamCvCapture *) raspiCamCvCreateCameraCapture2(0, config); 
	free(config);
	
	CvFont font;
	double hScale=0.8;
	double vScale=0.8;
	int    lineWidth=2;

	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale, vScale, 0, lineWidth, 8);

	IplImage* tmp  = 0;
    IplImage* image = 0;

	cvNamedWindow("RaspiCamTest", 1);
	cvMoveWindow("RaspiCamTest", 100,100);

	int viewport_x = 0;
	int viewport_y = -400;
	int viewport_width = 640;
	int viewport_height = 480;

	CvScalar sharpness= {100, 5.0, 1.0, .10};

	int flip=0;
	int full_view=0;

	FILE * f = fopen(SETTINGS_FILE,"r");
	if(f) {
		fscanf(f,"%d %d %d\n",&viewport_x,&viewport_y,&flip);
		fclose(f);
	}

	int exit =0;
	do {
		IplImage* big_img = raspiCamCvQueryFrame(capture);
        if(!image) {	
			image = cvCreateImage( cvSize(viewport_width, viewport_height), big_img->depth,3 );
		}
        if(!tmp) {	
			tmp = cvCreateImage( cvSize(viewport_width, viewport_height), big_img->depth,1 );
		}

		if(!full_view) {
			CvRect cropRect = cvRect( (big_img->width-viewport_width)/2+viewport_x
					,(big_img->height-viewport_height)/2+viewport_y
					,viewport_width ,viewport_height );
			cvSetImageROI(big_img,cropRect);
			CvRect destRect=cvRect(0,0,viewport_width,viewport_height);
			cvSetImageROI(image,destRect);
			cvCvtColor(big_img,image,CV_GRAY2BGR);

			destRect=cvRect(0,0,viewport_width,viewport_height);
			cvSetImageROI(image,destRect);
		} else {
			CvRect cropRect = cvRect( 0,0, full_width, full_height );
			cvSetImageROI(big_img,cropRect);
			cvRectangle(big_img
             ,cvPoint((big_img->width-viewport_width)/2+viewport_x,
		   	          (big_img->height-viewport_height)/2+viewport_y)
             ,cvPoint((big_img->width+viewport_width)/2+viewport_x,
		   	          (big_img->height+viewport_height)/2+viewport_y)
			 ,CV_RGB( 0, 255, 0 ));
  
			cvResize(big_img,tmp);
			cvCvtColor(tmp,image,CV_GRAY2BGR);
		}


		if(flip) {
			cvFlip(image,0,-1);
		}

		sharpness = GetSharpness(big_img,image);
		double threshold=2.8;

		char text[200];

		if(full_view) {
			sprintf(text , "full view" );	
			cvPutText (image, text, cvPoint(05, 360), &font, cvScalar(255, 0, 0, 0));
		}
		sprintf(text , (sharpness.val[1]>threshold ? "** OK **" : "!! keep going !!" ) );	
		cvPutText (image, text, cvPoint(05, 400), &font, cvScalar(255, 0, 0, 0));
		sprintf(text, "Sharpness: %.0f (%.2f) x=%d y=%d", sharpness.val[0], sharpness.val[1], viewport_x, viewport_y);
		cvPutText (image, text, cvPoint(05, 440), &font, cvScalar(255, 0, 0, 0));

		cvLine( image, cvPoint(0,240), cvPoint(639,240), CV_RGB( 255, 0, 0 ));
		cvLine( image, cvPoint(320,0), cvPoint(320,479),   CV_RGB( 255, 0, 0 ));
		cvCircle( image, cvPoint(320,240), 100,   CV_RGB( 255, 0, 0 ));


		cvShowImage("RaspiCamTest", image);

		
		char key = cvWaitKey(10);
	

		if(flip) {	
			switch(key)	
			{
				case 83:        //left
					viewport_x -= 10;

					if(viewport_x<-(full_width-viewport_width)/2) {
						viewport_x=-(full_width-viewport_width)/2;
					}
					break;
				case 84:        //up
					viewport_y -= 10;
					if(viewport_y< -(full_height-viewport_height)/2) {
						viewport_y=-(full_height-viewport_height)/2;
					}
					break;
				case 81:        //right
					viewport_x += 10;
					if(viewport_x>  (full_width-viewport_width)/2) {
						viewport_x= (full_width-viewport_width)/2;
					}
					break;
				case 82:        //down
					viewport_y += 10;
					if(viewport_x>  (full_height-viewport_height)/2) {
						viewport_x= (full_height-viewport_height)/2;
					}
					break;

			}

		} else {
			switch(key)	
			{
			case 81:        //left
					viewport_x -= 10;

					if(viewport_x<-(full_width-viewport_width)/2) {
						viewport_x=-(full_width-viewport_width)/2;
					}
					break;

			case 82:        //up
				viewport_y -= 10;
				if(viewport_y< -(full_height-viewport_height)/2) {
					viewport_y=-(full_height-viewport_height)/2;
				}
				break;
			case 83:        //right
				viewport_x += 10;
				if(viewport_x>  (full_width-viewport_width)/2) {
					viewport_x= (full_width-viewport_width)/2;
				}
				break;
			case 84:        //down
				viewport_y += 10;
				if(viewport_x>  (full_height-viewport_height)/2) {
					viewport_x= (full_height-viewport_height)/2;
				}
				break;

			}

		}

		switch(key)	
		{
			case 's':       //save current position
			{
				FILE *f = fopen(SETTINGS_FILE,"w");
				if(f) {
					fprintf(f,"%d %d %d\n",viewport_x,viewport_y,flip);
					fclose(f);
				}
				break;
			}


			case 'r':       //read position
            {
				FILE *f = fopen(SETTINGS_FILE,"r");
				if(f) {
					fscanf(f,"%d %d %d\n",&viewport_x,&viewport_y,&flip);
					fclose(f);
				}

				break;
			}

			case 'c':
				viewport_x=viewport_y=0;
				break;

			case 'f':
				flip = !flip;
				break;

			case 'x':
				system("sudo halt");
				break;

			case 27:		// Esc to exit
				exit = 1;
				break;
		}
		
	} while (!exit);

	cvDestroyWindow("RaspiCamTest");
	raspiCamCvReleaseCapture(&capture);
	return 0;
}