#include <c_FordFulkerson.h>
#include <string.h>

#define FF_SENTINEL ((c_size_t)-1)
#define C_MIN(a, b) ((a) < (b) ? (a) : (b))

/* ================================================================================================================== */
/* Edmonds-Karp Augmenting Path Finder Helper (Stack-Safe BFS Queue Engine) */

static c_bool_t c_FordFulkerson_HasAugmentingPath(c_FordFulkerson_t* self, c_FlowNetwork_t* graph, c_size_t s, c_size_t t, c_size_t* queue) {
    for (c_size_t v = 0; v < self->V; ++v) {
        self->marked[v] = C_FALSE;
        self->edge_to[v] = FF_SENTINEL;
    }

    c_size_t head = 0, tail = 0;
    self->marked[s] = C_TRUE;
    queue[tail++] = s;

    while (head < tail) {
        c_size_t v = queue[head++];
        c_UIntArray_t* adj = &graph->adj_list[v];
        c_size_t size = (c_size_t)c_UIntArray_GetSize(adj);

        for (c_size_t i = 0; i < size; ++i) {
            c_uint_t generic_id = 0;
            if (c_UIntArray_Get(adj, i, &generic_id) != C_SUCCESS) continue;

            c_size_t edge_id = (c_size_t)generic_id;
            c_FlowEdge_t* edge = &graph->edges_pool[edge_id];
            c_size_t w = edge->to;

            /* Check residual capacity: forward edge capacity constraint or backward flow return buffer */
            double residual_capacity = edge->capacity - edge->flow;
            if (residual_capacity > 0.0 && !self->marked[w]) {
                self->edge_to[w] = edge_id;
                self->marked[w] = C_TRUE;
                queue[tail++] = w;
                if (w == t) return C_TRUE; /* Short-circuit early if sink is reached */
            }
        }
    }
    return self->marked[t];
}

/* ================================================================================================================== */
/* Core Ford-Fulkerson Solver Engine */

c_err_t c_FordFulkerson_Init(c_FordFulkerson_t* self, c_FlowNetwork_t* graph, c_size_t s, c_size_t t, c_Allocator_t* allocator) {
    if (!self || !graph || s >= graph->V || t >= graph->V || s == t) return C_ERR_PARAM;

    self->allocator = allocator ? *allocator : c_DefaultAllocator;
    self->V = graph->V;
    self->max_flow = 0.0;

    self->edge_to = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));
    self->marked  = (c_bool_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_bool_t));
    c_size_t* queue = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));

    if (!self->edge_to || !self->marked || !queue) {
        if (queue) c_Allocator_Free(&self->allocator, queue);
        c_FordFulkerson_Destroy(self);
        return C_ERR_NOMEM;
    }

    /* Process Edmonds-Karp loop increments dynamically */
    while (c_FordFulkerson_HasAugmentingPath(self, graph, s, t, queue)) {

        /* Step 1: Compute bottleneck bottleneck structural threshold along path tree link entries */
        double bottle = DBL_MAX;
        for (c_size_t v = t; v != s; v = graph->edges_pool[self->edge_to[v]].from) {
            c_FlowEdge_t* edge = &graph->edges_pool[self->edge_to[v]];
            bottle = C_MIN(bottle, edge->capacity - edge->flow);
        }

        /* Step 2: Push bottleneck flow changes into forward and backward matching residual twins */
        for (c_size_t v = t; v != s; v = graph->edges_pool[self->edge_to[v]].from) {
            c_size_t forward_id  = self->edge_to[v];
            c_size_t residual_id = C_FLOW_EDGE_REVERSE(forward_id);

            graph->edges_pool[forward_id].flow  += bottle;
            graph->edges_pool[residual_id].flow -= bottle; /* Reverse path flow compensation */
        }
        self->max_flow += bottle;
    }

    c_Allocator_Free(&self->allocator, queue);
    return C_SUCCESS;
}

void c_FordFulkerson_Destroy(c_FordFulkerson_t* self) {
    if (!self) return;
    if (self->edge_to) c_Allocator_Free(&self->allocator, self->edge_to);
    if (self->marked)  c_Allocator_Free(&self->allocator, self->marked);
    memset(self, 0, sizeof(*self));
}