#include "c_FloydWarshall.h"


#define FW_SENTINEL ((c_size_t)-1)

static void c_FloydWarshall_FreeMatrices(c_FloydWarshall_t* self) {
    if (self->dist_to) {
        for (c_size_t i = 0; i < self->V; ++i) {
            if (self->dist_to[i]) c_Allocator_Free(&self->allocator, self->dist_to[i]);
        }
        c_Allocator_Free(&self->allocator, self->dist_to);
        self->dist_to = NULL;
    }
    if (self->next_vertex) {
        for (c_size_t i = 0; i < self->V; ++i) {
            if (self->next_vertex[i]) c_Allocator_Free(&self->allocator, self->next_vertex[i]);
        }
        c_Allocator_Free(&self->allocator, self->next_vertex);
        self->next_vertex = NULL;
    }
    if (self->edge_to) {
        for (c_size_t i = 0; i < self->V; ++i) {
            if (self->edge_to[i]) c_Allocator_Free(&self->allocator, self->edge_to[i]);
        }
        c_Allocator_Free(&self->allocator, self->edge_to);
        self->edge_to = NULL;
    }
}

c_err_t c_FloydWarshall_Init(c_FloydWarshall_t* self, c_EdgeWeightedDigraph_t* graph, c_Allocator_t* allocator) {
    if (!self || !graph) return C_ERR_PARAM;

    self->allocator = allocator ? *allocator : c_DefaultAllocator;
    self->V = graph->V;
    self->has_neg_cycle = C_FALSE;

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

    if (!self->dist_to || !self->next_vertex || !self->edge_to) {
        c_FloydWarshall_FreeMatrices(self);
        return C_ERR_NOMEM;
    }

    for (c_size_t i = 0; i < self->V; ++i) {
        self->dist_to[i]     = (double*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(double));
        self->next_vertex[i] = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));
        self->edge_to[i]     = (c_size_t*)c_Allocator_Calloc(&self->allocator, self->V, sizeof(c_size_t));

        if (!self->dist_to[i] || !self->next_vertex[i] || !self->edge_to[i]) {
            c_FloydWarshall_FreeMatrices(self);
            return C_ERR_NOMEM;
        }

        for (c_size_t j = 0; j < self->V; ++j) {
            self->dist_to[i][j]     = (i == j) ? 0.0 : DBL_MAX;
            self->next_vertex[i][j] = FW_SENTINEL;
            self->edge_to[i][j]     = FW_SENTINEL;
        }
    }

    /* Step 1: Base adjacency mapping from graph edges pool */
    for (c_size_t e = 0; e < graph->E; ++e) {
        c_DirectedEdge_t* edge = &graph->edges_pool[e];
        c_size_t u = edge->from;
        c_size_t v = edge->to;

        if (edge->weight < self->dist_to[u][v]) {
            self->dist_to[u][v]     = edge->weight;
            self->next_vertex[u][v] = v;
            self->edge_to[u][v]     = e;
        }
    }

    /* Step 2: Floyd-Warshall dynamic programming loops pass */
    for (c_size_t k = 0; k < self->V; ++k) {
        for (c_size_t i = 0; i < self->V; ++i) {
            if (self->dist_to[i][k] == DBL_MAX) continue;

            for (c_size_t j = 0; j < self->V; ++j) {
                if (self->dist_to[k][j] == DBL_MAX) continue;

                if (self->dist_to[i][j] > self->dist_to[i][k] + self->dist_to[k][j]) {
                    self->dist_to[i][j]     = self->dist_to[i][k] + self->dist_to[k][j];
                    self->next_vertex[i][j] = self->next_vertex[i][k];
                    self->edge_to[i][j]     = self->edge_to[i][k];
                }
            }

            /* Step 3: Negative cycle interception */
            if (self->dist_to[i][i] < 0.0) {
                self->has_neg_cycle = C_TRUE;
                return C_SUCCESS;
            }
        }
    }

    return C_SUCCESS;
}

void c_FloydWarshall_Destroy(c_FloydWarshall_t* self) {
    if (!self) return;
    c_FloydWarshall_FreeMatrices(self);
    self->V = 0;
    self->has_neg_cycle = C_FALSE;
}

/* ================================================================================================================== */
/* Exact Signature Re-implementation Match */

c_err_t c_FloydWarshall_Path(c_FloydWarshall_t* self, c_size_t u, c_size_t v, c_EdgeIdList_t* out_path) {
    if (!self || !out_path || u >= self->V || v >= self->V) return C_ERR_PARAM;
    if (self->has_neg_cycle || !c_FloydWarshall_HasPath(self, u, v)) return C_ERR_FAIL;

    /* If origin equals target, the shortest path requires 0 edge steps */
    if (u == v) return C_SUCCESS;

    c_size_t curr_u = u;
    c_err_t err = C_SUCCESS;

    /* Step forward from origin node 'u' using the pre-computed next_vertex mappings */
    while (curr_u != v) {
        c_size_t next = self->next_vertex[curr_u][v];
        if (next == FW_SENTINEL) return C_ERR_FAIL; /* Structural anomaly anchor guard */

        c_size_t edge_id = self->edge_to[curr_u][v];
        if (edge_id == FW_SENTINEL) return C_ERR_FAIL;

        /* Append the resolved edge token into your sequential vertex list wrapper */
        err = c_EdgeIdList_Append(out_path, (c_uint_t)edge_id);
        if (err != C_SUCCESS) return err;

        /* Move the sliding window origin forward to transition the trace */
        curr_u = next;
    }

    return C_SUCCESS;
}