#include <c_BellmanFordSP.h>
#include <c_Digraph.h>
#include "c_DirectedCycle.h"

#define SP_SENTINEL ((c_size_t)-1)

/* Private Negative Cycle Scanner Helper Routine using an ad-hoc local graph projection */
static void c_BellmanFord_FindNegativeCycle(c_BellmanFordSP_t* self) {
    /* Create a temporary lightweight unweighted digraph to map our current edge_to routing tree */
    c_Digraph_t spt_graph;
    c_err_t err = c_Digraph_Init(&spt_graph, self->V, &self->allocator);
    if (err != C_SUCCESS) return;

    for (c_size_t v = 0; v < self->V; ++v) {
        if (self->edge_to[v] != SP_SENTINEL) {
            c_size_t parent = self->from_vertex[v];
            c_Digraph_AddEdge(&spt_graph, parent, v);
        }
    }

    /* Reuse your pre-built stack-safe directed cycle detector */
    c_DirectedCycle_t detector;
    err = c_DirectedCycle_Init(&detector, &spt_graph, &self->allocator);
    if (err == C_SUCCESS) {
        if (c_DirectedCycle_HasCycle(&detector)) {
            /* Safely clone the cycle path sequence back into our persistent collection structure */
            c_DirectedCycle_GetCycle(&detector, &self->cycle);
        }
        c_DirectedCycle_Destroy(&detector);
    }
    c_Digraph_Destroy(&spt_graph);
}

/* Private FIFO relaxation subroutine */
static void c_BellmanFord_Relax(c_BellmanFordSP_t* self, const c_EdgeWeightedDigraph_t* graph, c_size_t v, c_size_t* queue, c_size_t* tail, c_size_t max_q_cap) {
    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_edge_id = 0;
        if (c_UIntArray_Get(adj, i, &generic_edge_id) != C_SUCCESS) continue;

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

        if (self->dist_to[w] > self->dist_to[v] + edge->weight) {
            self->dist_to[w] = self->dist_to[v] + edge->weight;
            self->edge_to[w] = edge_id;
            self->from_vertex[w] = v;

            if (!self->on_queue[w]) {
                queue[(*tail) % max_q_cap] = w;
                (*tail)++;
                self->on_queue[w] = C_TRUE;
            }
        }

        /* Periodically verify negative tree invariants every V edge relaxations */
        if (++self->cost_counter % self->V == 0) {
            c_BellmanFord_FindNegativeCycle(self);
            if (c_BellmanFordSP_HasNegativeCycle(self)) return; /* Stop early to optimize performance */
        }
    }
}

c_err_t c_BellmanFordSP_Init(c_BellmanFordSP_t* self, c_EdgeWeightedDigraph_t* graph, c_size_t s, c_Allocator_t* allocator) {
    if (!self || !graph || s >= graph->V) return C_ERR_PARAM;

    self->allocator = allocator ? *allocator : c_DefaultAllocator;
    self->s = s;
    self->V = graph->V;
    self->cost_counter = 0;
    c_VertexIdList_Init(&self->cycle, 0, allocator);

    self->edge_to    = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));
    self->from_vertex = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));
    self->dist_to    = (double*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(double));
    self->on_queue   = (c_bool_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_bool_t));

    if (!self->edge_to || !self->from_vertex || !self->dist_to || !self->on_queue) {
        c_BellmanFordSP_Destroy(self);
        return C_ERR_NOMEM;
    }

    for (c_size_t v = 0; v < self->V; ++v) {
        self->dist_to[v]     = DBL_MAX;
        self->edge_to[v]     = SP_SENTINEL;
        self->from_vertex[v] = SP_SENTINEL;
        self->on_queue[v]   = C_FALSE;
    }
    self->dist_to[s] = 0.0;

    /* Allocate circular FIFO layout array queue bounded at capacity limit V + 1 */
    c_size_t max_q_cap = self->V + 1;
    c_size_t* queue = (c_size_t*)c_Allocator_Calloc(&self->allocator, max_q_cap, sizeof(c_size_t));
    if (!queue) {
        c_BellmanFordSP_Destroy(self);
        return C_ERR_NOMEM;
    }

    c_size_t head = 0;
    c_size_t tail = 0;

    /* Enqueue source node */
    queue[tail++] = s;
    self->on_queue[s] = C_TRUE;

    while (head < tail && !c_BellmanFordSP_HasNegativeCycle(self)) {
        c_size_t v = queue[head % max_q_cap];
        head++;
        self->on_queue[v] = C_FALSE;

        c_BellmanFord_Relax(self, graph, v, queue, &tail, max_q_cap);
    }

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

void c_BellmanFordSP_Destroy(c_BellmanFordSP_t* self) {
    if (!self) return;
    if (self->edge_to)     c_Allocator_Free(&self->allocator, self->edge_to);
    if (self->from_vertex) c_Allocator_Free(&self->allocator, self->from_vertex);
    if (self->dist_to)     c_Allocator_Free(&self->allocator, self->dist_to);
    if (self->on_queue)    c_Allocator_Free(&self->allocator, self->on_queue);

    c_VertexIdList_Destroy(&self->cycle);

    self->edge_to     = NULL;
    self->from_vertex = NULL;
    self->dist_to     = NULL;
    self->on_queue    = NULL;
    self->V = 0;
    self->s = 0;
    self->cost_counter = 0;
}

c_err_t c_BellmanFordSP_PathTo(c_BellmanFordSP_t* self, c_size_t v, c_VertexIdList_t* out_path) {
    if (!self || !out_path || v >= self->V) return C_ERR_PARAM;
    if (c_BellmanFordSP_HasNegativeCycle(self) || !c_BellmanFordSP_HasPathTo(self, v)) return C_ERR_FAIL;

    c_size_t* edge_stack = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));
    if (!edge_stack) return C_ERR_NOMEM;

    c_size_t stack_size = 0;
    c_size_t curr_v = v;

    while (curr_v != self->s) {
        c_size_t edge_id = self->edge_to[curr_v];
        if (edge_id == SP_SENTINEL) break;

        edge_stack[stack_size++] = edge_id;
        curr_v = self->from_vertex[curr_v];
    }

    c_err_t err = C_SUCCESS;
    while (stack_size > 0) {
        c_size_t target_edge_id = edge_stack[--stack_size];
        err = c_VertexIdList_Append(out_path, (c_uint_t)target_edge_id);
        if (err != C_SUCCESS) break;
    }

    c_Allocator_Free(&self->allocator, edge_stack);
    return err;
}

c_err_t c_BellmanFordSP_GetNegativeCycle(c_BellmanFordSP_t* self, c_VertexIdList_t* out_cycle) {
    if (!self || !out_cycle) return C_ERR_PARAM;
    if (!c_BellmanFordSP_HasNegativeCycle(self)) return C_ERR_FAIL;
    return c_UIntArray_Copy(out_cycle, (c_UIntArray_t*)&self->cycle);
}