#include "Camera.h"
#include "JPEG_XL.h"
#include "timing.h"
#include <iostream>
#include <mutex>
#include <filesystem>
#include <opencv2/opencv.hpp>


class FrameObserver : public IFrameObserver
{
public:
	typedef std::function<void(int)> Callback;
	Callback call;
	int cam_id = 0;
	std::atomic<int> settle = { 3 };

	void registerCallback(Callback f) {
		call = f;
	}

	FrameObserver(CameraPtr camera) : IFrameObserver(camera) {};

	void FrameReceived(const FramePtr pFrame)
	{
		int old_val = settle.fetch_sub(1);
		std::cout << "[FrameObserver cam=" << cam_id << "] FrameReceived settle=" << old_val << " → new=" << old_val - 1 << std::endl;

		if (old_val > 0) {
			std::cout << "[FrameObserver cam=" << cam_id << "] DUMP (settling)" << std::endl;
			m_pCamera->QueueFrame(pFrame);
			return;
		}

		bool bQueueDirectly = true;
		VmbFrameStatusType eReceiveStatus;

		if (VmbErrorSuccess == pFrame->GetReceiveStatus(eReceiveStatus))
		{
			/* ignore any incompletely frame */
			if (VmbFrameStatusComplete != eReceiveStatus)
			{
				VmbUint64_t id;
				std::string name;
				std::string serial;
				pFrame->GetFrameID(id);
				m_pCamera->GetModel(name);
				m_pCamera->GetSerialNumber(serial);
				std::cout << "!!!incomplete frame, rcvd error: " << eReceiveStatus << " id: " << id << " camera: " << name << " + " << serial << std::endl;
				m_pCamera->QueueFrame(pFrame);
				return;
			}
			VmbUint64_t id;
			pFrame->GetFrameID(id);
			std::cout << "[FrameObserver cam=" << cam_id << "] rcvd frame ID=" << id << " → ENQUEUE" << std::endl;
			// Lock the frame queue
			m_FramesMutex.lock();
			// Add frame to queue
			m_Frames.push(pFrame);
			// Unlock frame queue
			m_FramesMutex.unlock();
			// Emit the frame received signal
			call(cam_id);
			// callback!
			bQueueDirectly = false;
		}
		else {
			std::cout << "[FrameObserver cam=" << cam_id << "] frame rcvd error" << std::endl;
		}

		// If any error occurred we queue the frame without notification
		if (true == bQueueDirectly)
		{
			m_pCamera->QueueFrame(pFrame);
		}
	};

	FramePtr GetFrame() {
		FramePtr res;
		// Lock the frame queue
		m_FramesMutex.lock();
		// Pop frame from queue
		if (!m_Frames.empty())
		{
			res = m_Frames.front();
			m_Frames.pop();
		}
		// Unlock frame queue
		m_FramesMutex.unlock();
		return res;
	}

	void QueueF(FramePtr& frame) {
		m_pCamera->QueueFrame(frame);
	}

	void ClearFrameQueue() {
		// Lock the frame queue
		m_FramesMutex.lock();
		std::queue<FramePtr> empty;
		std::swap(m_Frames, empty);
		m_FramesMutex.unlock();
	}
private:
	std::queue<FramePtr> m_Frames;
	std::mutex m_FramesMutex;
};

VimbaHandler::VimbaHandler(std::vector<std::string> cameraIds, MQTTClient* mqtt_c, motor_info* motor_i, bool demo) :
	m_vmbSystem(VmbSystem::GetInstance()), mqtt_client(mqtt_c), gimbal_data(motor_i), demo_flag(demo)
{
	VmbErrorType err = m_vmbSystem.Startup();

	if (err != VmbErrorSuccess)
	{
		throw std::runtime_error("Could not start API, err=" + std::to_string(err));
	}

	//CameraPtrVector cameras;
	//err = m_vmbSystem.GetCameras(cameras);
	//if (err != VmbErrorSuccess)
	//{
	//    m_vmbSystem.Shutdown();
	//    throw std::runtime_error("Could not get cameras, err=" + std::to_string(err));
	//}
	//if (cameras.empty())
	//{
	//    m_vmbSystem.Shutdown();
	//    throw std::runtime_error("No cameras found.");
	//}

	if (cameraIds.size() > 0)
	{
		int id = 0;

		for (std::string cameraId : cameraIds)
		{
			CameraPtr   cam;
			err = m_vmbSystem.GetCameraByID(cameraId.c_str(), cam);
			if (err != VmbErrorSuccess)
			{
				m_vmbSystem.Shutdown();
				throw std::runtime_error("No camera found with ID=" + std::string(cameraId) + ", err = " + std::to_string(err));
			}
			else {
				m_cameras.push_back(cam);
			}
		}
	}

	//else
	//{
	//    m_camera = cameras[0];
	//}
	for (CameraPtr camptr : m_cameras)
	{
		err = camptr->Open(VmbAccessModeFull);
		if (err != VmbErrorSuccess)
		{
			m_vmbSystem.Shutdown();
			throw std::runtime_error("Could not open camera, err=" + std::to_string(err));
		}
		else {
			std::string name;
			if (camptr->GetName(name) == VmbErrorSuccess)
			{
				std::cout << "Opened Camera " << name << std::endl;
			}
		}
	}

}



VimbaHandler::~VimbaHandler()
{
	try
	{
		Stop();
	}
	catch (std::runtime_error& e)
	{
		std::cout << e.what() << std::endl;
	}

	m_vmbSystem.Shutdown();
}

void VimbaHandler::Open()
{
}

void VimbaHandler::Close()
{
}

void VimbaHandler::Start()
{
	save_thread_running = true;
	std::cout << "starting Camera thread" << std::endl;
	image_saver_thread = std::thread(&VimbaHandler::SaveImage, this);
	int cam_id = 0;
	for (CameraPtr cam : m_cameras) {
		IFrameObserverPtr FO_ptr;
		ImageQueue8 image_queue;
		m_ImageQueue_vec.push_back(image_queue);
		SP_SET(FO_ptr, new FrameObserver(cam));
		SP_DYN_CAST<FrameObserver>(FO_ptr)->cam_id = cam_id;
		cam_id++;
		VmbErrorType err = cam->StartContinuousImageAcquisition(5, FO_ptr);
		FO_ptr_vec.push_back(FO_ptr);
		if (err != VmbErrorSuccess)
		{
			throw std::runtime_error("Could not start acquisition, err=" + std::to_string(err));
		}
		else {

			SP_DYN_CAST<FrameObserver>(FO_ptr)->registerCallback(std::bind(&VimbaHandler::EnqueueToStoreStruct, this, std::placeholders::_1));
		}
	}
	cam_started = true;
}

void VimbaHandler::Stop()
{
	for (CameraPtr cam : m_cameras) {
		VmbErrorType err = cam->StopContinuousImageAcquisition();
		if (err != VmbErrorSuccess)
		{
			throw std::runtime_error("Could not stop acquisition, err=" + std::to_string(err));
		}
	}
	cam_started = false;
	save_thread_running = false;
	if (image_saver_thread.joinable()) {
		image_saver_thread.join();
	}
}

void VimbaHandler::evaluateCommand(std::string cmd, double val)
{
	if (cmd == "fps" && val > 0)
		ChangeFramerate(val);
	else if (cmd == "jxlq" && val > 0)
		jxlq = val;
	else if (cmd == "jxle" && val > 0)
		jxle = val;
	else if (cmd == "display" && val >= 0)
		display_image = int(val);
}

void VimbaHandler::EnqueueToStoreStruct(int cam_id)
{
	FramePtr frame = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->GetFrame();

	VmbUint32_t         Width;
	VmbUint32_t         Height;
	VmbUint32_t         BufferSize;
	VmbPixelFormatType  PixelFormat;
	const VmbUchar_t* pBuffer(NULL);

	if (VmbErrorSuccess == frame->GetPixelFormat(PixelFormat)
		&& VmbErrorSuccess == frame->GetWidth(Width)
		&& VmbErrorSuccess == frame->GetHeight(Height)
		&& VmbErrorSuccess == frame->GetBufferSize(BufferSize)
		&& VmbErrorSuccess == frame->GetBuffer(pBuffer))
	{
		NanoUnixTimer time;
		long long tstamp = time.Stamp_longlong();
		std::lock_guard<std::mutex> lg(queue_mut);
		ImageStore8Ptr pFrame;
		// in case we reached the maximum number of queued frames
		// take of the oldest and reuse it to store the newly arriving frame

		std::vector<VmbUchar_t> data_in = std::vector<VmbUchar_t>(pBuffer, pBuffer + BufferSize);
		if (m_ImageQueue_vec[cam_id].size() >= 100)
		{
			pFrame = m_ImageQueue_vec[cam_id].front();
			m_ImageQueue_vec[cam_id].pop();
			if (!pFrame->equal(Width, Height, PixelFormat))
			{
				pFrame.reset();
			}
		}
		if (pFrame == NULL)
		{
			pFrame = ImageStore8Ptr(new image_store_8bit(data_in.data(), data_in.size(), Width, Height, PixelFormat, tstamp, cam_id));
		}
		else
		{
			pFrame->setData(data_in.data(), data_in.size(), tstamp);
		}
		m_ImageQueue_vec[cam_id].push(pFrame);

		SP_DYN_CAST<FrameObserver>(FO_ptr_vec[cam_id])->QueueF(frame);
		std::cout << "Enqueue finished Camera:" << cam_id << " -- Queue size: " << m_ImageQueue_vec[cam_id].size() << std::endl;
		if (cam_id == 0)
			cv_proc.notify_one();
	}
}

void VimbaHandler::SaveImage()
{
	while (save_thread_running) {
		std::unique_lock<std::mutex> lock(proc_wait_mut);
		cv_proc.wait(lock);
		ImageStore8Ptr pFrame;
		for (ImageQueue8& imageQueue : m_ImageQueue_vec) {
			while (imageQueue.size() > 0 && save_jxl)
			{
				Timer time;
				queue_mut.lock();
				pFrame = imageQueue.front();
				imageQueue.pop();
				queue_count_rgb = imageQueue.size();
				queue_mut.unlock();


				int imagetype;
				int imagechannels;
				if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 1) {
					imagetype = CV_8UC1;
					imagechannels = 1;
				}
				else if (pFrame->dataSize() / (pFrame->height() * pFrame->width()) == 3) {
					imagetype = CV_8UC3;
					imagechannels = 3;
				}

				cv::Mat img_to_rotate = cv::Mat(pFrame->height(), pFrame->width(), imagetype, pFrame->data());
				cv::Mat img;
				cv::rotate(img_to_rotate, img, cv::ROTATE_90_COUNTERCLOCKWISE);
				if (display_image) {
					cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
					cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
					cv::imshow("Display Image", img);
					cv::waitKey(10);
				}
				std::string cameraname;
				if (pFrame->getCamId() == 0)
					cameraname = "RGB";
				if (pFrame->getCamId() == 1)
					cameraname = "ACR";
				if (pFrame->getCamId() == 2)
					cameraname = "NIR";
				std::string filename = output_dir + "/" + cameraname + "/" + std::to_string(pFrame->getTimestamp()) + ".jxl";
				std::filesystem::path path(filename);
				// Extract the directory part of the path
				std::filesystem::path dir = path.parent_path();
				// Create directories if they don't exist
				if (!dir.empty() && !std::filesystem::exists(dir)) {
					std::filesystem::create_directories(dir);
					std::cout << "Created directories: " << dir << std::endl;
				}
				if (!demo_flag) {
					JPEGXL jxl_writer(img.cols, img.rows, img.data, imagechannels, (float)jxlq, (int)jxle);
					jxl_writer.WriteFile(filename.c_str());
					std::cout << "Camera: " + std::to_string(pFrame->getCamId()) + " -- Compress TIME:" << std::to_string(time.ElapsedMillis()) << " -- SaveQueue size:" << imageQueue.size() << std::endl;
				}
				else {
					std::filesystem::copy("test_smoke.jxl", filename);
				}
				time.Reset();
				// NOTE: optional network upload of saved images to the ground
				// station was removed here; it had hardcoded NFS/SMB paths. If
				// reintroduced it should read its destination from config.
				std::string payload = "{ \"fwt\":\"" + fwt_name + "\" ,\"cam\":\"" + cameraname + "\", \"hdg\":" + std::to_string((int)(gimbal_data->hdg * 10)) + ", \"time\":" + std::to_string(pFrame->getTimestamp()) + " }";
				mqtt_client->publish(mqtt_RGB, payload);
			}
		}
	}
}

bool VimbaHandler::ChangeFramerate(double fr)
{
	VmbErrorType result;
	for (CameraPtr cam : m_cameras) {
		FeaturePtr pFeature;
		result = SP_ACCESS(cam)->GetFeatureByName("AcquisitionFrameRate", pFeature);
		if (result == VmbErrorSuccess)
			result = pFeature->SetValue(fr);
		if (result == VmbErrorSuccess) {
			std::cout << "camera fps changed: " << fr << std::endl;
		}
	}
	if (result == VmbErrorSuccess) {
		return true;
	}
	else {
		std::cout << "camera fps change failed with error:" << result << std::endl;
		return false;
	}
}

void VimbaHandler::TriggerSettle()
{
	for (auto& fo : FO_ptr_vec) {
		SP_DYN_CAST<FrameObserver>(fo)->settle.store(3);
	}
}

bool VimbaHandler::TriggerCamera()
{
	TriggerSettle();
	std::cout << "[TriggerCamera] settle reset to 3, firing 4 triggers (3 settle + 1 real)..." << std::endl;

	FeaturePtr pFeature;
	VmbErrorType result;

	for (int i = 0; i < 4; i++) {
		result = SP_ACCESS(m_cameras[0])->GetFeatureByName("TriggerSoftware", pFeature);
		if (result == VmbErrorSuccess)
			result = pFeature->RunCommand();
		int sl = SP_DYN_CAST<FrameObserver>(FO_ptr_vec[0])->settle.load();
		std::cout << "[TriggerCamera] trigger #" << (i+1) << " settle_before=" << sl << " result=" << result << std::endl;
		std::this_thread::sleep_for(std::chrono::milliseconds(400));
	}

	std::cout << "[TriggerCamera] done, 4th frame should be real image" << std::endl;
	return (result == VmbErrorSuccess);
}

void VimbaHandler::SetTowerName(std::string name)
{
	fwt_name = name;
	mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent";
}

void VimbaHandler::SetOutputDir(std::string dir)
{
	output_dir = std::move(dir);
}