#include <iostream>
#include <map>
#include <string>

#include "epiceye.h"
#include "httplib.h"
#include "nlohmann_json.hpp"

// 连接超时时间：300 milliseconds
#define HTTP_CONNECTION_TIMEOUT 300000
// Read 超时时间 30秒
#define HTTP_READ_TIMEOUT 30
// write 超时时间 30秒
#define HTTP_WRITE_TIMEOUT 30

httplib::Result httpGet(std::string ip, std::string uri) {
    httplib::Client client(ip);
    client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
    client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
    client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
    return client.Get(uri.c_str());
}

httplib::Result httpPost(std::string ip, std::string uri) {
    httplib::Client client(ip);
    client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
    client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
    client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
    return client.Post(uri.c_str());
}

httplib::Result httpPut(std::string ip, std::string uri, std::string content) {
    httplib::Client client(ip);
    client.set_connection_timeout(0, HTTP_CONNECTION_TIMEOUT);
    client.set_read_timeout(HTTP_READ_TIMEOUT, 0);
    client.set_write_timeout(HTTP_WRITE_TIMEOUT, 0);
    return client.Put(uri.c_str(), content, "application/json");
}

#ifdef _WIN32
#include <Winsock2.h>
#include <stdio.h>
#endif

#define EPICEYE_SDK_VERSION "3.3.0"
#define EPICRAW2_HEADER_SIZE 40

#define CAMERA_DISCOVERY_PORT 5665

#define JSON_KEY_SN "sn"
#define JSON_KEY_IP "ip"
#define JSON_KEY_ALIAS "alias"
#define JSON_KEY_MODEL "model"
#define JSON_KEY_WIDTH "width"
#define JSON_KEY_HEIGHT "height"

#define JSON_KEY_CAMERA_MATRIX "cameraMatrix"
#define JSON_KEY_DISTORTION "distortion"
#define JSON_KEY_M "m"
#define JSON_KEY_K1 "k1"
#define JSON_KEY_K2 "k2"
#define JSON_KEY_K3 "k3"
#define JSON_KEY_P1 "p1"
#define JSON_KEY_P2 "p2"

namespace TFTech {

static std::map<std::string, std::vector<float>> undistortLutMap;
const std::vector<uint32_t> cameraDiscoveryPorts = {5665, 5666, 5667, 5668};

bool isValid(httplib::Result &res, std::string message) {
    if (!res) {
        std::cerr << message << ": " << "request failed!" << std::endl;
        return false;
    }
    if (res->status != 200) {
        std::cerr << message << ": " << res->status << " " << httplib::detail::status_message(res->status) << std::endl;
        return false;
    }
    return true;
}

std::string EpicEye::getSDKVersion() {
    return EPICEYE_SDK_VERSION;
}

bool requestUndistortLut(std::string ip) {
    EpicEyeInfo info;
    if (!EpicEye::getInfo(ip, info)) {
        return false;
    }
    httplib::Result res = httpGet(ip, "/api/UndistortLut");

    if (!res) {
        std::cerr << "requestUndistortLut" << ": "
                  << "request failed!" << std::endl;
        return false;
    }
    if (res->status  != 200) {
        if(res->status == 404) {
            std::vector<float> emptyArray;
            undistortLutMap[ip] = emptyArray;
            return true;
        } else {
            std::cerr << "requestUndistortLut" << ": " << res->status << " " << httplib::detail::status_message(res->status) << std::endl;
            return false;
        }
    }

    const float *buffer = reinterpret_cast<const float *>(res->body.c_str());
    uint32_t bufferSize = info.height * info.width * 2;
    std::vector<float> undistortLut(buffer, buffer + bufferSize);
    undistortLutMap[ip] = undistortLut;
    return true;
}

//获取相机信息
bool EpicEye::getInfo(std::string ip, EpicEyeInfo &info) {
    httplib::Result res = httpGet(ip, "/api/EpicEye/Info");
    if (!isValid(res, "getInfo")) {
        return false;
    }
    nlohmann::json infoJson = nlohmann::json::parse(res->body);
    if (infoJson.contains(JSON_KEY_SN) &&
        infoJson.contains(JSON_KEY_IP) &&
        infoJson.contains(JSON_KEY_MODEL) &&
        infoJson.contains(JSON_KEY_ALIAS) &&
        infoJson.contains(JSON_KEY_WIDTH) &&
        infoJson.contains(JSON_KEY_HEIGHT)) {
        // 直接赋值，避免聚合初始化问题
        info.SN = infoJson[JSON_KEY_SN].get<std::string>();
        info.IP = infoJson[JSON_KEY_IP].get<std::string>();
        info.model = infoJson[JSON_KEY_MODEL].get<std::string>();
        info.alias = infoJson[JSON_KEY_ALIAS].get<std::string>();
        info.width = infoJson[JSON_KEY_WIDTH].get<uint32_t>();
        info.height = infoJson[JSON_KEY_HEIGHT].get<uint32_t>();
        return true;
    } else {
        std::cerr << "json parse error!" << std::endl;
        return false;
    }
}

//触发拍照
bool EpicEye::triggerFrame(std::string ip, std::string &frameID, bool pointCloud) {
    std::string paramPointCloud = pointCloud ? "true" : "false";
    std::string uri = "/api/Frame?pointCloud=" + paramPointCloud;
    httplib::Result res = httpPost(ip, uri);
    if (!isValid(res, "triggerFrame")) {
        return false;
    }
    frameID = res->body;
    return true;
}

bool EpicEye::getImage(std::string ip, std::string frameID, void *imageBuffer, int &pixelByteSize) {
    std::string uri = "/api/Frame?frameId=" + frameID;
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getImage")) {
        return false;
    }
    const char *bytes = res->body.c_str();
    int width = *(const int *)(bytes + 8);
    int height = *(const int *)(bytes + 12);
    int dataType = *(const int *)(bytes + 16);
    int cameraMatrixLength = *(const int *)(bytes + 20);
    int distortionLength = *(const int *)(bytes + 24);
    int configStrLength = *(const int *)(bytes + 28);
    int depthDataLength = *(const int *)(bytes + 32);
    int imageDataLength = *(const int *)(bytes + 36);
    memcpy(
        imageBuffer, 
        bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength + depthDataLength, 
        imageDataLength
    );
    pixelByteSize = imageDataLength / width / height;
    return true;
}

bool EpicEye::getPointCloud(std::string ip, std::string frameID, float *pointCloudBuffer) {
    std::string uri = "/api/Frame?frameId=" + frameID;
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getPointCloud")) {
        return false;
    }
    if(undistortLutMap.find(ip) == undistortLutMap.end()) {
        if(!requestUndistortLut(ip)) {
            std::cout << "not using undistortLutMap" << std::endl;
        }
    }
    const std::vector<float> &undistortLut = undistortLutMap[ip];
    bool useUndistortLut = undistortLut.size() > 0 ? true: false;
    
    const char *bytes = res->body.c_str();
    int width = *(const int *)(bytes + 8);
    int height = *(const int *)(bytes + 12);
    int dataType = *(const int *)(bytes + 16);
    int cameraMatrixLength = *(const int *)(bytes + 20);
    int distortionLength = *(const int *)(bytes + 24);
    int configStrLength = *(const int *)(bytes + 28);
    int depthDataLength = *(const int *)(bytes + 32);
    int imageDataLength = *(const int *)(bytes + 36);

    double A = *(const double *)(bytes + EPICRAW2_HEADER_SIZE);
    double B = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 16);
    double C = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 32);
    double D = *(const double *)(bytes + EPICRAW2_HEADER_SIZE + 40);
    const char *data = bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength;
    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
            float Z = *(const float *)(data + (i * width + j) * 4);
            if (Z > 0.1 && Z < 6000) {
                uint32_t index = i * width * 2 + j *2;
                float u = (float)j; 
                float v = (float)i; 
                if(useUndistortLut) {
                     u = undistortLut[index];
                     v =  undistortLut[index + 1];
                }
                *(pointCloudBuffer + (i * width + j) * 3) = (float)(Z * (u - B) / A);
                *(pointCloudBuffer + (i * width + j) * 3 + 1) = (float)(Z * (v - D) / C);
                *(pointCloudBuffer + (i * width + j) * 3 + 2) = Z;
            } else {
                *(pointCloudBuffer + (i * width + j) * 3) = NAN;
                *(pointCloudBuffer + (i * width + j) * 3 + 1) = NAN;
                *(pointCloudBuffer + (i * width + j) * 3 + 2) = NAN;
            }
        }
    }
    return true;
}

bool EpicEye::getDepth(std::string ip, std::string frameID, float *depth) {
    std::string uri = "/api/Depth?frameId=" + frameID;
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getDepth")) {
        return false;
    }
    const char *bytes = res->body.c_str();
    int width = *(const int *)(bytes + 8);
    int height = *(const int *)(bytes + 12);
    int dataType = *(const int *)(bytes + 16);
    int cameraMatrixLength = *(const int *)(bytes + 20);
    int distortionLength = *(const int *)(bytes + 24);
    int configStrLength = *(const int *)(bytes + 28);
    int depthDataLength = *(const int *)(bytes + 32);
    int imageDataLength = *(const int *)(bytes + 36);
    memcpy(
        depth, 
        bytes + EPICRAW2_HEADER_SIZE + cameraMatrixLength + distortionLength + configStrLength,
        depthDataLength
    );
    return true;
}

bool EpicEye::getConfig(std::string ip, nlohmann::json &configJson) {
    std::string uri = "/api/CameraConfig";
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getConfig")) {
        return false;
    }
    try {
        configJson = nlohmann::json::parse(res->body);
        return true;
    } catch(...) {
        std::cerr << "json parse error!" << std::endl;
        return false;
    }
}

bool EpicEye::setConfig(std::string ip, const nlohmann::json &configJson) {
    std::string uri = "/api/CameraConfig";
    std::string content = configJson.dump();
    httplib::Result res = httpPut(ip, uri, content);
    if (!isValid(res, "setConfig")) {
        return false;
    }
    return true;
}

bool EpicEye::getCameraMatrix(std::string ip, std::vector<float> &cameraMatrix) {
    std::string uri = "/api/CameraParameters/Intrinsic/CameraMatrix";
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getIntrinsic")) {
        return false;
    }
    nlohmann::json cameraMatrixJson = nlohmann::json::parse(res->body);
    cameraMatrix.clear();
    for (const auto &element : cameraMatrixJson) {
        cameraMatrix.push_back(element);
    }
    return true;
}

bool EpicEye::getDistortion(std::string ip, std::vector<float> &distortion) {
    std::string uri = "/api/CameraParameters/Intrinsic/Distortion";
    httplib::Result res = httpGet(ip, uri);
    if (!isValid(res, "getDistortion")) {
        return false;
    }
    nlohmann::json distortionJson = nlohmann::json::parse(res->body);
    distortion.clear();
    for (const auto &element : distortionJson) {
        distortion.push_back(element);
    }
    return true;
}

bool EpicEye::searchCamera(std::vector<EpicEyeInfo> &cameraList) {
    cameraList.clear();

    int socketUDPv4 = socket(AF_INET, SOCK_DGRAM, 0);
    int socketUDPv6 = socket(AF_INET6, SOCK_DGRAM, 0);
    if (socketUDPv4 < 0 || socketUDPv6 < 0) {
        return false;
    }

    struct sockaddr_in addr_serv_v4;
    memset(&addr_serv_v4, 0, sizeof(struct sockaddr_in));
    addr_serv_v4.sin_family = AF_INET;
    addr_serv_v4.sin_port = htons(CAMERA_DISCOVERY_PORT);
    addr_serv_v4.sin_addr.s_addr = htonl(INADDR_ANY);

    struct sockaddr_in6 addr_serv_v6;
    memset(&addr_serv_v6, 0, sizeof(struct sockaddr_in6));
    addr_serv_v6.sin6_family = AF_INET6;
    addr_serv_v6.sin6_port = htons(CAMERA_DISCOVERY_PORT);
    addr_serv_v6.sin6_addr = in6addr_any;

    int len_v4, len_v6;
    len_v4 = sizeof(addr_serv_v4);
    len_v6 = sizeof(addr_serv_v6);

#ifdef _WIN32
    int nNetTimeout = 4000;
    if (setsockopt(socketUDPv4, SOL_SOCKET, SO_RCVTIMEO, (char*)&nNetTimeout, sizeof(int)) < 0 ||
        setsockopt(socketUDPv6, SOL_SOCKET, SO_RCVTIMEO, (char*)&nNetTimeout, sizeof(int)) < 0) {
        std::cerr << "searchCamera:: can not set timeout for udp socket!" << std::endl;
        return false;
    }
#else
    struct timeval tv;
    tv.tv_sec = 4;
    tv.tv_usec = 0;
    if (setsockopt(socketUDPv4, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0 ||
        setsockopt(socketUDPv6, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) {
        std::cerr << "searchCamera:: can not set timeout for udp socket!" << std::endl;
        return false;
    }
#endif
    bool bindv4Success = false, bindv6Success = false;
    for (const auto& port : cameraDiscoveryPorts) {
	if (!bindv4Success) {
	    addr_serv_v4.sin_port = htons(port);
	    if (bind(socketUDPv4, (struct sockaddr *)&addr_serv_v4, sizeof(addr_serv_v4)) < 0) {
	        std::cerr << "searchCamera:: ipv4 bind port " << port << " failed!" << std::endl;
	    } else {
	        bindv4Success = true;
	    }
	}
	if (!bindv6Success) {
            addr_serv_v6.sin6_port = htons(port);
            if (bind(socketUDPv6, (struct sockaddr *)&addr_serv_v6, sizeof(addr_serv_v6)) < 0) {
	        std::cerr << "searchCamera:: ipv6 bind port " << port << " failed!" << std::endl;
	    } else {
	        bindv6Success = true;
	    }
	}
	if (bindv4Success && bindv6Success) {
	    break;
	}
    }

    if (!(bindv4Success && bindv6Success)) {
        return false;
    }

    ip_mreq mreq_v4;
    mreq_v4.imr_multiaddr.s_addr = inet_addr("224.0.0.251");
    mreq_v4.imr_interface.s_addr = htonl(INADDR_ANY);
    if (setsockopt(socketUDPv4, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq_v4, sizeof(mreq_v4)) < 0) {
        std::cerr << "searchCamera:: failed to join multicast group for IPv4!" << std::endl;
        return false;
    }

    ipv6_mreq mreq_v6;
    inet_pton(AF_INET6, "ff02::fb", &mreq_v6.ipv6mr_multiaddr);
    mreq_v6.ipv6mr_interface = 0; // Use the default interface
    if (setsockopt(socketUDPv6, IPPROTO_IPV6, IPV6_JOIN_GROUP, (char *)&mreq_v6, sizeof(mreq_v6)) < 0) {
        std::cerr << "searchCamera:: failed to join multicast group for IPv6!" << std::endl;
        return false;
    }

    int recv_num_v4, recv_num_v6;
    char recvBuf_v4[1025], recvBuf_v6[1025];
    std::vector<std::string> ipList;
    struct sockaddr_in addr_client_v4;
    struct sockaddr_in6 addr_client_v6;

    auto pushBackIfNotExists = [&](const std::string& infoStr) {
        nlohmann::json infoJson = nlohmann::json::parse(infoStr);
        if (infoJson.contains(JSON_KEY_SN) &&
            infoJson.contains(JSON_KEY_IP)) {
            EpicEyeInfo info;
            info.SN = infoJson[JSON_KEY_SN];
            info.IP = infoJson[JSON_KEY_IP];
            bool exists = std::any_of(cameraList.begin(), cameraList.end(), [&](const EpicEyeInfo& existingInfo) {
                return existingInfo.IP == info.IP;
            });
            if (!exists) {
                cameraList.push_back(info);
            }
        }
    };

    for (int i = 0; i < 6; i++) {
        recv_num_v4 = recvfrom(socketUDPv4, recvBuf_v4, sizeof(recvBuf_v4) - 1, 0,
                               (struct sockaddr *)&addr_client_v4, (socklen_t *)&len_v4);

        if (recv_num_v4 > 0) {
            char addressArray[INET_ADDRSTRLEN] = {'\0'};
            inet_ntop(AF_INET, &addr_client_v4.sin_addr, addressArray, sizeof(addressArray));
            std::string peerIP = addressArray;
            recvBuf_v4[recv_num_v4] = '\0';
            std::string infoStr = (std::string)recvBuf_v4;
            pushBackIfNotExists(infoStr);
        }

        recv_num_v6 = recvfrom(socketUDPv6, recvBuf_v6, sizeof(recvBuf_v6) - 1, 0,
                               (struct sockaddr *)&addr_client_v6, (socklen_t *)&len_v6);

        if (recv_num_v6 > 0) {
            char addressArray[INET6_ADDRSTRLEN] = {'\0'};
            inet_ntop(AF_INET6, &addr_client_v6.sin6_addr, addressArray, sizeof(addressArray));
            std::string peerIP = addressArray;
            recvBuf_v6[recv_num_v6] = '\0';
            std::string infoStr = (std::string)recvBuf_v6;
            pushBackIfNotExists(infoStr);
        }
    }

#ifdef _WIN32
    closesocket(socketUDPv4);
    closesocket(socketUDPv6);
#else
    close(socketUDPv4);
    close(socketUDPv6);
#endif

    if (cameraList.size() == 0) {
        return false;
    }
    return true;
}
}  // namespace TFTech