#include "EpicEyeDevice.h"
#include "VrError.h"
#include "epiceye.h"
#include <iostream>
#include <cstring>

using namespace TFTech;

// 静态工厂方法实现
int IEpicEyeDevice::CreateObject(IEpicEyeDevice** ppDevice)
{
    if (ppDevice == nullptr)
        return ERR_CODE(DEV_CTRL_ERR);

    *ppDevice = new CEpicEyeDevice();
    return (*ppDevice) ? 0 : -1;
}

CEpicEyeDevice::CEpicEyeDevice()
    : m_bConnected(false)
{
    memset(&m_deviceInfo, 0, sizeof(m_deviceInfo));
}

CEpicEyeDevice::~CEpicEyeDevice()
{
    if (m_bConnected)
    {
        Disconnect();
    }
}

int CEpicEyeDevice::GetSDKVersion(std::string& version)
{
    try
    {
        version = EpicEye::getSDKVersion();
        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetSDKVersion exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::SearchCameras(std::vector<EpicEyeDeviceInfo>& deviceList)
{
    try
    {
        std::vector<EpicEyeInfo> cameraList;

        if (!EpicEye::searchCamera(cameraList))
        {
            std::cerr << "SearchCameras failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 转换为我们的数据结构
        deviceList.clear();
        for (const auto& camera : cameraList)
        {
            EpicEyeDeviceInfo info;
            info.serialNumber = camera.SN;
            info.ipAddress = camera.IP;
            info.modelName = camera.model;
            info.alias = camera.alias;
            info.width = camera.width;
            info.height = camera.height;
            deviceList.push_back(info);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "SearchCameras exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::Connect(const std::string& ipAddress)
{
    std::lock_guard<std::mutex> lock(m_mutex);

    if (m_bConnected)
    {
        std::cerr << "Already connected to " << m_ipAddress << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 获取相机信息验证连接
        EpicEyeInfo info;
        if (!EpicEye::getInfo(ipAddress, info))
        {
            std::cerr << "Failed to connect to camera at " << ipAddress << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 保存连接信息
        m_ipAddress = ipAddress;
        m_deviceInfo.serialNumber = info.SN;
        m_deviceInfo.ipAddress = info.IP;
        m_deviceInfo.modelName = info.model;
        m_deviceInfo.alias = info.alias;
        m_deviceInfo.width = info.width;
        m_deviceInfo.height = info.height;
        m_bConnected = true;

        std::cout << "Connected to EpicEye camera: " << m_deviceInfo.serialNumber
                  << " at " << m_ipAddress << std::endl;

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "Connect exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::Disconnect()
{
    std::lock_guard<std::mutex> lock(m_mutex);

    if (!m_bConnected)
        return SUCCESS;

    m_ipAddress.clear();
    memset(&m_deviceInfo, 0, sizeof(m_deviceInfo));
    m_bConnected = false;

    std::cout << "Disconnected from EpicEye camera" << std::endl;
    return SUCCESS;
}

bool CEpicEyeDevice::IsConnected()
{
    return m_bConnected;
}

int CEpicEyeDevice::GetDeviceInfo(EpicEyeDeviceInfo& deviceInfo)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    deviceInfo = m_deviceInfo;
    return SUCCESS;
}

int CEpicEyeDevice::GetWidth(unsigned int& width)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    width = m_deviceInfo.width;
    return SUCCESS;
}

int CEpicEyeDevice::GetHeight(unsigned int& height)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    height = m_deviceInfo.height;
    return SUCCESS;
}

int CEpicEyeDevice::TriggerCapture(std::string& frameID, bool withPointCloud)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        if (!EpicEye::triggerFrame(m_ipAddress, frameID, withPointCloud))
        {
            std::cerr << "TriggerCapture failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "TriggerCapture exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::GetPointCloud(const std::string& frameID, PointCloudData& pointCloud)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 分配点云内存
        unsigned int pointCount = m_deviceInfo.width * m_deviceInfo.height;
        float* buffer = new float[pointCount * 3];  // x, y, z

        // 获取点云数据
        if (!EpicEye::getPointCloud(m_ipAddress, frameID, buffer))
        {
            std::cerr << "GetPointCloud failed for frameID: " << frameID << std::endl;
            delete[] buffer;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 填充点云数据结构
        pointCloud.pData = buffer;
        pointCloud.width = m_deviceInfo.width;
        pointCloud.height = m_deviceInfo.height;
        pointCloud.pointCount = pointCount;
        pointCloud.frameID = frameID;

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetPointCloud exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::GetImage(const std::string& frameID, ImageData2D& image)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 预先分配足够大的内存（假设最大为RGB 3通道）
        unsigned int maxSize = m_deviceInfo.width * m_deviceInfo.height * 3;
        unsigned char* buffer = new unsigned char[maxSize];

        int pixelByteSize = 0;

        // 获取图像数据
        if (!EpicEye::getImage(m_ipAddress, frameID, buffer, pixelByteSize))
        {
            std::cerr << "GetImage failed for frameID: " << frameID << std::endl;
            delete[] buffer;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 填充图像数据结构
        image.pData = buffer;
        image.width = m_deviceInfo.width;
        image.height = m_deviceInfo.height;
        image.pixelByteSize = pixelByteSize;
        image.dataSize = m_deviceInfo.width * m_deviceInfo.height * pixelByteSize;
        image.frameID = frameID;

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetImage exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::GetDepth(const std::string& frameID, DepthData& depth)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 分配深度图内存
        unsigned int dataSize = m_deviceInfo.width * m_deviceInfo.height;
        float* buffer = new float[dataSize];

        // 获取深度图数据
        if (!EpicEye::getDepth(m_ipAddress, frameID, buffer))
        {
            std::cerr << "GetDepth failed for frameID: " << frameID << std::endl;
            delete[] buffer;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 填充深度图数据结构
        depth.pData = buffer;
        depth.width = m_deviceInfo.width;
        depth.height = m_deviceInfo.height;
        depth.dataSize = dataSize;
        depth.frameID = frameID;

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetDepth exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::CapturePointCloud(PointCloudData& pointCloud, unsigned int timeout)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 触发拍摄
        std::string frameID;
        if (TriggerCapture(frameID, true) != 0)
        {
            std::cerr << "CapturePointCloud: TriggerCapture failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 获取点云数据
        if (GetPointCloud(frameID, pointCloud) != 0)
        {
            std::cerr << "CapturePointCloud: GetPointCloud failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "CapturePointCloud exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::CaptureImage(ImageData2D& image, unsigned int timeout)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 触发拍摄（不需要点云）
        std::string frameID;
        if (TriggerCapture(frameID, false) != 0)
        {
            std::cerr << "CaptureImage: TriggerCapture failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 获取图像数据
        if (GetImage(frameID, image) != 0)
        {
            std::cerr << "CaptureImage: GetImage failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "CaptureImage exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::GetCameraIntrinsics(CameraIntrinsics& intrinsics)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        // 获取相机矩阵
        if (!EpicEye::getCameraMatrix(m_ipAddress, intrinsics.cameraMatrix))
        {
            std::cerr << "GetCameraMatrix failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        // 获取畸变参数
        if (!EpicEye::getDistortion(m_ipAddress, intrinsics.distortion))
        {
            std::cerr << "GetDistortion failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetCameraIntrinsics exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::GetConfig(std::string& configJson)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        nlohmann::json json;
        if (!EpicEye::getConfig(m_ipAddress, json))
        {
            std::cerr << "GetConfig failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        configJson = json.dump(4);  // 格式化输出，缩进4个空格
        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "GetConfig exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}

int CEpicEyeDevice::SetConfig(const std::string& configJson)
{
    if (!m_bConnected)
    {
        std::cerr << "Not connected" << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }

    try
    {
        nlohmann::json json = nlohmann::json::parse(configJson);

        if (!EpicEye::setConfig(m_ipAddress, json))
        {
            std::cerr << "SetConfig failed" << std::endl;
            return ERR_CODE(DEV_CTRL_ERR);
        }

        return SUCCESS;
    }
    catch (const std::exception& e)
    {
        std::cerr << "SetConfig exception: " << e.what() << std::endl;
        return ERR_CODE(DEV_CTRL_ERR);
    }
}