#pragma once

#include <VmbCPP/VmbCPP.h>
#include <thread>
#include <mutex>
#include <memory>
#include <queue> 
#include <condition_variable>
#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;
public:
    image_store_8bit(const VmbUchar_t* pBuffer, VmbUint32_t BufferByteSize, VmbUint32_t Width, VmbUint32_t Height, VmbPixelFormatType PixelFormat, long long stamp)
        : m_Data(pBuffer, pBuffer + BufferByteSize)
        , m_Width(Width)
        , m_Height(Height)
        , m_PixelFormat(PixelFormat)
        , m_timestamp(stamp)
    {
    }

    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(); }
};

class VimbaHandler
{
public:
    VimbaHandler(const char* cameraId, MQTTClient* mqtt_c, motor_info* motor_i);
    ~VimbaHandler();
	void Open();
	void Close();
	void Start();
	void Stop();
    void evaluateCommand(std::string cmd, double val);
    void EnqueueToStoreStruct();
	void SaveImage();
	bool ChangeFramerate(double fr);
    bool TriggerCamera();


	bool cam_started = false;
    int queue_count_rgb=0;
private:
    double jxlq = 2.0;
    double jxle = 3.0;
    const std::string fwt_name = "Dev";
    const std::string mqtt_RGB = "GGS/FWT/" + fwt_name + "/CamEvent/RGB";
    motor_info* gimbal_data;
    MQTTClient* mqtt_client;
    typedef std::shared_ptr<image_store_8bit> ImageStore8Ptr;
    typedef std::queue<ImageStore8Ptr> ImageQueue8;
    ImageQueue8 m_ImageQueueRGB8;
    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;
	IFrameObserverPtr FO_ptr;
	VmbSystem& m_vmbSystem;
	CameraPtr   m_camera;
};