/*
 * thermal_camera.cpp
 *
 *  Created on: 11/06/2013
 *      Author: mmcgill
 */

#include <ros/ros.h>
#include <crosbot/config.hpp>
#include <crosbot/data.hpp>
#include <crosbot/utils.hpp>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_transport/image_transport.h>


#ifndef THERMAL_USE_FIREWIRE

#include <opencv/highgui.h>
#define TYPE_V4L		"v4l"
#define TYPE_V4L2		"v4l2"
#define TYPE_FIREWIRE	"firewire"
#define TYPE_UNICAP		"unicap"

int main(int argc, char** argv) {
	ros::init(argc, argv, "thermal_camera");
	ros::NodeHandle nh;

	ros::Publisher imagePub = nh.advertise< sensor_msgs::Image >("image", 1);

	ros::NodeHandle config("~");

	LOG("ThermalCamera: Configuring camera.\n");
	CvCapture* cvCam = NULL;
	std::string type;
	int id;
//	config.param<std::string>("device", device, "/dev/video0");
	config.param<std::string>("type", type, "v4l");
	config.param<int>("id", id, 0);


	int cvCameraTypeIndex = CV_CAP_ANY;
	if (strcasecmp(type.c_str(), TYPE_V4L) == 0)
		cvCameraTypeIndex = CV_CAP_V4L;
	else if (strcasecmp(type.c_str(), TYPE_V4L2) == 0)
		cvCameraTypeIndex = CV_CAP_V4L2;
#ifdef CV_CAP_FIREWIRE
	else if (strcasecmp(type.c_str(), TYPE_FIREWIRE) == 0)
		cvCameraTypeIndex = CV_CAP_FIREWIRE;
#endif
#ifdef CV_CAP_UNICAP
	else if (strcasecmp(type.c_str(), TYPE_UNICAP) == 0)
		cvCameraTypeIndex = CV_CAP_UNICAP;
#endif
	else if (type != "") {
		ERROR("ThermalCamera: Unknown OpenCV camera type %s.\n", type.c_str());
		return -1;
	}

	LOG("ThermalCamera: Opening camera type: %s, id: %d.\n", type.c_str(), id);
	cvCam = cvCaptureFromCAM(cvCameraTypeIndex+id);
	if (cvCam == NULL) {
		ERROR("ThermalCamera: Unable to start OpenCV camera %s.\n", type.c_str());
		return -2;
	}

//	LOG("ThermalCamera: Camera opened.\n");
	while (ros::ok()) {
//		ros::spin();
		// Get image from camera

//		LOG("ThermalCamera: Capturing from camera.\n");
		if (!cvGrabFrame(cvCam)){
			ERROR("ThermalCamera: Failed to grab frame in OpenCV.\n");
		    break;
		}

		IplImage *cvframe = cvRetrieveFrame(cvCam);

		if (cvframe == NULL){
			ERROR("TheramlCamera: Unable to retrieve OpenCV frame.\n");
		    break;
		}

		// Copy image to message for publishing
//		LOG("ThermalCamera: Image recieved.\n");

		sensor_msgs::Image image;
		image.header.stamp = ros::Time::now();
		image.width = cvframe->width; image.height = cvframe->height;
		image.step = cvframe->widthStep;

		if (cvframe->nChannels == 1 && (cvframe->depth == (int)IPL_DEPTH_8U || cvframe->depth == (int)IPL_DEPTH_8S)) {
			// MONO8 image
			image.encoding = sensor_msgs::image_encodings::MONO8;
		} else if (cvframe->nChannels == 1 && (cvframe->depth == (int)IPL_DEPTH_16U || cvframe->depth == (int)IPL_DEPTH_16S)) {
			// MONO16 image
			image.encoding = sensor_msgs::image_encodings::MONO16;
		} else if (cvframe->nChannels == 3 && (cvframe->depth == (int)IPL_DEPTH_8U || cvframe->depth == (int)IPL_DEPTH_8S)) {
			// BGR image
			image.encoding = sensor_msgs::image_encodings::BGR8;
		} else {
			ERROR("ThermalCamera: Unsupported image format - channels: %d, depth: %d\n", cvframe->nChannels, cvframe->depth);
			break;
		}

		image.data.resize(cvframe->imageSize);
		memcpy(&image.data[0], cvframe->imageData, cvframe->imageSize);

		imagePub.publish(image);
//		usleep(1000000);
	}

	cvReleaseCapture(&cvCam);

	return 0;
}

#else

#include <dc1394/dc1394.h>

#define NUM_DMA_BUFFERS		8
#define DEFAULT_FRAMERATE	DC1394_FRAMERATE_15

unsigned int minTemp = 0, maxTemp = __INT32_MAX__;
unsigned int minTempSeen = __INT32_MAX__, maxTempSeen = 0;

dc1394camera_t* initialise_camera(dc1394_t* dc1394, const std::string& id) {
	dc1394camera_t* camera = NULL;

	uint64_t guid = 0;
	uint32_t unit;

	if (id == "") {
		// Use first found camera.
		dc1394camera_list_t* cameras = NULL;
		dc1394_camera_enumerate(dc1394, &cameras);

		if (cameras == NULL || cameras->num == 0) {
			ERROR("ThermalCamera: Could not find camera on firewire bus.\n");
			return NULL;
		}

		guid = cameras->ids[0].guid;
		unit = cameras->ids[0].unit;
		dc1394_camera_free_list(cameras);
	} else {
		unsigned int scanned;

#if __WORDSIZE == 64
		scanned = sscanf(id.c_str(), "0x%lx %u", &guid, &unit);
#else
		scanned = sscanf(id.c_str(), "0x%Lx %u", &guid, &unit);
#endif

		if (scanned == 1) {
			unit = 0;
		} else if (scanned != 2)
		{
			ERROR("ThermalCamera: Could not parse GUID for camera %s\n", id.c_str());
			return NULL;
		}
	}

#if __WORDSIZE == 64
LOG("ThermalCamera: Using camera 0x%lx %d\n", guid, unit);
#else
LOG("ThermalCamera: Using camera 0x%Lx %d\n", guid, unit);
#endif

	camera = dc1394_camera_new_unit(dc1394, guid, unit);
	if (camera == NULL)
	{
		ERROR("ThermalCamera: Could not get access to dc1394 camera %s\n", id.c_str());
		return NULL;
	}

	dc1394featureset_t features;
	if (dc1394_feature_get_all(camera, &features) == DC1394_SUCCESS)
	{
		int i;
		for (i = 0; i < DC1394_FEATURE_NUM; i++)
		{
			if (!(features.feature[i].available) ||
				features.feature[i].id < DC1394_FEATURE_MIN ||
				features.feature[i].id > DC1394_FEATURE_MAX)
				continue;

			int j;
			for (j = 0; j < features.feature[i].modes.num &&
						j < DC1394_FEATURE_NUM; j++)
			{
				if (features.feature[i].modes.modes[j] == DC1394_FEATURE_MODE_AUTO)
				{
					if (dc1394_feature_set_mode(camera, features.feature[i].id, DC1394_FEATURE_MODE_AUTO) != DC1394_SUCCESS)
					{
						ERROR("ThermalCamera: Could not set feature %s of dc1394 camera %s to auto.\n",
							dc1394_feature_get_string(features.feature[i].id), id.c_str());
					}
					break;
				}
			}
		}
	}
	else
		ERROR("ThermalCamera: Could not get features of dc1394 camera %s.\n", id.c_str());

	// find mode with largest res/info
	dc1394video_mode_t mode;
	dc1394video_modes_t modes;
	if (dc1394_video_get_supported_modes(camera, &modes) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Unable to query available video modes for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}
	else if (modes.num < 1)
	{
		ERROR("ThermalCamera: No video modes are available for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}

	mode = modes.modes[0];
	uint32_t width, height, cWidth, cHeight;
	if (dc1394_get_image_size_from_video_mode(camera, mode, &width, &height) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Unable to query image size for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}

	dc1394color_coding_t colourCoding, cColourCoding;
	if (dc1394_get_color_coding_from_video_mode(camera, mode, &colourCoding) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Unable to query colour coding for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}
	int i;
	for (i = 1; i < modes.num; i++)
	{
		if (dc1394_get_image_size_from_video_mode(camera, modes.modes[i], &cWidth, &cHeight) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Unable to query image size for dc1394 camera %s.\n", id.c_str());
			return NULL;
		}
		if (dc1394_get_color_coding_from_video_mode(camera, modes.modes[i], &cColourCoding) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Unable to query colour coding for dc1394 camera %s.\n", id.c_str());
			return NULL;
		}

		if (modes.modes[i] != DC1394_VIDEO_MODE_FORMAT7_0 &&
				((width * height < cWidth * cHeight) ||
					(width * height == cWidth * cHeight &&
						(colourCoding < cColourCoding ||
							(colourCoding == cColourCoding && modes.modes[i] > mode)))))
		{
			mode = modes.modes[i];
			width = cWidth; height = cHeight;
			colourCoding = cColourCoding;
		}
	}
	//printf("CASCAM: Using mode %d on camera %s.\n", mode,  id.c_str());
	if (dc1394_video_set_mode(camera, mode) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Unable to set video mode for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}

	if (dc1394_video_set_framerate(camera, DEFAULT_FRAMERATE) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Unable to set framerate for dc1394 camera %s.\n", id.c_str());
		//return CASCAM_DEVICE_ERROR;
	}

	if (dc1394_capture_setup(camera, NUM_DMA_BUFFERS, DC1394_CAPTURE_FLAGS_DEFAULT) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Could not start video capture for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}

	// Start camera transmitting on the firewire bus.
	if (dc1394_video_set_transmission(camera, DC1394_ON) != DC1394_SUCCESS)
	{
		ERROR("ThermalCamera: Failed to start iso transmission for dc1394 camera %s.\n", id.c_str());
		return NULL;
	}

	return camera;
}

void shutdown_camera(dc1394camera_t* camera, const std::string& id) {
	if (camera != NULL)
	{
		if (dc1394_video_set_transmission(camera, DC1394_OFF) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Failed stopping transmission for dc1394 camera %s\n", id.c_str());
		}

		if (dc1394_capture_stop(camera) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Unable to stop capture for dc1394 camera %s.\n", id.c_str());
		}

		if (dc1394_iso_release_all(camera) != DC1394_SUCCESS) {
			ERROR("ThermalCamera: Error releasing dc1394 camera bandwidth and channel for dc1394 camera %s.\n", id.c_str());
		}

		if (dc1394_camera_reset(camera) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Error resetting dc1394 camera %s.\n", id.c_str());
		}

		dc1394_camera_free(camera);

//		if (minTempSeen != __INT32_MAX__ || maxTempSeen != 0)
			LOG("ThermalCamera: Temperature range encountered: min %d - max %d\n", minTempSeen, maxTempSeen);
	}
}

crosbot::ImagePtr generateImage(dc1394video_frame_t* frame1394) {
	crosbot::ImagePtr rval = NULL;
	uint32_t width = frame1394->size[0],
			height = frame1394->size[1];

	if (frame1394->color_coding == DC1394_COLOR_CODING_MONO16 && (minTemp != 0 || maxTemp != __INT32_MAX__)) {
		rval = new crosbot::Image(crosbot::Image::Mono8, height, width);

		unsigned int min = minTemp, max = maxTemp;
		if (min > max) {
			int tmp = min;
			min = max;
			max = tmp;
		} else if (min == max) {
			++max;
		}

		// create normalised image
		float scale = ((float)255.0) / (max - min);
		uint16_t *inL;
		uint8_t* outL;
		uint32_t val;

		for (uint32_t y = 0; y < height; ++y) {
			inL = (uint16_t *)(frame1394->image + y*frame1394->stride);
			outL = (uint8_t *)(((char*)rval->data) + y*rval->step);

			for (uint32_t x = 0; x < width; ++x) {
				val = ntohs(inL[x]);

				if (val < minTempSeen)
					minTempSeen = val;
				if(val > maxTempSeen)
					maxTempSeen = val;

				if (val <= min) {
					outL[x] = 0;
				} else if (val >= max) {
					outL[x] = 255;
				} else {
					outL[x] = scale * (val - min);
				}
			}
		}

		return rval;
	} else {
		crosbot::Image::Encoding imageFormat = crosbot::Image::Unknown;
		switch (frame1394->color_coding) {
			case DC1394_COLOR_CODING_MONO8:
				imageFormat = crosbot::Image::Mono8;
				break;
			case DC1394_COLOR_CODING_MONO16:
				imageFormat = crosbot::Image::Mono16;
				break;
			case DC1394_COLOR_CODING_YUV422:
				imageFormat = crosbot::Image::YUV422;
				break;
			case DC1394_COLOR_CODING_RGB8:
				imageFormat = crosbot::Image::RGB8;
				break;
			case DC1394_COLOR_CODING_YUV411:
			case DC1394_COLOR_CODING_YUV444:
			case DC1394_COLOR_CODING_RGB16:
			case DC1394_COLOR_CODING_MONO16S:
			case DC1394_COLOR_CODING_RGB16S:
			case DC1394_COLOR_CODING_RAW8:
			case DC1394_COLOR_CODING_RAW16:
				fprintf(stderr, "CASCAM: Unable to get image for dc1394 camera as colour coding is incompatible.\n");
				return NULL;
		}

		rval = new crosbot::Image(imageFormat, height, width);

		if (imageFormat == crosbot::Image::Mono16) {
			// image is in network byte order
			uint16_t *inL, *outL;
			for (uint32_t y = 0; y < height; ++y) {
				inL = (uint16_t *)(frame1394->image + y*frame1394->stride);
				outL = (uint16_t *)(((char*)rval->data) + y*rval->step);

				for (uint32_t x = 0; x < width; ++x)
					outL[x] = ntohs(inL[x]);
			}
		} else if (rval->step != frame1394->stride) {
			fprintf(stderr, "CASCAM: Image step sizes are incompatible.\n");
			return NULL;
		} else {
			// copy image data
			for (uint32_t i = 0; i < height; ++i) {
				memcpy(((char *)rval->data) + rval->step*i, frame1394->image + frame1394->stride*i, rval->step);
			}
		}
	}
	return rval;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "thermal_camera");
	ros::NodeHandle nh("~");
	image_transport::ImageTransport it(nh);
	image_transport::Publisher imagePub = it.advertise("image", 1);

	crosbot::ConfigElementPtr cfg = new crosbot::ROSConfigElement(nh);

	std::string id = cfg->getParam("id", "");
	minTemp = cfg->getParamAsInt("min", minTemp);
	maxTemp = cfg->getParamAsInt("max", maxTemp);

//	LOG("ThermalCamera: Opening camera type: %s, id: %d.\n", type.c_str(), id);
	dc1394_t* dc1394 = dc1394_new();
	if (dc1394 == NULL) {
		ERROR("ThermalCamera: Unable to access firewire bus.\n");
		return -1;
	}

	dc1394camera_t* camera = initialise_camera(dc1394, id);
	if (camera == NULL) {
		ERROR("ThermalCamera: Unable to initialise camera.\n");
		return -2;
	}

	LOG("ThermalCamera: Camera opened.\n");
	while (ros::ok()) {
//		ros::spin();
		// Get image from camera
		dc1394video_frame_t *frame1394 = NULL;
		if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame1394) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Unable to capture frame from dc1394 camera %s.\n", id.c_str());
			continue;
		}


		// Copy image to message for publishing
		crosbot::ImagePtr image = generateImage(frame1394);
		if (image == NULL) {
			ERROR("ThermalCamera: Unable to process frame from dc1394 camera %s.\n", id.c_str());
			continue;
		}

		if (frame1394 != NULL && dc1394_capture_enqueue(camera , frame1394) != DC1394_SUCCESS)
		{
			ERROR("ThermalCamera: Unable to return frame to dc1394 camera %s.\n", id.c_str());
		}
//		LOG("ThermalCamera: Image recieved.\n");

		imagePub.publish(image->toROS());
//		usleep(1000000);
	}

	// close camera
	shutdown_camera(camera, id);
	dc1394_free(dc1394);

	return 0;
}

#endif
