safesight/src/graph_manager.cpp

#include "graph_manager.h"

#include <fstream>
#include <iostream>
#include <map>
#include <set>

namespace rk3588 {

Graph::Graph(std::string name) : name_(std::move(name)) {}

Graph::~Graph() { Stop(); }

bool Graph::Build(const SimpleJson& graph_cfg, PluginLoader& loader, size_t default_queue_size,
                  QueueDropStrategy default_strategy, std::string& err) {
    const auto& obj = graph_cfg.AsObject();
    auto name_it = obj.find("name");
    if (name_it != obj.end() && name_it->second.IsString()) {
        name_ = name_it->second.AsString(name_);
    }

    // Parse nodes
    auto nodes_it = obj.find("nodes");
    if (nodes_it == obj.end() || !nodes_it->second.IsArray()) {
        err = "Graph missing 'nodes' array";
        return false;
    }

    for (const auto& node_val : nodes_it->second.AsArray()) {
        if (!node_val.IsObject()) {
            err = "Node entry is not object";
            return false;
        }
        NodeEntry entry;
        entry.config = node_val;
        entry.id = node_val.ValueOr<std::string>("id", "");
        entry.type = node_val.ValueOr<std::string>("type", "");
        entry.enabled = node_val.ValueOr<bool>("enable", true);
        if (entry.id.empty() || entry.type.empty()) {
            err = "Node missing id or type";
            return false;
        }
        nodes_.push_back(std::move(entry));
    }

    // Parse edges
    auto edges_it = obj.find("edges");
    if (edges_it == obj.end() || !edges_it->second.IsArray()) {
        err = "Graph missing 'edges' array";
        return false;
    }

    std::map<std::string, NodeEntry*> id_to_node;
    for (auto& n : nodes_) {
        id_to_node[n.id] = &n;
    }

    for (const auto& edge_val : edges_it->second.AsArray()) {
        if (!edge_val.IsArray() || edge_val.AsArray().size() != 2) {
            err = "Edge must be [from, to]";
            return false;
        }
        const auto& edge_arr = edge_val.AsArray();
        std::string from = edge_arr[0].AsString("");
        std::string to = edge_arr[1].AsString("");
        if (from.empty() || to.empty()) {
            err = "Edge has empty endpoint";
            return false;
        }
        auto from_it = id_to_node.find(from);
        auto to_it = id_to_node.find(to);
        if (from_it == id_to_node.end() || to_it == id_to_node.end()) {
            err = "Edge references unknown node";
            return false;
        }
        size_t qsize = default_queue_size;
        QueueDropStrategy strategy = default_strategy;
        if (const auto* qcfg = edge_val.Find("queue")) {
            if (qcfg->IsObject()) {
                qsize = static_cast<size_t>(qcfg->ValueOr<int>("size", static_cast<int>(default_queue_size)));
                std::string strat = qcfg->ValueOr<std::string>("strategy", "drop_oldest");
                if (strat == "drop_oldest") strategy = QueueDropStrategy::DropOldest;
                else strategy = QueueDropStrategy::Block;
            }
        }
        auto queue = std::make_shared<SpscQueue<FramePtr>>(qsize, strategy);
        from_it->second->context.output_queues.push_back(queue);
        // For now support single input per node; first edge wins.
        if (!to_it->second->context.input_queue) {
            to_it->second->context.input_queue = queue;
        }
    }

    // Instantiate nodes via plugins
    for (auto& entry : nodes_) {
        if (!entry.enabled) continue;
        std::string load_err;
        entry.node = loader.Create(entry.type, load_err);
        if (!entry.node) {
            err = load_err;
            return false;
        }
        if (!entry.node->Init(entry.config, entry.context)) {
            err = "Init failed for node " + entry.id;
            return false;
        }
    }

    return true;
}

bool Graph::Start() {
    for (auto& entry : nodes_) {
        if (!entry.enabled || !entry.node) continue;
        if (!entry.node->Start()) {
            std::cerr << "[Graph] failed to start node: " << entry.id << "\n";
            return false;
        }
    }
    std::cout << "[Graph] started graph " << name_ << " with " << nodes_.size() << " nodes\n";
    return true;
}

void Graph::Stop() {
    for (auto it = nodes_.rbegin(); it != nodes_.rend(); ++it) {
        if (it->node) {
            it->node->Stop();
        }
        if (it->context.input_queue) it->context.input_queue->Stop();
        for (auto& q : it->context.output_queues) q->Stop();
    }
}

GraphManager::GraphManager(std::string plugin_dir)
    : loader_(std::move(plugin_dir)) {}

GraphManager::~GraphManager() { StopAll(); }

bool GraphManager::LoadConfigFile(const std::string& path, SimpleJson& out, std::string& err) {
    std::ifstream ifs(path);
    if (!ifs.is_open()) {
        err = "Failed to open config: " + path;
        return false;
    }
    std::string content((std::istreambuf_iterator<char>(ifs)), std::istreambuf_iterator<char>());
    return ParseSimpleJson(content, out, err);
}

bool GraphManager::Build(const SimpleJson& root_cfg, std::string& err) {
    auto graphs_it = root_cfg.AsObject().find("graphs");
    if (graphs_it == root_cfg.AsObject().end() || !graphs_it->second.IsArray()) {
        err = "Root config missing 'graphs' array";
        return false;
    }

    size_t default_queue_size = 8;
    QueueDropStrategy default_strategy = QueueDropStrategy::DropOldest;
    if (const auto* queue_cfg = root_cfg.Find("queue")) {
        if (queue_cfg->IsObject()) {
            default_queue_size = static_cast<size_t>(queue_cfg->ValueOr<int>("size", 8));
            std::string strategy = queue_cfg->ValueOr<std::string>("strategy", "drop_oldest");
            if (strategy == "block") default_strategy = QueueDropStrategy::Block;
        }
    }

    for (const auto& graph_val : graphs_it->second.AsArray()) {
        if (!graph_val.IsObject()) {
            err = "Graph entry is not object";
            return false;
        }
        std::string name = graph_val.ValueOr<std::string>("name", "noname");
        auto graph = std::make_unique<Graph>(name);
        if (!graph->Build(graph_val, loader_, default_queue_size, default_strategy, err)) {
            return false;
        }
        graphs_.push_back(std::move(graph));
    }
    return true;
}

bool GraphManager::StartAll() {
    for (auto& g : graphs_) {
        if (!g->Start()) {
            return false;
        }
    }
    {
        std::lock_guard<std::mutex> lock(mu_);
        running_ = true;
    }
    return true;
}

void GraphManager::StopAll() {
    {
        std::lock_guard<std::mutex> lock(mu_);
        if (!running_) return;
        running_ = false;
    }
    for (auto& g : graphs_) {
        g->Stop();
    }
    cv_.notify_all();
}

void GraphManager::RequestStop() {
    StopAll();
}

void GraphManager::BlockUntilStop() {
    std::unique_lock<std::mutex> lock(mu_);
    cv_.wait(lock, [&] { return !running_; });
}

}  // namespace rk3588