/*
 * module to read video data from SVBcams.
 * maybe you have to add something like: /etc/udev.d/90-svbony.rules to get read/wirte access for users in 'plugdev'
 * 		SUBSYSTEMS=="usb", ATTRS{idVendor}=="f266", ATTRS{idProduct}=="9a0a", GROUP="plugdev", MODE="0660"
 */

#include "config.h"

#ifdef USE_SVBONY

#include <stdlib.h>
#include <SVBCameraSDK.h>
#include "convert.h"
#include "videodev-svbcam.h"

VideoDev_SVBCam::VideoDev_SVBCam() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	inframe = NULL;
	inframe_size = 0;
	inframe_pixfmt = 0;
	inframe_w = 0;
	inframe_h = 0;
	camid = -1;
};


VideoDev_SVBCam::~VideoDev_SVBCam() {
	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if (running > 0) 	CaptureStop();
}


void VideoDev_SVBCam::print_error(int err) {
	switch (err) {
		case SVB_ERROR_INVALID_ID:
			printf ("SVB_ERROR_INVALID_ID\n");
			break;
		case SVB_ERROR_CAMERA_REMOVED:
			printf ("SVB_ERROR_CAMERA_REMOVED\n");
			break;
		case SVB_ERROR_CAMERA_CLOSED:
			printf ("SVB_ERROR_CAMERA_CLOSED\n");
			break;
		case SVB_ERROR_INVALID_SIZE:
			printf ("SVB_ERROR_INVALID_SIZE\n");
			break;
		case SVB_ERROR_INVALID_IMGTYPE:
			printf ("SVB_ERROR_INVALID_IMGTYPE\n");
			break;
		case SVB_ERROR_INVALID_MODE:
			printf ("SVB_ERROR_INVALID_MODE\n");
			break;
		case SVB_ERROR_EXPOSURE_IN_PROGRESS:
			printf("SVB_ERROR_EXPOSURE_IN_PROGRESS\n");
			break;
		case SVB_ERROR_GENERAL_ERROR:
			printf("SVB_ERROR_GENERAL_ERROR\n");
			break;
		case SVB_ERROR_TIMEOUT:
			printf("SVB_ERROR_TIMEOUT\n");
			break;
		default:
			printf ("unknown %d\n", err);
			break;
	}
};



/*
 * check for connected devices and return the result in a lite.
 */
int VideoDev_SVBCam::GetDeviceList(std::list<std::string> *list) {
	int iNumofConnectCameras = SVBGetNumOfConnectedCameras();
	std::string device;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if (list == NULL) return VDEV_STATUS_ERROR;

	// from sample code
	SVB_CAMERA_INFO *caminfo = (SVB_CAMERA_INFO *)malloc(sizeof(SVB_CAMERA_INFO)*iNumofConnectCameras);
	for (int i = 0; i < iNumofConnectCameras; i++) {
		SVBGetCameraInfo(&caminfo[i], i);
		device = "SVBCAM " + to_string(caminfo[i].CameraID) + " [" + caminfo[i].FriendlyName + "]";
		list->push_back(device);
	}

	return VDEV_STATUS_OK;
}



/*
 * Open Device
 * prepare the buffer
 */
int VideoDev_SVBCam::Open() {
	int camnumcontrols, err;
	SVB_CONTROL_CAPS *camcontrols = NULL;
	SVB_CAMERA_PROPERTY camprop;
	VideoDevCtrl vctl;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);

	camid = atoi (conf_device.c_str());
	printf ("%s:%d %s opening cam %d\n", __FILE__, __LINE__, __FUNCTION__, camid);

	if ((err = SVBOpenCamera(camid)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}

	//
	// get cam property
	if ((err = SVBGetCameraProperty(camid, &camprop)) != SVB_SUCCESS) {
		print_error(err);
		Close();
		return VDEV_STATUS_ERROR;
	}
	printf ("%s:%d %s MaxHeight: %ld  MaxWidth: %ld\n", __FILE__, __LINE__, __FUNCTION__, camprop.MaxHeight, camprop.MaxWidth);

	for(int i=0; i < 8 && camprop.SupportedVideoFormat[i] != SVB_IMG_END; i++) {
		printf ("%s:%d %s VideoFormat Index:%d ", __FILE__, __LINE__, __FUNCTION__, i);
		switch (camprop.SupportedVideoFormat[i]) {
			case SVB_IMG_RAW8:
				printf("\t\tSVB_IMG_RAW8\n");
				break;
			case SVB_IMG_RAW10:
				printf("\t\tSVB_IMG_RAW10\n");
				break;
			case SVB_IMG_RAW12:
				printf("\t\tSVB_IMG_RAW12\n");
				break;
			case SVB_IMG_RAW14:
				printf("\t\tSVB_IMG_RAW14\n");
				break;
			case SVB_IMG_RAW16:
				printf("\t\tSVB_IMG_RAW16\n");
				break;
			case SVB_IMG_Y8:
				printf("\t\tSVB_IMG_Y8\n");
				break;
			case SVB_IMG_Y10:
				printf("\t\tSVB_IMG_Y10\n");
				break;
			case SVB_IMG_Y12:
				printf("\t\tSVB_IMG_Y12\n");
				break;
			case SVB_IMG_Y14:
				printf("\t\tSVB_IMG_Y14\n");
				break;
			case SVB_IMG_Y16:
				printf("\t\tSVB_IMG_Y16\n");
				break;
			case SVB_IMG_RGB24:
				printf("\t\tSVB_IMG_RGB24\n");
				break;
			case SVB_IMG_RGB32:
				printf("\t\tSVB_IMG_RGB32\n");
				break;
			case SVB_IMG_END:
				printf("\t\tSVB_IMG_END\n");
				break;
			default:
				printf ("\t\tunknown\n");
				break;
		}
	}


	//
	// get controls
	vidctrls.clear();
	if ((err = SVBGetNumOfControls(camid, &camnumcontrols)) != SVB_SUCCESS) print_error(err);
	camcontrols = (SVB_CONTROL_CAPS*) malloc (sizeof(SVB_CONTROL_CAPS) * camnumcontrols);
	for (int i = 0; i < camnumcontrols; i++) {
		if ((err = SVBGetControlCaps(camid, i, &camcontrols[i])) != SVB_SUCCESS) {
			print_error(err);
			return VDEV_STATUS_ERROR;
		}
		printf ("%s:%d %s Control-%02d name:%s desc:%s [%ld:%ld-%ld] auto:%ld write:%ld type:%d\n", __FILE__, __LINE__, __FUNCTION__,
			i, camcontrols[i].Name, camcontrols[i].Description,
			(long)camcontrols[i].DefaultValue, (long)camcontrols[i].MinValue, (long)camcontrols[i].MaxValue,
			(long)camcontrols[i].IsAutoSupported, (long)camcontrols[i].IsWritable, (unsigned int)camcontrols[i].ControlType);
		vctl.name = (char*)camcontrols[i].Name;
		vctl.id = camcontrols[i].ControlType;
		vctl.min = camcontrols[i].MinValue;
		vctl.max = camcontrols[i].MaxValue;
		GetDevCtrl(i, &vctl.value);
		vidctrls.push_back(vctl);
	}

	//
	// set ROI (region of interest)
	if ((err = SVBSetROIFormat(camid, 0, 0, camprop.MaxWidth, camprop.MaxHeight, 1)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}

	// Set Mode and IMG Format
	if ((err = SVBSetCameraMode(camid, SVB_MODE_NORMAL)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}

	inframe_pixfmt = convert_to_pixelformat(conf_format);
	switch (inframe_pixfmt) {
		case (V4L2_PIX_FMT_SGRBG16):
			if ((err = SVBSetOutputImageType(camid, SVB_IMG_RAW16)) != SVB_SUCCESS) {
				print_error(err);
				return VDEV_STATUS_ERROR;
			}
			break;
		case (V4L2_PIX_FMT_SGRBG8):
			if ((err = SVBSetOutputImageType(camid, SVB_IMG_RAW8)) != SVB_SUCCESS) {
				print_error(err);
				return VDEV_STATUS_ERROR;
			}
			break;
		case (V4L2_PIX_FMT_BGR32):
			if ((err = SVBSetOutputImageType(camid, SVB_IMG_RGB32)) != SVB_SUCCESS) {
				print_error(err);
				return VDEV_STATUS_ERROR;
			}
			break;
		case (V4L2_PIX_FMT_BGR24):
			if ((err = SVBSetOutputImageType(camid, SVB_IMG_RGB24)) != SVB_SUCCESS) {
				print_error(err);
				return VDEV_STATUS_ERROR;
			}
			break;
		default:
			conf_format = convert_from_pixelformat(V4L2_PIX_FMT_BGR24);
			inframe_pixfmt = V4L2_PIX_FMT_BGR24;
			if ((err = SVBSetOutputImageType(camid, SVB_IMG_RGB24)) != SVB_SUCCESS) {
				print_error(err);
				return VDEV_STATUS_ERROR;
			}
			break;
	}


	//
	// preprare buffer
	conf_width  = inframe_w = camprop.MaxWidth;
	conf_height = inframe_h = camprop.MaxHeight;

	return VDEV_STATUS_OK;
};


/*
 * Close Device
 * Free videobuffer
 */
int VideoDev_SVBCam::Close() {
	int err;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if ((err = SVBCloseCamera(camid)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}
	return VDEV_STATUS_OK;
};



/*****************************************************************************************************
 * VideoGrabbing
 */


/*
 * prepare inframe for raw picture data, will hold a video frame with 16bit per channel or BGR32/BGR24
 * inframe size = 4*W*H
 * send the start capture signal to the cam
 */
int VideoDev_SVBCam::CaptureStart() {
	int err;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	// start video capture
	if ((err = SVBStartVideoCapture(camid)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}

	//
	// allocate memory for frame data
	if (inframe != NULL) free (inframe);
	inframe_size = get_bytesperpixel(inframe_pixfmt) * inframe_w * inframe_h;
	inframe = (unsigned char*) malloc(inframe_size);
	if (inframe == NULL) {
		errorexit ((char*)"%s:%d could not allocate memory for framebuffer. Error:%s\n", __FILE__, __LINE__, strerror(errno));
	}
	pixelformat = inframe_pixfmt;

	return VDEV_STATUS_OK;
};


/*
 * free inbuffer
 */
int VideoDev_SVBCam::CaptureStop() {
	int err;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	if ((err = SVBStopVideoCapture(camid)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}

	//
	// free inframe memory
	if (inframe) {
		free (inframe);
		inframe_size = 0;
	}

	return VDEV_STATUS_OK;
};


/*
 * try to grab one frame and convert it into RGB32.
 * If something goes wrong return an error code.
 * Return code VDEV_STATUS_AGAIN is not an error. There was no video image ready to read.
 */
// FIXME: SVBGetVideoData needs to be outside of Lock/UnLockMutex - using inside thread inbuffer
int VideoDev_SVBCam::Grab(VideoFrameRaw *vfr) {
	int err;

	if (inframe == NULL) return VDEV_STATUS_ERROR;
	if ((err = SVBGetVideoData(camid, inframe, (long)inframe_size, 50)) != SVB_SUCCESS) {
		if (err != SVB_ERROR_TIMEOUT) {
			print_error(err);
			// UnLockMutex();   <-- Warum?
			return VDEV_STATUS_ERROR;
		}
		else {
			return VDEV_STATUS_AGAIN;
		}
	}
	LockMutex();

	vfr->CopyFrom(inframe_pixfmt, inframe_w, inframe_h, inframe_size, inframe);
	UnLockMutex();

	return VDEV_STATUS_OK;
}


int VideoDev_SVBCam::GetDeviceFormats(string device, std::list<string> *formats) {
	int camid_;
	std::string result = "";
	int err;
	SVB_CAMERA_PROPERTY camprop;
	VideoDevCtrl vctl;

	printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
	LockMutex();

	if (camid == -1) {
		camid_ = atoi (device.c_str());
		printf ("%s:%d %s opening cam %d\n", __FILE__, __LINE__, __FUNCTION__, camid_);

		if ((err = SVBOpenCamera(camid_)) != SVB_SUCCESS) {
			print_error(err);
			return VDEV_STATUS_ERROR;
		}
	}
	else camid_ = camid;

	//
	// get cam property
	if ((err = SVBGetCameraProperty(camid_, &camprop)) != SVB_SUCCESS) {
		print_error(err);
		Close();
		return VDEV_STATUS_ERROR;
	}
	printf ("%s:%d %s MaxHeight:%ld  MaxWidth:%ld  MaxDepth:%d\n", __FILE__, __LINE__, __FUNCTION__, camprop.MaxHeight,
			camprop.MaxWidth, camprop.MaxBitDepth);
	char pattern[5];
	switch(camprop.BayerPattern) {
	case SVB_BAYER_RG:
	  strcpy(pattern, "RGGB");
	  break;
	case SVB_BAYER_BG:
	  strcpy(pattern, "BGGR");
	  break;
	case SVB_BAYER_GR:
	  strcpy(pattern, "GRBG");
	  break;
	case SVB_BAYER_GB:
	  strcpy(pattern, "GBRG");
	  break;
	default:
	  strcpy(pattern, "NONE");
	  break;
	}
	printf ("%s:%d %s BayerPattern:%s  Color:%d  Binning: \n", __FILE__, __LINE__, __FUNCTION__, pattern, camprop.IsColorCam);
	for(int i=0; i < 16 && camprop.SupportedBins[i] != 0; i++) {
		printf ("%s:%d %s Binning Index:%d ", __FILE__, __LINE__, __FUNCTION__, i);
		printf ("\t\t%dx%d\n", camprop.SupportedBins[i], camprop.SupportedBins[i]);
	}

	for(int i=0; i < 8 && camprop.SupportedVideoFormat[i] != SVB_IMG_END; i++) {
		printf ("%s:%d %s VideoFormat Index:%d ", __FILE__, __LINE__, __FUNCTION__, i);
		switch (camprop.SupportedVideoFormat[i]) {
			case SVB_IMG_RAW8:
				printf("\t\tSVB_IMG_RAW8\n");
				switch(camprop.BayerPattern) {
				case SVB_BAYER_RG:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SRGGB8));
				  break;
				case SVB_BAYER_BG:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SBGGR8));
				  break;
				case SVB_BAYER_GR:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SGRBG8));
				  break;
				case SVB_BAYER_GB:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SGBRG8));
				  break;
				default:
				  break;
				}
				break;
			case SVB_IMG_RAW10:
				printf("\t\tSVB_IMG_RAW10\n");
				break;
			case SVB_IMG_RAW12:
				printf("\t\tSVB_IMG_RAW12\n");
				break;
			case SVB_IMG_RAW14:
				printf("\t\tSVB_IMG_RAW14\n");
				break;
			case SVB_IMG_RAW16:
				printf("\t\tSVB_IMG_RAW16\n");
				switch(camprop.BayerPattern) {
				case SVB_BAYER_RG:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SRGGB16));
				  break;
				case SVB_BAYER_BG:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SBGGR16));
				  break;
				case SVB_BAYER_GR:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SGRBG16));
				  break;
				case SVB_BAYER_GB:
				  formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_SGBRG16));
				  break;
				default:
				  break;
				}
				break;
			case SVB_IMG_Y8:
				printf("\t\tSVB_IMG_Y8\n");
				break;
			case SVB_IMG_Y10:
				printf("\t\tSVB_IMG_Y10\n");
				break;
			case SVB_IMG_Y12:
				printf("\t\tSVB_IMG_Y12\n");
				break;
			case SVB_IMG_Y14:
				printf("\t\tSVB_IMG_Y14\n");
				break;
			case SVB_IMG_Y16:
				printf("\t\tSVB_IMG_Y16\n");
				break;
			case SVB_IMG_RGB24:
				printf("\t\tSVB_IMG_RGB24\n");
				formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_BGR24));
				break;
			case SVB_IMG_RGB32:
				printf("\t\tSVB_IMG_RGB32\n");
				formats->push_back(convert_from_pixelformat(V4L2_PIX_FMT_BGR32));
				break;
			case SVB_IMG_END:
				printf("\t\tSVB_IMG_END\n");
				break;
			default:
				printf ("\t\tunknown\n");
				break;
		}
	}

	if (camid == -1) {
		if ((err = SVBCloseCamera(camid_)) != SVB_SUCCESS) {
			print_error(err);
			return VDEV_STATUS_ERROR;
		}
	}

	UnLockMutex();
	return VDEV_STATUS_OK;
}



/*****************************************************************************************************
 * Controls
 */

/*
 * set video control identified by id
 */
int VideoDev_SVBCam::SetDevCtrl(unsigned int id, int value) {
	int err;
	int oldv;

	if (value < 0) {			// enable auto
		GetDevCtrl (id, &oldv);
		if ((err = SVBSetControlValue(camid, static_cast<SVB_CONTROL_TYPE>(id), oldv, SVB_TRUE)) != SVB_SUCCESS) {
			print_error(err);
			return VDEV_STATUS_ERROR;
		}
	}
	else {
		if ((err = SVBSetControlValue(camid, static_cast<SVB_CONTROL_TYPE>(id), value, SVB_FALSE)) != SVB_SUCCESS) {
			print_error(err);
			return VDEV_STATUS_ERROR;
		}
	}

	return VDEV_STATUS_OK;
};


/*
 * get video control identified by id
 */
int VideoDev_SVBCam::GetDevCtrl(unsigned int id, int *value) {
	int err;
	long pvalue;
	SVB_BOOL ctlauto;

	if ((err = SVBGetControlValue(camid, static_cast<SVB_CONTROL_TYPE>(id), &pvalue, &ctlauto)) != SVB_SUCCESS) {
		print_error(err);
		return VDEV_STATUS_ERROR;
	}
	*value = (int) pvalue;
	return VDEV_STATUS_OK;
};


#endif
