Ошибка (возможно) в реализации алгоритма Дейкстры

Задача: дан взвешенный ориентированный граф, все вершины пронумерованы начиная с нуля, нужно найти длину кратчайшего пути от вершины с номером 0 до выделенной вершины, кроме того, нужно восстановить сам путь.

Формат входных данных такой: в первой строке записаны три числа: K N M, K - номер выделенной вершины, N - количество вершин, M - количество рёбер. В следующих M строках записаны ребра в формате: from to weight, from - из какой вершины, to - в какую вершину, weight - вес ребра.

Для решения реализовал алгоритм Дейкстры, для восстановления пути в процессе работы вычисляются предшественники (массив p, в p[v] записан номер вершины, которая является предшественником вершины v). Расстояния, или, если точнее, оценки на расстояния от начальной вершины до всех остальных, хранятся в массиве d, в d[v] записано расстояние от вершины 0 до вершины v. Очередь (массив q), в которую записываются вершины, реализовал так: просто массив, индекс элемента - номер вершины, значение - 0 или 1, 0 означает, что для вершины еще не вычислен кратчайший путь, 1 означает обратное. Когда нам нужно вытащить из очереди элемент с наименьшим расстоянием, то мы находим такую вершину v, что d[v] минимально и при этом q[v] == 0. Соответственно, пока очередь не пуста, то есть есть вершины, для которых не вычислен кратчайший путь, то извлекаем вершину с минимальной оценкой расстояния, помечаем ее как обработанную и ослабляем все смежные с ней ребра. Решение не проходит один тест из восьми, неправильный ответ на тесте. Прошу подсказать, где в реализации (или, может быть, в теории) может быть ошибка. Не могу также исключить, что ошибка может быть и в тестирующей системе, но перед тем, как выяснять это вопрос, хотелось бы быть уверенным в том, что в решении нет ошибок.

#define _CRT_SECURE_NO_WARNINGS

#include<stdio.h>
#include<stdlib.h>

#define UNDEFINED -1

int *p, *d, *q;
int **matrix;

int **allocate_matrix(int n)
{
     int **matrix = (int**)malloc(n * sizeof(int*));

     if (!matrix)
     {
         return NULL;
     }

     for (int i = 0; i < n; i++)
     {
         matrix[i] = (int*)malloc(n * sizeof(int));

         if (!matrix[i])
         {
             for (int j = 0; j < i; j++)
             {
                 free(matrix[j]);
             }
             free(matrix);

             return NULL;
         }

         for (int j = 0; j < n; j++)
         {
             matrix[i][j] = UNDEFINED;
         }
     }

     return matrix;
 }

 void free_matrix(int **matrix, int size)
 {
     for (int i = 0; i < size; i++)
     {
         free(matrix[i]);
     }
     free(matrix);
 }     

 void print_matrix(int** matrix, int n)
 {
     for (int i = 0; i < n; i++)
     {
         for (int j = 0; j < n; j++)
         {
             printf("%d ", matrix[i][j]);
         }
         printf("\n");
     }
 }

 void relax(int u, int v)
 {
     if (d[v] > d[u] + matrix[u][v])
     {
         d[v] = d[u] + matrix[u][v];
         p[v] = u;
     }
 }

 int extract_min(int size)
 {
     int result = UNDEFINED;

     for (int i = 0; i < size; i++)
     {
         if (!q[i])
         {
             result = i;
             break;
         }
     }

     if (result == UNDEFINED)
     {
         return result;
     }

     for (int i = result + 1; i < size; i++)
     {
         if (!q[i] && d[i] < d[result])
         {
             result = i;
         }
     }

     return result;
 }

 void dijkstra(int s, int size)
 {
     int u;

     d[s] = 0;

     while (1)
     {
         u = extract_min(size);

         if (u == UNDEFINED)
         {
             break;
         }

         q[u] = 1;

         for (int i = 0; i < size; i++)
         {
             if (matrix[u][i] != UNDEFINED && !q[i])
             {
                 relax(u, i);
             }
         }
     }
 }

 void print_path(FILE *out, int size, int start, int current)
 {
     if (current == start)
     {
         fprintf(out, "%d ", current);
         return;
     }

     print_path(out, size, start, p[current]);
     fprintf(out, "%d ", current);
 }

 int main()
 {
     int target, points, paths, from, to;
     FILE *input = fopen("input.txt", "r");

     if (!input)
     {
         printf("Can't open input.txt");
         return -1;
     }

     fscanf(input, "%d %d %d", &target, &points, &paths);

     matrix = allocate_matrix(points);

     if (!matrix)
     {
         printf("Can't allocate matrix for graph.");
         return -1;
     }

     for (int i = 0; i < paths; i++)
     {
         fscanf(input, "%d %d", &from, &to);
         fscanf(input, "%d", &matrix[from][to]);
     }

     fclose(input);

     p = (int*)malloc(points * sizeof(int));
     d = (int*)malloc(points * sizeof(int));
     q = (int*)calloc(points, sizeof(int));

     if (!p || !d || !q)
     {
         printf("Can't allocate memory for p-array, d-array or q-array.");
         return -1;
     }

     for (int i = 0; i<points; i++)
     {
         p[i] = UNDEFINED;
         d[i] = INT_MAX;
     }

     dijkstra(0, points);

     FILE *output = fopen("output.txt", "w");
     if (!output)
     {
         printf("Can't open output.txt");
         return -1;
     }

     fprintf(output, "%d\n", d[target]);
     print_path(output, points, 0, target);

     fclose(output);

     free_matrix(matrix, points);
     free(p);
     free(q);
     free(d);

     return 0;
 }