#include "CameraController.h"
#include <sstream>
#include <iostream>

CameraController::CameraController(std::shared_ptr<VxCamera> camera)
    : camera_(camera), gstPipeline_("videoconvert ! autovideosink") {
    streamingEngine_ = std::make_shared<StreamingEngine>(camera);
}

std::string CameraController::processCommand(const std::string& jsonCommand) {
    std::lock_guard<std::mutex> lock(mutex_);

    // Simple JSON parsing (basic implementation)
    // Format: {"command":"name","params":{...}}

    size_t cmdPos = jsonCommand.find("\"command\"");
    if (cmdPos == std::string::npos) {
        return createErrorResponse("Missing command field");
    }

    size_t colonPos = jsonCommand.find(":", cmdPos);
    size_t quoteStart = jsonCommand.find("\"", colonPos);
    size_t quoteEnd = jsonCommand.find("\"", quoteStart + 1);

    if (quoteStart == std::string::npos || quoteEnd == std::string::npos) {
        return createErrorResponse("Invalid command format");
    }

    std::string command = jsonCommand.substr(quoteStart + 1, quoteEnd - quoteStart - 1);

    // Helper lambda to extract parameter value
    auto getParam = [&jsonCommand](const std::string& paramName) -> std::string {
        size_t pos = jsonCommand.find("\"" + paramName + "\"");
        if (pos == std::string::npos) return "";

        size_t colonPos = jsonCommand.find(":", pos);
        size_t valueStart = jsonCommand.find_first_not_of(" \t\n\r", colonPos + 1);

        if (jsonCommand[valueStart] == '\"') {
            size_t valueEnd = jsonCommand.find("\"", valueStart + 1);
            return jsonCommand.substr(valueStart + 1, valueEnd - valueStart - 1);
        } else {
            size_t valueEnd = jsonCommand.find_first_of(",}", valueStart);
            return jsonCommand.substr(valueStart, valueEnd - valueStart);
        }
    };

    // Route commands
    if (command == "set_format") {
        return handleSetFormat(getParam("width"), getParam("height"),
                              getParam("framerate"), getParam("format"));
    } else if (command == "get_formats") {
        return handleGetFormats();
    } else if (command == "set_exposure") {
        return handleSetExposure(getParam("mode"), getParam("value"));
    } else if (command == "set_whitebalance") {
        return handleSetWhiteBalance(getParam("mode"), getParam("temperature"));
    } else if (command == "set_brightness") {
        return handleSetBrightness(getParam("value"));
    } else if (command == "set_contrast") {
        return handleSetContrast(getParam("value"));
    } else if (command == "set_saturation") {
        return handleSetSaturation(getParam("value"));
    } else if (command == "set_sharpness") {
        return handleSetSharpness(getParam("value"));
    } else if (command == "set_gamma") {
        return handleSetGamma(getParam("value"));
    } else if (command == "set_gain") {
        return handleSetGain(getParam("value"));
    } else if (command == "get_status") {
        return handleGetStatus();
    } else if (command == "start_stream") {
        return handleStartStream();
    } else if (command == "stop_stream") {
        return handleStopStream();
    } else if (command == "set_pipeline") {
        return handleSetPipeline(getParam("pipeline"));
    } else {
        return createErrorResponse("Unknown command: " + command);
    }
}

std::string CameraController::handleSetFormat(const std::string& width, const std::string& height,
                                             const std::string& framerate, const std::string& format) {
    if (streamingEngine_->isRunning()) {
        return createErrorResponse("Cannot change format while streaming");
    }

    try {
        VxFormat fmt;
        fmt.width = std::stoi(width);
        fmt.height = std::stoi(height);
        fmt.framerate = std::stoi(framerate);
        fmt.format = stringToFormat(format);
        fmt.mediatypeIdx = 0;

        if (VxSetFormat(camera_, fmt) != 0) {
            return createErrorResponse("Failed to set format");
        }

        streamingEngine_->setFormat(fmt);
        return createSuccessResponse("Format set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleGetFormats() {
    std::vector<VxFormat> fmtList;
    if (VxGetFormatList(camera_, fmtList) != 0) {
        return createErrorResponse("Failed to get format list");
    }

    std::ostringstream oss;
    oss << "{\"status\":\"success\",\"formats\":[";
    for (size_t i = 0; i < fmtList.size(); i++) {
        if (i > 0) oss << ",";
        oss << "{\"width\":" << fmtList[i].width
            << ",\"height\":" << fmtList[i].height
            << ",\"framerate\":" << fmtList[i].framerate
            << ",\"format\":\"" << formatToString(fmtList[i].format) << "\"}";
    }
    oss << "]}";
    return oss.str();
}

std::string CameraController::handleSetExposure(const std::string& mode, const std::string& value) {
    try {
        int flag = (mode == "auto") ? 1 : 0;
        long expValue = value.empty() ? 0 : std::stol(value);

        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_EXPOSURE,
                                   expValue, flag) != 0) {
            return createErrorResponse("Failed to set exposure");
        }

        return createSuccessResponse("Exposure set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetWhiteBalance(const std::string& mode, const std::string& temperature) {
    try {
        int flag = (mode == "auto") ? 1 : 0;
        long tempValue = temperature.empty() ? 0 : std::stol(temperature);

        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_WHITEBALANCE,
                                   tempValue, flag) != 0) {
            return createErrorResponse("Failed to set white balance");
        }

        return createSuccessResponse("White balance set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetBrightness(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_BRIGHTNESS,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set brightness");
        }
        return createSuccessResponse("Brightness set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetContrast(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_CONTRAST,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set contrast");
        }
        return createSuccessResponse("Contrast set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetSaturation(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_SATURATION,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set saturation");
        }
        return createSuccessResponse("Saturation set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetSharpness(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_SHARPNESS,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set sharpness");
        }
        return createSuccessResponse("Sharpness set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetGamma(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_GAMMA,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set gamma");
        }
        return createSuccessResponse("Gamma set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleSetGain(const std::string& value) {
    try {
        long val = std::stol(value);
        if (VxSetUVCImageProcessing(camera_, VX_UVC_IMAGE_PROPERTIES::UVC_IMAGE_GAIN,
                                   val, 0) != 0) {
            return createErrorResponse("Failed to set gain");
        }
        return createSuccessResponse("Gain set successfully");
    } catch (const std::exception& e) {
        return createErrorResponse(std::string("Invalid parameters: ") + e.what());
    }
}

std::string CameraController::handleGetStatus() {
    std::ostringstream oss;
    oss << "{\"status\":\"success\",\"streaming\":" << (streamingEngine_->isRunning() ? "true" : "false")
        << ",\"pipeline\":\"" << gstPipeline_ << "\"}";
    return oss.str();
}

std::string CameraController::handleStartStream() {
    if (streamingEngine_->isRunning()) {
        return createErrorResponse("Already streaming");
    }

    if (!streamingEngine_->start(gstPipeline_)) {
        return createErrorResponse("Failed to start streaming");
    }

    return createSuccessResponse("Streaming started");
}

std::string CameraController::handleStopStream() {
    if (!streamingEngine_->isRunning()) {
        return createErrorResponse("Not streaming");
    }

    streamingEngine_->stop();
    return createSuccessResponse("Streaming stopped");
}

std::string CameraController::handleSetPipeline(const std::string& pipeline) {
    if (streamingEngine_->isRunning()) {
        return createErrorResponse("Cannot change pipeline while streaming");
    }

    if (pipeline.empty()) {
        return createErrorResponse("Pipeline cannot be empty");
    }

    gstPipeline_ = pipeline;
    streamingEngine_->setPipelineDescription(pipeline);
    return createSuccessResponse("Pipeline set successfully");
}

VX_IMAGE_FORMAT CameraController::stringToFormat(const std::string& format) {
    if (format == "YUY2") return VX_IMAGE_FORMAT::YUY2;
    if (format == "UYVY") return VX_IMAGE_FORMAT::UYVY;
    if (format == "NV12") return VX_IMAGE_FORMAT::NV12;
    if (format == "MJPG") return VX_IMAGE_FORMAT::MJPG;
    if (format == "BGR") return VX_IMAGE_FORMAT::BGR;
    if (format == "RGB") return VX_IMAGE_FORMAT::RGB;
    return VX_IMAGE_FORMAT::NONE;
}

std::string CameraController::formatToString(VX_IMAGE_FORMAT format) {
    switch (format) {
        case VX_IMAGE_FORMAT::YUY2: return "YUY2";
        case VX_IMAGE_FORMAT::UYVY: return "UYVY";
        case VX_IMAGE_FORMAT::NV12: return "NV12";
        case VX_IMAGE_FORMAT::MJPG: return "MJPG";
        case VX_IMAGE_FORMAT::BGR: return "BGR";
        case VX_IMAGE_FORMAT::RGB: return "RGB";
        default: return "NONE";
    }
}

std::string CameraController::createErrorResponse(const std::string& error) {
    return "{\"status\":\"error\",\"message\":\"" + error + "\"}";
}

std::string CameraController::createSuccessResponse(const std::string& message) {
    if (message.empty()) {
        return "{\"status\":\"success\"}";
    }
    return "{\"status\":\"success\",\"message\":\"" + message + "\"}";
}