//
// Created by xtkuang on 2025/6/4.
//

#include "mechmind_camera.h"

using namespace std;
using namespace cmvr::device;
using namespace mmind::eye;

MechmindCamera::MechmindCamera(const XmlNode& config) : AbstractCamera(config) {
    try {
        ip_ = config.getAttrString("ip");
        if (ip_.empty()) {
            state_.is_error = true;
            state_.error_message = "ip attribute not set";
            LOG(ERROR) << "[MechmindCamera] (MechmindCamera): ip attribute not set";
            throw std::runtime_error("[MechmindCamera] (MechmindCamera): ip attribute not set");
        }

        align_ = cfg_.getAttrDefault("align", true);

        const string temp = cfg_.getAttrDefault("_2dtype", "color");
        if (temp == "color") {
            image2d_type_ = COLOR;
        }
        else if (temp == "gray") {
            image2d_type_ = GRAY;
        }
    }
    catch (const exception& e) {
        const string error_msg = "[MechmindCamera] (start): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }

}

void MechmindCamera::init() {
    std::lock_guard lock(dev_mtx_);
    clear_error_();
    state_.is_initialized = false;
    state_.is_initialized = true;
}

void MechmindCamera::start() {
    try {
        std::lock_guard lock(dev_mtx_);
        clear_error_();
        auto status = camera_.connect(ip_);
        if (status.isOK()) {
            state_.is_opened = true;
        }
        else {
            const string error_msg = "start camera failed, error_code=" + to_string(status.errorCode);
            LOG(ERROR) << "[MechmindCamera] (start): "<< error_msg;
            throw std::runtime_error(error_msg);
        }
    }
    catch (const std::exception& e) {
        const string error_msg = "[MechmindCamera] (start): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }
}

void MechmindCamera::stop() {
    try {
        std::lock_guard lock(dev_mtx_);
        clear_error_();
        camera_.disconnect();
        state_.is_opened = false;
    }
    catch (const exception &e) {
        const string error_msg = "[MechmindCamera] (stop): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }
}

void MechmindCamera::getRGBImage(cv::Mat& color, Rs2Intrinsics& intrinsics) {
    try {
        std::lock_guard lock(dev_mtx_);
        clear_error_();
        if (!state_.is_initialized || !state_.is_opened) {
            LOG(ERROR) << "[MechmindCamera] (getRGBImage): camera not opened";
            throw runtime_error("[MechmindCamera] (getRGBImage): camera not opened");
        }

        if (image2d_type_ != COLOR) {
            LOG(ERROR) << "[MechmindCamera] (getRGBImage): image 2d type not color";
            throw runtime_error("[MechmindCamera] (getRGBImage): image 2d type not color");
        }

        Frame2D frame2D;
        auto ret = camera_.capture2D(frame2D);
        if (!ret.isOK()) {
            const string error_msg = "get color image failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getRGBImage): "<< error_msg;
            throw std::runtime_error(error_msg);
        }
        switch (frame2D.colorType()) {
            case ColorTypeOf2DCamera::Monochrome:
            {
                GrayScale2DImage grayImage = frame2D.getGrayScaleImage();
                color = cv::Mat(grayImage.height(), grayImage.width(), CV_8UC1, grayImage.data());
                break;
            }
            case ColorTypeOf2DCamera::Color:
            {
                Color2DImage colorImage = frame2D.getColorImage();
                color = cv::Mat(colorImage.height(), colorImage.width(), CV_8UC3, colorImage.data());
                break;
            }
            case ColorTypeOf2DCamera::Undefined:
                break;
        }
    }
    catch (const exception &e) {
        const string error_msg = "[MechmindCamera] (getRGBImage): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }
}

void MechmindCamera::getDepthImage(cv::Mat& depth, Rs2Intrinsics& intrinsics) {
    try {
        std::lock_guard lock(dev_mtx_);
        clear_error_();
        if (!state_.is_initialized || !state_.is_opened) {
            LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened";
            throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened");
        }

        Frame3D frame3D;
        auto ret = camera_.capture3D(frame3D);
        if (!ret.isOK()) {
            const string error_msg = "get depth image failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getDepthImage): "<< error_msg;
            throw std::runtime_error(error_msg);
        }
        DepthMap depthMap = frame3D.getDepthMap();
        depth = cv::Mat(depthMap.height(), depthMap.width(), CV_32FC1, depthMap.data());
    }
    catch (const exception &e) {
        const string error_msg = "[MechmindCamera] (getDepthImage): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }
}

void MechmindCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics) {
    try {
        std::lock_guard lock(dev_mtx_);
        clear_error_();
        if (!state_.is_initialized || !state_.is_opened) {
            LOG(ERROR) << "[MechmindCamera] (getDepthImage): camera not opened";
            throw runtime_error("[MechmindCamera] (getDepthImage): camera not opened");
        }

        Frame2D frame_2d;
        Frame3D frame_3d;
        auto ret = camera_.capture2D(frame_2d);
        if (!ret.isOK()) {
            const string error_msg = "get color failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
            throw std::runtime_error(error_msg);
        }
        Color2DImage color_image = frame_2d.getColorImage();

        ret = camera_.capture3D(frame_3d);
        if (!ret.isOK()) {
            const string error_msg = "get depth failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
            throw std::runtime_error(error_msg);
        }
        DepthMap depth_map = frame_3d.getDepthMap();

        CameraIntrinsics intrinsics;
        showError(camera_.getCameraIntrinsics(intrinsics));

        GrayScale2DImage mask;
        mask.resize(color_image.width(), color_image.height());
        for (int y = 0; y < color_image.height(); ++y)
            for (int x = 0; x < color_image.width(); ++x)
                mask.at(y, x).gray = 255;

        PointCloud pcl;
        ret = getPointCloudAfterMapping(depth_map, mask, intrinsics, pcl);
        if (!ret.isOK()) {
            const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
            throw std::runtime_error(error_msg);
        }

        TexturedPointCloud textured_pcl;
        ret = getPointCloudAfterMapping(depth_map, mask, color_image, intrinsics, textured_pcl);
        if (!ret.isOK()) {
            const string error_msg = "get pcl after mapping failed, error_code=" + to_string(ret.errorCode);
            LOG(ERROR) << "[MechmindCamera] (getRGBDImages): "<< error_msg;
            throw std::runtime_error(error_msg);
        }

        textured_plc_to_rgbd_(textured_pcl, color, depth);
    }
    catch (const exception &e) {
        const string error_msg = "[MechmindCamera] (getRGBDImages): " + string(e.what());
        LOG(ERROR) << error_msg;
        throw std::runtime_error(error_msg);
    }

}

void MechmindCamera::updateParams(const std::pair<std::string, std::string>& param)
{
    //mmind::eye::UserSet currentUserSet = camera_.currentUserSet();
    //std::vector<Parameter*> parameters = currentUserSet.getAvailableParameters();
    throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
}

void MechmindCamera::startRecording(const std::string& video_path) {
    throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
}

void MechmindCamera::stopRecording() {
    throw runtime_error("[MechmindCamera] (startRecording): Not implemented");
}

void MechmindCamera::textured_plc_to_rgbd_(const TexturedPointCloud& cloud, cv::Mat& color, cv::Mat& depth) {
    const int width = cloud.width();
    const int height = cloud.height();
    color = cv::Mat(height, width, CV_8UC3);
    depth = cv::Mat(height, width, CV_32FC1);
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            const PointXYZBGR& pt = cloud.at(y, x);
            if (std::isnan(pt.z)) {
                depth.at<float>(y, x) = 0.0f;
                color.at<cv::Vec3b>(y, x) = {0, 0, 0};  // black for invalid
            } else {
                depth.at<float>(y, x) = pt.z / 1000.0f;  // mm to meters
                color.at<cv::Vec3b>(y, x) = {pt.b, pt.g, pt.r};  // OpenCV is BGR
            }
        }
    }
}