/***************************************************************************
 *   Copyright (C) 2006 by Miguel Armando Riem de Oliveira   *
 *   mike@BlackBandit   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <camera1394_control.h>


camera1394_control::camera1394_control()
{
	//set flags
	flg.verbose=TRUE;
	flg.hashandle=FALSE;
	flg.testimgparams=TRUE;
	//set defaults
	handlenum=0;
	nodenum=0;
	//strcpy(devicename, "/dev/video1394/1");
	

	captureparams.width=320;
	captureparams.height=240;
	captureparams.numchannels=3;
	captureparams.colormode=MODE_320x240_YUV422;
	captureparams.framerate=FRAMERATE_30;
	//alocate size for tmp buffer
	TmpBuffer = new unsigned char[4*640*480]; //allocate space for worst case scenario
}

camera1394_control::camera1394_control(int HN, int NN, int CF, int FR)
{
		//set flags
	flg.verbose=TRUE;
	flg.hashandle=FALSE;
	flg.testimgparams=TRUE;
	//set defaults
	handlenum=0;
	nodenum=0;
	//strcpy(devicename, "/dev/video1394/1");
	

	captureparams.width=320;
	captureparams.height=240;
	captureparams.numchannels=3;
	captureparams.colormode=MODE_320x240_YUV422;
	captureparams.framerate=FRAMERATE_30;
	//alocate size for tmp buffer
	TmpBuffer = new unsigned char[4*640*480]; //allocate space for worst case scenario
	

	SetHandleNum(HN);
	SetNodeNum(NN);
	SetColorFormat(CF);
	OpenHandle();
	GetCameraNodes();
	SetFrameRate(FR);
	SetupCapture_dma();
	StartIsoTransmission();

}

camera1394_control::~camera1394_control()
{
	dc1394_stop_iso_transmission(handle, numnodes[nodenum]);
	dc1394_dma_done_with_buffer(&cameracapture);
	dc1394_dma_unlisten(handle, &cameracapture);
	
	dc1394_dma_release_camera(handle, &cameracapture);
	dc1394_destroy_handle(handle);
	dc1394_free_camera_nodes(numnodes);	
// 	dc1394_release_camera(handle, &numCameras)

}

int camera1394_control::SetPredefinedParams(int paramsset)
{


	switch (paramsset)
	{
	case 1:
	{
		SetBrightness(SET1_BRIGHTNESS);
		SetExposure(SET1_EXPOSURE);
		SetSharpness(SET1_SHARPNESS);
		SetSaturation(SET1_SATURATION);
		SetGamma(SET1_GAMMA);
		SetShutter(SET1_SHUTTER);
		SetGain(SET1_GAIN);
	break;}
	case 2:
	{
		SetBrightness(SET2_BRIGHTNESS);
		SetExposure(SET2_EXPOSURE);
		SetSharpness(SET2_SHARPNESS);
		SetSaturation(SET2_SATURATION);
		SetGamma(SET2_GAMMA);
		SetShutter(SET2_SHUTTER);
		SetGain(SET2_GAIN);
	break;}
	}



}

int camera1394_control::SetColorFormat(int cm)
{
	switch (cm)
	{
		case MODE_160x120_YUV444:
		{	
			captureparams.width=160;
			captureparams.height=120;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_160x120_YUV444;
			printf("ColorFormatMode MODE_160x120_YUV444 set...OK (WARNING: Option CV_WINDOW_AUTOSIZE returns segmentation fault with this format)\n");
			break;
		}
		case MODE_320x240_YUV422:
		{
			captureparams.width=320;
			captureparams.height=240;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_320x240_YUV422;
			printf("ColorFormatMode MODE_320x240_YUV422 set...OK\n");
			break;
		}
		case MODE_640x480_YUV411:
		{
			captureparams.width=640;
			captureparams.height=480;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_640x480_YUV411;
			printf("ColorFormatMode MODE_640x480_YUV411 set...OK\n");
			break;
		}
		case MODE_640x480_YUV422:
		{
			//for some reason does not work
			captureparams.width=640;
			captureparams.height=480;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_640x480_YUV422;
			printf("ColorFormatMode MODE_640x480_YUV422 set...OK (WARNING: TESTS FAILED WITH THIS OPTION)\n");
			break;
		}
		case MODE_640x480_RGB:
		{
			captureparams.width=640;
			captureparams.height=480;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_640x480_RGB;
			printf("ColorFormatMode MODE_640x480_RGB set...OK (WARNING: TESTS FAILED WITH THIS OPTION)\n");
			break;
		}
		case MODE_640x480_MONO:
		{
			captureparams.width=640;
			captureparams.height=480;
			captureparams.numchannels=1;
			captureparams.colormode=MODE_640x480_MONO;
			printf("ColorFormatMode MODE_640x480_MONO set...OK (WARNING: TESTS FAILED WITH THIS OPTION)\n");
			break;
		}
		case MODE_640x480_MONO16:
		{
			captureparams.width=640;
			captureparams.height=480;
			captureparams.numchannels=1;
			captureparams.colormode=MODE_640x480_MONO16;
			printf("ColorFormatMode MODE_640x480_MONO16 set...OK (WARNING: TESTS FAILED WITH THIS OPTION)\n");
			break;
		}
		default:
		{
			printf("Capture mode unknown.\n");
			PrintPossibleModes();
			printf("Using default format MODE_320x240_YUV422.\n");
			captureparams.width=320;
			captureparams.height=240;
			captureparams.numchannels=3;
			captureparams.colormode=MODE_320x240_YUV422;
			printf("Check dc1394_control.h for aditional info...ERROR\n");
			break;
		}
	}
	return 1;
}




int camera1394_control::OpenHandle(void)
{
	if (flg.verbose) cout << "Opening handle " << handlenum << " ..." <<endl;

	handle = dc1394_create_handle(handlenum);

	if (handle==NULL)
	{
		if (flg.verbose) printf("Bronca a abrir o handle!\n");
		flg.hashandle=FALSE;
		return -1;
	}
	else 
	{
		if (flg.verbose) printf("Handle %d opened...OK\n",handlenum);
		flg.hashandle=TRUE;
		return 1;
	}
}

int camera1394_control::TurnOffCamera(void)
{
	//Shutdown cameras
	int ret = dc1394_camera_off(handle, nodenum);
	
	if (flg.verbose)
	{
		cout << "Shuting down camera on node... " << nodenum << endl;
		if (ret)
			cout << "Camera is off... OK:" << endl;
		else
			cout << "Cannot shutdown camera... ERROR:" << endl;
	}	
	
	return ret;
}

int camera1394_control::TurnOnCamera(void)
{
	//TurnOn cameras
	int ret = dc1394_camera_on(handle, nodenum);
	
	if (flg.verbose)
	{
		cout << "Turning on camera on node... " << nodenum << endl;
		if (ret)
		{
			cout << "Camera is on... OK:" << endl;
			return 1;
		}
		else
		{
			cout << "Cannot turn on camera... ERROR:" << endl;
			return -1;
		}
	}	
	else return ret;
}


int camera1394_control::GetCameraNodes(void)
{
	//get cameras nodes
	if (flg.verbose) cout << "Getting node information..." << endl;
	
	numnodes =  dc1394_get_camera_nodes(handle, &numCameras,1);
	
	if (flg.verbose)
	{
		cout << "Number of cameras = " << numCameras<< endl;
		cout << "Number of nodes = " << numnodes << endl;
	}
	
	return 1;
}

int camera1394_control::SetupCapture_dma()
{
	/*-----------------------------------------------------------------------
     *  to prevent the iso-transfer bug from raw1394 system, check if
     *  camera is highest node. For details see 
     *  http://linux1394.sourceforge.net/faq.html#DCbusmgmt
     *  and
     *  http://sourceforge.net/tracker/index.php?func=detail&aid=435107&group_id=8157&atid=108157
     *-----------------------------------------------------------------------*/
    if( numnodes[nodenum] == *numnodes-1)
    {
        fprintf( stderr, "\n"
                 "Sorry, your camera is the highest numbered node\n"
                 "of the bus, and has therefore become the root node.\n"
                 "The root node is responsible for maintaining \n"
                 "the timing of isochronous transactions on the IEEE \n"
                 "1394 bus.  However, if the root node is not cycle master \n"
                 "capable (it doesn't have to be), then isochronous \n"
                 "transactions will not work.  The host controller card is \n"
                 "cycle master capable, however, most cameras are not.\n"
                 "\n"
                 "The quick solution is to add the parameter \n"
                 "attempt_root=1 when loading the OHCI driver as a \n"
                 "module.  So please do (as root):\n"
                 "\n"
                 "   rmmod ohci1394\n"
                 "   insmod ohci1394 attempt_root=1\n"
                 "\n"
                 "for more information see the FAQ at \n"
                 "http://linux1394.sourceforge.net/faq.html#DCbusmgmt\n"
                 "\n");
        exit( 1);
    }


	//setting up capture (dma)
	cout << "Starting camera setup..." << endl;
	
	//build device string 
	char str[255];
	sprintf(str,"/dev/video1394/%d",handlenum);
	
	int ret = dc1394_dma_setup_capture(/*raw1394handle_t */handle,
					/*nodeid_t*/  numnodes[nodenum],
                         		/*int channel*/ nodenum,
					/*FORMAT_VGA_NONCOMPRESSED= 384, FORMAT_SVGA_NONCOMPRESSED_1, FORMAT_SVGA_NONCOMPRESSED_2, (two others)*/ FORMAT_VGA_NONCOMPRESSED,
			 		/*int mode needs more options*/captureparams.colormode,
                         		/*int speed*/ SPEED_400, 
					/*int frame_rate: FRAMERATE_1_875= 32,
							FRAMERATE_3_75,
							FRAMERATE_7_5,
							FRAMERATE_15,
					 NO_BAYER_DECODING,		FRAMERATE_30,
							FRAMERATE_60,
							FRAMERATE_120,
							FRAMERATE_240*/ captureparams.framerate, 
                         		/*int num_dma_buffers*/ 4,
                         		/*int drop_frames*/ 1,
                         		str /* device file */,
                         		&cameracapture);

	if (flg.verbose)	
	{
		if (ret!=DC1394_SUCCESS)
			cout << "Camera setup error" << endl;
		else 
			cout << "Camera setup sucessfull ... OK" << endl;
	}

	return ret;
}

int camera1394_control::PrintPossibleModes(void)
{ 
	cout << "Possible format modes are:" << endl;
	cout << "MODE_160x120_YUV444= 64, "<< endl;
    	cout << "MODE_320x240_YUV422, "<< endl;
    	cout << "MODE_640x480_YUV411, "<< endl;
    	cout << "MODE_640x480_YUV422, "<< endl;
    	cout << "MODE_640x480_RGB, "<< endl;
    	cout << "MODE_640x480_MONO, "<< endl;
    	cout << "MODE_640x480_MONO16 "<< endl;
	return 1;
}

int camera1394_control::PrintPossibleFrameRates(void)
{
	cout << "Possible frame rates in libdc1394 are:" << endl;
	cout << "FRAMERATE_1_875= 32 (not suported in camera1394_control), "<< endl;
    	cout << "FRAMERATE_3_75, "<< endl;
    	cout << "FRAMERATE_7_5, "<< endl;
    	cout << "FRAMERATE_15, "<< endl;
    	cout << "FRAMERATE_30, "<< endl;
    	cout << "FRAMERATE_60, "<< endl;
    	cout << "FRAMERATE_120 (not suported in camera1394_control), "<< endl;
	cout << "FRAMERATE_240 (not suported in camera1394_control), "<< endl;
	return 1;
}

int camera1394_control::StartIsoTransmission(void)
{
	if (dc1394_start_iso_transmission(handle, cameracapture.node) != DC1394_SUCCESS) 
	{
		if (flg.verbose) cout << "pIso transmission start error ... ERROR" << endl;
		return -1;
	}
	else
	{
		if (flg.verbose) cout << "Iso transmission started ... OK" << endl;
		return 1;
	}
}


int camera1394_control::GetFrame_dma(IplImage *img)
{
	if (dc1394_dma_single_capture(&cameracapture)!=DC1394_SUCCESS)
        {
        	if (flg.verbose) cout << "Cannot capture frame... ERROR" << endl;
        }
	else
	{
		//if (flg.verbose) cout << "Frame captured... OK" << endl;
	}
		
	convert_to_rgb( (unsigned char *) (cameracapture.capture_buffer),
                    TmpBuffer,
                    captureparams.colormode,
                    cameracapture.frame_width, cameracapture.frame_height, COLOR_FORMAT7_YUV422,
                    NO_BAYER_DECODING,
                    8 /* bits */ );

	
	if (flg.testimgparams)
	{
		unsigned char testval=TRUE;
		if (img==NULL) return -1;	
		if (img->height != captureparams.height) testval = FALSE;
		if (img->width != captureparams.width) testval = FALSE;
		if (img->nChannels != captureparams.numchannels) testval = FALSE;

		if (testval==FALSE)
		{
			printf("Given IplImage does not comply with capture setup definitions. Cannot GetImage...ERROR\n");
			dc1394_dma_done_with_buffer(&cameracapture);
			return -1;
		}		
	}
	
 	cvSetData( img, TmpBuffer, cameracapture.frame_width*3 );
	cvConvertImage(img ,img ,CV_CVTIMG_SWAP_RB);
	dc1394_dma_done_with_buffer(&cameracapture);
	return 1;
}

int camera1394_control::SetFeatureToManualAuto(unsigned int f, unsigned int val) //manual=0; auto=1;
{
	return dc1394_auto_on_off(handle, numnodes[nodenum], f, val);
}
