not stable graph algo c++

#include <iostream>

using namespace std;


int v[1000];
float cache_1[1000];
int cache_2[1000];
int s[5000];
int e[5000];
float l[5000];
int graph_1[1000][20];
float graph_2[1000][20];

int get_len(int arr[]) {
  int len = 0;
  //int i=0;
  while (arr[len] > -2) {
    len++;
    //i++;
  }

  return len;
}

void myfill(int arr[]) {
  for (int i=0; i<1000; i++) {
    arr[i] = -2;
  }
}
void init_arr() {
  for (int i=0; i < 1000; ++i) {
    v[i] = -2;
    cache_1[i] = -2.0;
    cache_2[i] = -2;
  }
  for (int i=0; i < 5000; i++) {
    s[i] = -2;
    e[i] = -2;
    l[i] = -2.0;
  }
  for (int i=0; i < 1000; i++) {
    for (int j=0; j < 20; j++) {
      graph_1[i][j] = -2;
      graph_2[i][j] = -2.0;
    }
  }

}



void add_point(int i, int point) {
  v[i] = point;
}

void add_edge(int i, int s_p, int e_p, float edge_len) {
  s[i] = s_p;
  e[i] = e_p;
  l[i] = edge_len;
}

void fill_graph() {
  for (int i=0; i < 5000; i++) {
    int start = s[i];
    int end = e[i];
    float dist = l[i];
    if (start == -2) {
      break;
    }
    // найдём место, куда вставить
    int j = 0;
    while (graph_1[start][j] > -2) {
      j++;
    }
    graph_1[start][j] = end;
    graph_2[start][j] = dist;
    j=0;
    while (graph_1[end][j] > -2) {
      j++;
    }
    graph_1[end][j] = start;
    graph_2[end][j] = dist;
  }
}

void get_dists(int a) {

  int L=0;
  for (int i=0; i<1000; i++) {
    if (graph_1[i][0] > -2) {
      L++;
    } else {
      break;
    }
  }

  int links_1[1000][20];
  float links_2[1000][20];

  for (int i=0; i < 1000; i++) {
    for (int j=0; j < 20; j++) {
      links_1[i][j] = -2;
      links_2[i][j] = -2.0;
    }
  }

  int i = L;

  while (--i >= 0) {
    int neighbors_verts[20];
    float neighbors_dists[20];
    for (int j=0; j<20; j++) {
      neighbors_verts[j] = graph_1[i][j];
    }
    for (int j=0; j < 20; j++) {
      neighbors_dists[j] = graph_2[i][j];
    }

    cache_1[i] = 100000.0;
    cache_2[i] = i;
    for (int j=0; j < 20; j++) {

      links_1[i][j] = neighbors_verts[j];
      links_2[i][j] = neighbors_dists[j];
    }
  }


  float root_1 = cache_1[a];
  int root_2 = cache_2[a];

  cache_1[a] = 0;
  cache_2[a] = -1;


  root_1 = 0.0;
  root_2 = -1;

  float queue_1[1000];
  int queue_2[1000];
  int queue_ids[1000];

  for (int j=0; j < 1000; j++) {
    queue_1[j] = -2.0;
    queue_2[j] = -2;
    queue_ids[j] = -2;
  }

  queue_1[0] = root_1;
  queue_2[0] = root_2;
  queue_ids[0] = 0;

  i=0;
  int len = 1;


  while (i < len) {


    float node_1 = queue_1[i];
    int node_2 = queue_2[i];
    int node_id = queue_ids[i];


    int node_verts[20];
    float node_dists[20];

    int node_verts_len = 0;

    for (int j=0; j < 20; j++) {
      node_verts[j] = links_1[node_id][j];
      node_dists[j] = links_2[node_id][j];
      if (node_verts[j] > -2) {
        node_verts_len++;
      }
    }

    //int j = get_len(node_verts);
    int j = node_verts_len-1;
    while (j >= 0) {

      int link_vert = node_verts[j];
      float link_dist = node_dists[j];

      float c1 = cache_1[link_vert];
      int c2 = cache_2[link_vert];

      float d = node_1 + link_dist;

      if (d < c1) {
        cout << "update " << endl;
        cache_1[link_vert] = d;

        cache_2[link_vert] = node_id;

        queue_1[len] = d;

        queue_ids[len] = link_vert;
        queue_2[len] = node_id;
        len+=1;

      }

      j--;
    }


    i++;

  }


}

int get_graph_1(int i, int j) {
  return graph_1[i][j];
}

float get_graph_2(int i, int j) {
  return graph_2[i][j];
}

float get_cache_1(int i) {
  return cache_1[i];
}

int get_cache_2(int i) {
  return cache_2[i];
}

int get_v(int i) {
  return v[i];
}

int get_sum() {
  int sum = 0;
  for (int i=0; i < 1000; i++) {
    sum += v[i];
  }

  return sum;
}


int main() {

    init_arr();

    add_point(0, 0);
    add_point(1, 1);
    add_point(2, 2);
    add_point(3, 3);
    add_point(4, 4);

    add_edge(0, 0, 2, 1);
    add_edge(1, 0, 1, 1);
    add_edge(2, 1, 2, 1);
    add_edge(3, 2, 3, 1);
    add_edge(4, 2, 4, 1);
    add_edge(5, 3, 4, 1);

    fill_graph();

    get_dists(0);

    for (int i=0; i < 5; i++) {
        cout << i << " " << cache_1[i] << " " << cache_2[i] << endl;
    }

  return 0;
}