/**
* uvoscam
* Copyright (C) 2021 Carl Klemm
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* version 3 as published by the Free Software Foundation.
*
* 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., 51 Franklin Street, Fifth Floor,
* Boston, MA  02110-1301, USA.
*/

#include "uvoscam.h"

#include <iostream>
#include <thread>
#include <atomic>
#include <opencv2/imgproc.hpp>
#include <string.h>

#include"log.h"

#include <arv.h>

using namespace cam;

cv::Mat Camera::debayer(ArvBuffer *buffer)
{
	size_t bufferSize;
	const void* data =  arv_buffer_get_data(buffer, &bufferSize);
	const cv::Mat cvBuffer(static_cast<int>(height_), static_cast<int>(width_), CV_8U, const_cast<void*>(data), bufferSize/width_);
	cv::Mat cvResult;
	if(bayerMode == BAYER_DEBAYER)
	{
		cv::cvtColor(cvBuffer, cvResult,  cv::COLOR_BayerBG2BGR);
	}
	else if(bayerMode == BAYER_RED || bayerMode == BAYER_BLUE)
	{
		cvBuffer.copyTo(cvResult);
		for(int y = 0; y < cvResult.rows; y+=2)
		{
			uint8_t* colH = cvResult.ptr<uint8_t>(y);
			uint8_t* colL = cvResult.ptr<uint8_t>(y+1);
			for(int x = 0; x < cvResult.cols; x+=2)
			{
				if(bayerMode == BAYER_BLUE)
				{
					colH[x+1] = colH[x];
					colL[x]   = colH[x];
					colL[x+1] = colH[x];
				} else
				{
					colH[x]   = colL[x+1];
					colH[x+1] = colL[x+1];
					colL[x]   = colL[x+1];
				}
			}
		}
	}
	else if(bayerMode == BAYER_GREEN)
	{
		cvBuffer.copyTo(cvResult);
		for(int y = 0; y < cvResult.rows; ++y)
		{
			uint8_t* col = cvResult.ptr<uint8_t>(y);
			for(int x = 0; x < cvResult.cols; x+=2)
 				col[x+(y & 1)] = col[x+!(y & 1)];
		}
	}
	return cvResult;
}

cv::Mat Camera::pack8(ArvBuffer *buffer, bool color)
{
	size_t bufferSize;
	const void* data =  arv_buffer_get_data(buffer, &bufferSize);
	Log(Log::DEBUG)<<"bufferSize "<<bufferSize;
	const cv::Mat cvBuffer(static_cast<int>(height_), static_cast<int>(width_), color ? CV_8UC3 : CV_8U, 
						   const_cast<void*>(data));
	cv::Mat cvResult;
	cvBuffer.copyTo(cvResult);
	return cvResult;
}

cv::Mat Camera::pack16(ArvBuffer *buffer)
{
	/*size_t bufferSize;
	void* data =  arv_buffer_get_data(buffer, &bufferSize);
	cv::Mat cvBuffer(static_cast<int>(height_), static_cast<int>(width_), color ? CV_8SC3 : CV_8SC1, data, bufferSize/width_);*/
	cv::Mat cvResult;
	//cvBuffer.copyTo(cvResult);
	return cvResult;
}

void Camera::decoderThreadFunc()
{
	while(true)
	{
		std::unique_lock<std::mutex> bufferConditionMutexLock(bufferConditionMutex);
		bufferCondition.wait(bufferConditionMutexLock);
		if(join)
			return;
		decodeBufferMutex.lock();
		if(decodeBuffer != nullptr)
		{
			Log(Log::DEBUG)<<"decoderThreadFunc";
			if(format == ARV_PIXEL_FORMAT_BAYER_BG_8)
			{
				if(bayerMode != BAYER_PASSTHOUGH)
					callback_(debayer(decodeBuffer));
				else
					callback_(pack8(decodeBuffer, false));
			}
			else if(format == ARV_PIXEL_FORMAT_MONO_8)
			{
				callback_(pack8(decodeBuffer, false));
			}
			arv_stream_push_buffer(aStream_, decodeBuffer);
		}
		decodeBufferMutex.unlock();
	}
}

static void aFCallback(void* instance, ArvStreamCallbackType type, ArvBuffer *buffer)
{
	Camera::aCallback(instance, static_cast<int>(type), buffer);
}

void Camera::aCallback(void* instance, int type, ArvBuffer *buffer)
{
	Camera* instance_ = reinterpret_cast<Camera*>(instance);
	if(type == ARV_STREAM_CALLBACK_TYPE_BUFFER_DONE)
	{
		if(instance_->run)
		{
			gint inBuffers;
			gint outBuffers;
			arv_stream_get_n_buffers(instance_->aStream_, &inBuffers, &outBuffers);
			Log(Log::DEBUG)<<"ARV_STREAM_CALLBACK_TYPE_BUFFER_DONE in "<<inBuffers<<" out "<<outBuffers;
			std::unique_lock<std::mutex> decodeBufferMutexLock(instance_->decodeBufferMutex);
			instance_->decodeBuffer = arv_stream_pop_buffer(instance_->aStream_);
			instance_->bufferCondition.notify_all();
			if(instance_->aqmode == MODE_SINGLE)
				instance_->run = false;
		}
		else
		{
			arv_stream_push_buffer(instance_->aStream_, arv_stream_pop_buffer(instance_->aStream_));
		}
	}
}

void Camera::getSize(unsigned int* width, unsigned int* height)
{
	*width = width_;
	*height = height_;
}

bool Camera::chooseFormat()
{
	guint count = 0;
	GError* error = nullptr;
	gint64* formats = arv_camera_dup_available_pixel_formats(aCamera_, &count, &error);
	if(error)
		return false;
	for(guint i = 0; i < count && format < 0; ++i)
	{
		switch(formats[i])
		{
			case ARV_PIXEL_FORMAT_MONO_8:
			case ARV_PIXEL_FORMAT_MONO_10:
			case ARV_PIXEL_FORMAT_MONO_12:
			case ARV_PIXEL_FORMAT_RGB_8_PLANAR:
			case ARV_PIXEL_FORMAT_BAYER_BG_8:
				format = formats[i];
			default:
				break;
		}
	}
	
	arv_camera_set_pixel_format(aCamera_, format, &error);
	if(error)
		return false;
	
	return format >= 0;
}

bool Camera::setupCamera()
{
	if(!chooseFormat())
	{
		Log(Log::ERROR)<<"Camera provides no supported pixel format";
		return false;
	}

	GError* error = nullptr;
	gint ymin, ymax;
	arv_camera_get_height_bounds(aCamera_, &ymin, &ymax, &error);
	if(error)
	{
		Log(Log::ERROR)<<"Can not get hight bounds";
		return false;
	}
	
	Log(Log::DEBUG)<<"Height bounds min: "<<ymin<<" max: "<<ymax;
	
	gint xmin, xmax;
	arv_camera_get_width_bounds(aCamera_, &xmin, &xmax, &error);
	if(error)
	{
		Log(Log::ERROR)<<"Can not get width bounds";
		return false;
	}
	
	Log(Log::DEBUG)<<"Width bounds min: "<<xmin<<" max: "<<xmax;

	arv_camera_set_region(aCamera_, 0, 0, xmax, ymax, nullptr);

	gint x, y, w, h;
	arv_camera_get_region(aCamera_, &x, &y, &w, &h, &error);
	if(error)
	{
		Log(Log::ERROR)<<"Can not get camera roi";
		return false;
	}
	
	width_ = w;
	height_= h;
	
	Log(Log::DEBUG)<<"Wdith "<<width_<<" Heigt "<<height_;

	bool binning = arv_camera_is_binning_available(aCamera_, &error);
	if(error)
	{
		Log(Log::ERROR)<<"Can not get camera binning";
		return false;
	}
	if(binning)
	{
		arv_camera_set_binning(aCamera_, 1, 1, &error);
		if(error)
		{
			Log(Log::ERROR)<<"failed to set binning";
			return false;
		}
	}
	Log(Log::DEBUG)<<"Binning: "<<(binning ? "yes" : "no");
	
	decoderThread = new std::thread([this](){decoderThreadFunc();});
	
	Log(Log::DEBUG)<<"Using "<<arv_camera_get_pixel_format_as_string(aCamera_, nullptr)<<" as pixel format";
	
	aStream_ = arv_camera_create_stream(aCamera_, aFCallback, this, nullptr);
	if(!ARV_IS_STREAM(aStream_))
	{
		Log(Log::ERROR)<<"Can not create stream";
		return false;
	}
	
	size_t frameSize = arv_camera_get_payload(aCamera_, nullptr);
	for(size_t i = 0; i < BUFFER_DEPTH; ++i)
	{
		imageBuffers[i] = arv_buffer_new(frameSize, nullptr);
		arv_stream_push_buffer(aStream_, imageBuffers[i]);
	}
	setExposureAuto(false);
	setTriggerMode(TRIGGER_FREE);
	setAcquisitionMode(ARV_ACQUISITION_MODE_CONTINUOUS);
	setFrameRate(10);
	setExposureTime(100000);
	arv_camera_stop_acquisition(aCamera_, nullptr);
	
	return true;
}

std::vector<Camera::Description> Camera::getAvailableCameras(bool update)
{
	if(!scanned_ || update)
	{
		arv_update_device_list();
		scanned_ = true;
	}
	std::vector<Description> cameras;
	for(size_t i = 0; i < arv_get_n_devices(); ++i)
	{
		Description camera;
		camera.vendor = arv_get_device_vendor(i);
		camera.serial = arv_get_device_serial_nbr(i);
		camera.id     = arv_get_device_id(i);
		camera.model  = arv_get_device_model(i);
		cameras.push_back(camera);
	}
	return cameras;
}

Camera::Camera(std::function<void(cv::Mat)> callback): join(false), run(false), callback_(callback)
{
	arv_enable_interface("Fake");
}
	
bool Camera::openCamera(const std::string& name)
{
	aCamera_ = arv_camera_new(name.c_str(), nullptr);
	if(ARV_IS_CAMERA(aCamera_))
		setupCamera();
	else
		Log(Log::ERROR)<<"Can not create camera from name "<<name;
	return true;
}

bool Camera::openCamera(const Camera::Description& camera)
{
	aCamera_ = arv_camera_new(camera.id.c_str(), nullptr);
	if(ARV_IS_CAMERA(aCamera_))
		setupCamera();
	else
		Log(Log::ERROR)<<"Can not create camera";
	return true;
}

bool Camera::openCamera(const size_t index)
{
	Description camera = getAvailableCameras().at(index);
	aCamera_ = arv_camera_new(camera.id.c_str(), nullptr);
	if(ARV_IS_CAMERA(aCamera_))
		setupCamera();
	else
		Log(Log::ERROR)<<"Can not create camera from index "<<index<<" name "<<camera.model;
	return true;
}

Camera::~Camera()
{
	join = true;
	if(decoderThread)
	{
		bufferCondition.notify_all();
		decoderThread->join();
	}
	if(ARV_IS_STREAM(aStream_))
	{
		stopAcquisition();
		g_object_unref(aStream_);
	}
	if(ARV_IS_CAMERA(aCamera_))
		g_object_unref(aCamera_);
}

bool Camera::isOpen()
{
	return ARV_IS_STREAM(aStream_) && ARV_IS_CAMERA(aCamera_);
}

bool Camera::setAcquisitionMode(AquisitionMode mode)
{
	GError* error = nullptr;
	aqmode = mode;
	arv_camera_set_acquisition_mode(aCamera_, mode == MODE_FREE ? 
	                                ARV_ACQUISITION_MODE_CONTINUOUS : ARV_ACQUISITION_MODE_SINGLE_FRAME, &error);
	return !error;
}

bool Camera::setExposureTime(uint64_t us)
{
	GError* error = nullptr;
	arv_camera_set_exposure_time(aCamera_, static_cast<double>(us), &error);
	return !error;
}

uint64_t Camera::getExposureTime()
{
	GError* error = nullptr;
	double ret = arv_camera_get_exposure_time(aCamera_, &error);
	return !error ? static_cast<uint64_t>(ret) : 0;
}

double Camera::getFrameRate()
{
	GError* error = nullptr;
	double ret = arv_camera_get_frame_rate(aCamera_, &error);
	return !error ? ret : -1;
}

bool Camera::setFrameRate(double rate)
{
	GError* error = nullptr;
	arv_camera_set_frame_rate(aCamera_, rate, &error);
	return !error;
}

int64_t Camera::getPixelFormat()
{
	return static_cast<ArvPixelFormat>(format);
}

bool Camera::setExposureAuto(bool enable)
{
	if(!arv_camera_is_exposure_auto_available(aCamera_, nullptr))
		return false;
	
	GError* error = nullptr;
	arv_camera_set_exposure_time(aCamera_, enable ? ARV_AUTO_CONTINUOUS : ARV_AUTO_OFF, &error);
	return !error;
}

bool Camera::startAcquisition()
{
	GError* error = nullptr;
	arv_camera_start_acquisition(aCamera_, &error);
	if(!error)
		run = true;
	return !error;
}

bool Camera::stopAcquisition()
{
	GError* error = nullptr;
	arv_camera_stop_acquisition(aCamera_, &error);
	if(!error)
		run = false;
	return !error;
}

void Camera::setBayerMode(Camera::BayerMode mode)
{
	bayerMode = mode;
}

Camera::BayerMode Camera::getBayerMode()
{
	return bayerMode;
}

void Camera::setTriggerMode(Camera::TriggerMode mode)
{
	switch(mode)
	{
		case TRIGGER_FREE:
			arv_camera_clear_triggers(aCamera_, nullptr);
			break;
		case TRIGGER_SOFTWARE:
			arv_camera_set_trigger(aCamera_, "Software", nullptr);
			break;
		case TRIGGER_HARDWARE:
			arv_camera_set_trigger(aCamera_, "Line1", nullptr);
			break;
	}
}

void Camera::trigger()
{
	arv_camera_software_trigger(aCamera_, nullptr);
}

Camera::Description Camera::getDescription()
{
	Description desc;
	if(ARV_IS_CAMERA(aCamera_))
	{
		GError* error;
		desc.vendor = arv_camera_get_vendor_name(aCamera_, &error);
		desc.serial = arv_camera_get_device_serial_number(aCamera_, &error);
		desc.id     = arv_camera_get_device_id(aCamera_, &error);
		desc.model= arv_camera_get_model_name(aCamera_, &error);
		if(!error)
			return desc;
		else return Description();
	}
	return desc;
}