#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()> Callback;
    Callback call;

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

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

    void FrameReceived(const FramePtr pFrame)
    {
        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;
            }
            // 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();
            // callback!
            bQueueDirectly = false;
        }
        else {
            std::cout << "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(const char* cameraId, MQTTClient* mqttc, motor_info* motor_i) :
    m_vmbSystem(VmbSystem::GetInstance()), mqtt_client(mqttc), gimbal_data(motor_i)
{
    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 (cameraId != nullptr)
    {
        err = m_vmbSystem.GetCameraByID(cameraId, m_camera);
        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_camera = cameras[0];
    }

    err = m_camera->Open(VmbAccessModeFull);
    if (err != VmbErrorSuccess)
    {
        m_vmbSystem.Shutdown();
        throw std::runtime_error("Could not open camera, err=" + std::to_string(err));
    }

    std::string name;
    if (m_camera->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);
    SP_SET(FO_ptr, new FrameObserver(m_camera));
    VmbErrorType err = m_camera->StartContinuousImageAcquisition(5, FO_ptr);
    if (err != VmbErrorSuccess)
    {
        throw std::runtime_error("Could not start acquisition, err=" + std::to_string(err));
    }
    else {
        cam_started = true;
        SP_DYN_CAST<FrameObserver>(FO_ptr)->registerCallback(std::bind(&VimbaHandler::EnqueueToStoreStruct,this));
    }
}

void VimbaHandler::Stop()
{
    VmbErrorType err = m_camera->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()
{
    FramePtr frame = SP_DYN_CAST<FrameObserver>(FO_ptr)->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_ImageQueueRGB8.size() >= 100)
        {
            pFrame = m_ImageQueueRGB8.front();
            m_ImageQueueRGB8.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));
        }
        else
        {
            pFrame->setData(data_in.data(), data_in.size(), tstamp);
        }
        m_ImageQueueRGB8.push(pFrame);

        SP_DYN_CAST<FrameObserver>(FO_ptr)->QueueF(frame);  
        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;
        while (m_ImageQueueRGB8.size() > 0 && save_jxl)
        {
            std::cout << "RGB SaveQueue size: " << m_ImageQueueRGB8.size() << std::endl;
            Timer time;
            queue_mut.lock();
            pFrame = m_ImageQueueRGB8.front();
            m_ImageQueueRGB8.pop();
            queue_count_rgb = m_ImageQueueRGB8.size();
            queue_mut.unlock();
            cv::Mat img_to_rotate = cv::Mat(pFrame->height(), pFrame->width(), CV_8UC3, 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 homedir = getenv("HOME");
            std::string filename = homedir+ "/projects/Fire_Gimbal_Control/bin/x64/Release/RGB/" + std::to_string(pFrame->getTimestamp()) + ".jxl";
            JPEGXL jxl_writer(img.cols, img.rows, img.data, 3, (float)jxlq, (int)jxle);
            jxl_writer.WriteFile(filename.c_str());
            std::cout << "RGB Compress TIME:" << std::to_string(time.ElapsedMillis()) << std::endl;
            time.Reset();
            //cv::imwrite(filename, img, compression_params);
            std::string net_path = "/mnt/ggs-smb/FirewatchTowers/FWT_Podrosche/";
            std::string backup_net_path = "/home/ggs/fwt_image_backup/";
            //try {
            //    std::filesystem::copy_file(filename, net_path + filename);
            //    std::cout << "RGB Upload TIME:" << std::to_string(time.ElapsedMillis()) << std::endl;
            //}
            //catch (std::filesystem::filesystem_error& e)
            //{
            //    std::cout << "Could not copy image to zkms: " << e.what() << '\n';
            //    std::cout << "Copy Backup to LattePanda Sigma" << '\n';
            //    try {
            //        std::filesystem::copy_file(filename, backup_net_path + filename);
            //    }
            //    catch (std::filesystem::filesystem_error& e)
            //    {
            //        std::cout << "Could not copy image to LattePanda Sigma: " << e.what() << '\n';
            //    }
            //}
            //std::filesystem::remove(filename);
            mqtt_client->publish(mqtt_RGB,std::to_string((int)(gimbal_data->hdg*10)) + "," + std::to_string(pFrame->getTimestamp()));
        }
    }
}

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

bool VimbaHandler::TriggerCamera()
{
    VmbErrorType result;
    FeaturePtr pFeature;
    result = SP_ACCESS(m_camera)->GetFeatureByName("TriggerSoftware", pFeature);
    if (result == VmbErrorSuccess)
        result = pFeature->RunCommand();
    if (result == VmbErrorSuccess) {
        std::cout << "camera triggerd " << std::endl;
        return true;
    }
    std::cout << "camera trigger failed with error:" << result << std::endl;
    return false;
}