#pragma once

#include <VmbCPP/VmbCPP.h>
#include <thread>
#include <mutex>
#include <memory>
#include <queue> 
#include <condition_variable>
#include <atomic>
#include "MQTT.h"
#include "Serial.h"
using namespace VmbCPP;

struct image_store_8bit
{
private:
	typedef std::vector<VmbUchar_t> data_vec;
	data_vec                    m_Data;
	VmbUint32_t                 m_Width;            // frame width
	VmbUint32_t                 m_Height;           // frame height
	VmbPixelFormat_t            m_PixelFormat;      // frame pixel format
	long long m_timestamp;
	int m_cam_id;
public:
	image_store_8bit(const VmbUchar_t* pBuffer, VmbUint32_t BufferByteSize, VmbUint32_t Width, VmbUint32_t Height, VmbPixelFormatType PixelFormat, long long stamp, int cam_id)
		: m_Data(pBuffer, pBuffer + BufferByteSize)
		, m_Width(Width)
		, m_Height(Height)
		, m_PixelFormat(PixelFormat)
		, m_timestamp(stamp)
		, m_cam_id(cam_id)
	{
	}

	long long getTimestamp() {
		return(m_timestamp);
	}

	bool equal(VmbUint32_t Width, VmbUint32_t Height, VmbPixelFormat_t PixelFormat) const
	{
		return      m_Width == Width
			&& m_Height == Height
			&& m_PixelFormat == PixelFormat;
	}

	bool setData(const VmbUchar_t* Buffer, VmbUint32_t BufferSize, long long stamp)
	{
		if (BufferSize == dataSize())
		{
			std::copy(Buffer, Buffer + BufferSize, m_Data.begin());
			m_timestamp = stamp;
			return true;
		}
		return false;
	}
	VmbPixelFormat_t    pixelFormat()   const { return m_PixelFormat; }
	VmbUint32_t         width()         const { return m_Width; }
	VmbUint32_t         height()        const { return m_Height; }
	VmbUint32_t         dataSize()      const { return static_cast<VmbUint32_t>(m_Data.size()); }
	const VmbUchar_t* data()          const { return &*m_Data.begin(); }
	VmbUchar_t* data() { return &*m_Data.begin(); }
	int getCamId() { return m_cam_id; }
};

class VimbaHandler
{
public:
	VimbaHandler(std::vector<std::string> cameraIds, MQTTClient* mqtt_c, motor_info* motor_i, bool demo);
	~VimbaHandler();
	void Open();
	void Close();
	void Start();
	void Stop();
	void evaluateCommand(std::string cmd, double val);
	void EnqueueToStoreStruct(int cam_id);
	void SaveImage();
	bool ChangeFramerate(double fr);
	bool TriggerCamera();
	void TriggerSettle();
	void SetTowerName(std::string name);

	bool cam_started = false;
	int queue_count_rgb = 0;
private:
	bool demo_flag = false;
	double jxlq = 2.0;
	double jxle = 3.0;
	std::string fwt_name = "Rietschen";//"Dev";   //TODO: put in config
	std::string mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent";
	motor_info* gimbal_data;
	MQTTClient* mqtt_client;
	typedef std::shared_ptr<image_store_8bit> ImageStore8Ptr;
	typedef std::queue<ImageStore8Ptr> ImageQueue8;
	std::vector <ImageQueue8> m_ImageQueue_vec;
	std::mutex queue_mut;
	std::mutex proc_wait_mut;
	std::condition_variable cv_proc;
	bool save_thread_running = false;
	bool save_jxl = true;
	bool display_image = false;
	std::thread image_saver_thread;

	std::vector<IFrameObserverPtr> FO_ptr_vec;
	VmbSystem& m_vmbSystem;
	CameraPtrVector m_cameras;

};