#include "camera.h"

//Construct a camera, get init camera parameter
Camera::Camera(char *deviceIn, int widthIn, int heightIn, int formatIn, int grabmethodIn)
{
	memset (&videoIn, 0, sizeof (struct vdIn));
	memset(videodevice, 0, sizeof(device));

	if(NULL == deviceIn)
	{
		memcpy(videodevice, "/dev/v4l/video0", strlen("/dev/v4l/video0"));
	}
	if(strlen(deviceIn) > sizeof(videodevice)
	{
		printf("The video device name is too long, using default device:/dev/v4l/video0 \n");
		memcpy(videodevice, "/dev/v4l/video0", strlen("/dev/v4l/video0"));
	}
	else
	{
		memcpy(videodevice, videoIn, strlen(videoIn));
	}

	if(widthIn == 320 || widthIn || 640)
	{
		width = widthIn;
	}
	else
	{
		printf("With parameter error, support width is 320 and 640, Using default wotdh: 320\n");
		width = 320;
	}

	if(heightIn == 240 || heightIn == 480)
	{
		height =heightIn;
	}
	else
	{
		printf("Helght parameter error, support height is 240 and 480, Using default height: 240\n");
		height = 240;
	}

	if(formatIn != VIDEO_PALETTE_JPEG)
	{
		printf("Format error, suppot format is: VIDEO_PALETTE_JPEG=21, using default format: VIDEO_PALETTE_JPEG\n");
		format = VIDEO_PALETTE_JPEG;
	}

	if(grabmethodIn== methodRead || grabmethodIn== methodMmap)
	{
		grabmethod = grabmethodIn;
	}
	else
	{
		printf("Grab image method error, support method is methodRead =0 and methodMmap = 1, using default method: methodRead\n");
		grabmethod = methodRead;
	}
}
//overload construct function for convenience 
/*Camera::Camera()
{	
	memcpy(videodevice, "/dev/v4l/video0", strlen("/dev/v4l/video0"));
	format = VIDEO_PALETTE_JPEG;
	grabmethod = methodRead;
	height = 240;
	width = 320;		
}*/?
//destroy a camera, just close it
Camera::~Camera()
{
	close_v4l (&videoIn);
}

//Init the camera, and create a thread to grab image
void Camera::openCamera()
{
	init_videoIn(&videoIn, videodevice, width, height, format,grabmethod);
	pthread_create (&grabImgThrdId, NULL, (void *) grab, NULL);

	 signal(SIGPIPE, SIG_IGN);	/* Ignore sigpipe */

	sa.sa_handler = sigchld_handler;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = SA_RESTART;
}

void Camera::sigchld_handler()
{
	videoIn.signalquit = 0;
}

//close the cameara
void Camera::closeCamera()
{
	close_v4l (&videoIn);
}

int Camera::readFrame(char *buf)
{
	int ret;
	int frameout = 1;
	unsigned char wakeup = 0;
	struct frame_t *headerframe;
	 
	while ((frameout == videoIn.frame_cour) && videoIn.signalquit) 
	{
		printf("Image is not ready, waiting 1 ms\n");
	   	usleep(1000);		
	}	
   
	videoIn.framelock[frameout]++;
	headerframe = (struct frame_t *) videoIn.ptframe[frameout];	
	memcpy(buf, (unsigned char*)(videoIn.ptframe[frameout]+sizeof(struct frame_t)), headerframe->size);
	videoIn.framelock[frameout]--;
	frameout = (frameout+1)%4;     
	return headerframe->size;     
}

void Camera::upbright()
{
	unsigned short bright =0;
	
	bright = videoIn.videopict.brightness;
	
	if(( bright+0x200) < 0xffff)
	{
		bright += 0x200;
		videoIn.videopict.brightness = bright;
		if (SetVideoPict (&videoIn) < 0)
    		{
      			printf("Upbright bright failed\n");
		}
	}
}

void Camera::downbright()
{
	unsigned short bright =0;
	
	bright = videoIn.videopict.brightness;
	
	if(( bright-0x200) > 0)
	{
		bright -= 0x200;
		videoIn.videopict.brightness = bright;
		if (SetVideoPict (&videoIn) < 0)
    		{
      			printf("Down bright failed\n");
		}
	}
}

void Camera::upcontrast()
{
	unsigned short contrast =0;
	
	contrast = videoIn.videopict.contrast;
	
	if((contrast+0x200) < 0xffff)
	{
		contrast += 0x200;
		videoIn.videopict.contrast = contrast;
		
		if (SetVideoPict (&videoIn) < 0)
    		{
      			printf("Up contrast  failed\n");
    		}	
	}
}

void Camera::downcontrast()
{
	unsigned short contrast =0;
	
	contrast = videoIn.videopict.contrast;
	
	if((contrast-0x200) > 0)
	{
		contrast -= 0x200;
		videoIn.videopict.contrast = contrast;
		
		if (SetVideoPict (&videoIn) < 0)
    		{
      			printf("Down contrast  failed\n");
    		}	
	}
}


int Camera::init_videoIn (struct vdIn *vd, char *device, int width, int height, int format, int grabmethod)
{
	int err = -1;
	int i;
	
	if (vd == NULL || device == NULL)
		return -1;
	
	if (width == 0 || height == 0)
		return -1;
	
	if(grabmethod < 0 || grabmethod > 1)		
		grabmethod = 1; //read by default;
		
	// check format 
	vd->videodevice = NULL;
	vd->cameraname = NULL;
	vd->videodevice = NULL;
	vd->videodevice = (char *) realloc (vd->videodevice, 16);
	vd->cameraname = (char *) realloc (vd->cameraname, 32);
	snprintf (vd->videodevice, 12, "%s", device);
	
	if(debug) 
		printf("video %s \n",vd->videodevice);
	
	memset (vd->cameraname, 0, sizeof (vd->cameraname));
	memset(vd->bridge, 0, sizeof(vd->bridge));
	vd->signalquit = 1;
	vd->hdrwidth = width;
	vd->hdrheight = height;
  	/*compute the max frame size   */
	vd->formatIn = format; 
	vd->bppIn = GetDepth (vd->formatIn);
	vd->grabMethod = grabmethod;		//mmap or read 
	vd->pFramebuffer = NULL;
	/* init and check all setting */
	err = init_v4l (vd);
	/* allocate the 4 frames output buffer */
 
	for (i = 0; i < OUTFRMNUMB; i++)
	{
		vd->ptframe[i] = NULL;
		vd->ptframe[i] = (unsigned char *) realloc (vd->ptframe[i], sizeof(struct frame_t) + (size_t) vd->framesizeIn );
		vd->framelock[i] = 0;
	}
 
	vd->frame_cour = 0;  
	pthread_mutex_init (&vd->grabmutex, NULL);
	return err;
}

int Camera::GetDepth (int format)
{
	int depth;
	switch (format)
	{   
		case VIDEO_PALETTE_JPEG:
		{
			depth = 8;
			 break;
		}  
		
		case VIDEO_PALETTE_RAW:
		{
			depth = 8;
			 break;
		}
     
		case VIDEO_PALETTE_YUV420P:
		{
			depth = (8 * 3) >> 1;
			 break;
		}

		case VIDEO_PALETTE_RGB565:
      		{
			depth = 16;
			break;
		}
		
		case VIDEO_PALETTE_RGB24:
		{
			depth = 24;
			break;
		}
		
		case VIDEO_PALETTE_RGB32:
		{
			depth = 32;
			break;
		}
      
		default:
		{
			depth = -1;
			break;
		}
	}
	
	return depth;
}

int Camera::init_v4l (struct vdIn *vd)
{
	int f;
	int erreur = 0;
	
	if ((vd->fd = open (vd->videodevice, O_RDWR)) == -1)
		exit_fatal ("ERROR opening V4L interface");

	if (ioctl (vd->fd, VIDIOCGCAP, &(vd->videocap)) == -1)
		exit_fatal ("Couldn't get videodevice capability");

	if(debug) 
		printf ("Camera found: %s \n", vd->videocap.name);
		snprintf (vd->cameraname, 32, "%s", vd->videocap.name);

	erreur = GetVideoPict (vd);
	if (ioctl (vd->fd, VIDIOCGCHAN, &vd->videochan) == -1)
	{
		if(debug) printf ("Hmm did not support Video_channel\n");
			vd->cameratype = UNOW;
	}
	else
	{
		if (vd->videochan.name)
		{
			if(debug) printf ("Bridge found: %s \n", vd->videochan.name);
				snprintf (vd->bridge, 9, "%s", vd->videochan.name);
			vd->cameratype = GetStreamId (vd->videochan.name);
			spcaPrintParam (vd->fd,&vd->videoparam);
		}
	}
	else
	{
		if(debug) 
			printf ("Bridge not found not a spca5xx Webcam Probing the hardware !!\n");
		vd->cameratype = UNOW;
	}

	/* Only jpeg webcam allowed */
	if(vd->cameratype != JPEG)
	{
		exit_fatal ("Not a JPEG webcam sorry Abort !");
	}
	
	if(debug)
		printf ("StreamId: %d  Camera\n", vd->cameratype);
	
	vd->videopict.palette = vd->formatIn;
	vd->videopict.depth = GetDepth (vd->formatIn);
	vd->bppIn = GetDepth (vd->formatIn);
  
    
	vd->framesizeIn = (vd->hdrwidth * vd->hdrheight >> 2 );
	erreur = SetVideoPict (vd);
	erreur = GetVideoPict (vd);
	
	if (vd->formatIn != vd->videopict.palette || vd->bppIn != vd->videopict.depth)
		exit_fatal ("could't set video palette Abort !");
	
	if (erreur < 0)
		exit_fatal ("could't set video palette Abort !");

	if (vd->grabMethod)
	{
		if(debug)
			printf (" grabbing method default MMAP asked \n");
		
		// MMAP VIDEO acquisition
		memset (&(vd->videombuf), 0, sizeof (vd->videombuf));
		
		if (ioctl (vd->fd, VIDIOCGMBUF, &(vd->videombuf)) < 0)
		{
			perror (" init VIDIOCGMBUF FAILED\n");
		}
		
		if(debug) 
			printf ("VIDIOCGMBUF size %d  frames %d  offets[0]=%d offsets[1]=%d\n",
			
		vd->videombuf.size, vd->videombuf.frames,
		vd->videombuf.offsets[0], vd->videombuf.offsets[1]);
		vd->pFramebuffer = (unsigned char *) mmap (0, vd->videombuf.size, PROT_READ | PROT_WRITE, MAP_SHARED, vd->fd, 0);
		vd->mmapsize = vd->videombuf.size;
		vd->vmmap.height = vd->hdrheight;
		vd->vmmap.width = vd->hdrwidth;
		vd->vmmap.format = vd->formatIn;
		
		for (f = 0; f < vd->videombuf.frames; f++)
		{
			vd->vmmap.frame = f;
			
			if (ioctl (vd->fd, VIDIOCMCAPTURE, &(vd->vmmap)))
			{
				perror ("cmcapture");
			}
		}
		
		vd->vmmap.frame = 0;
	}