Porting of splib's dikh version (PQ as a heap)

Graph is:
   /// <summary>
   /// Usata nella lista di adiacenti del grafo
   /// </summary>
   public struct Adj
   {  public int id;    // id dell'altro estremo
      public double d;  // distanza (o costo) dell'arco
      public decimal arcid; // id dell'arco che congiunge
   }
   
   /// <summary>
   /// Un elemento del grafo stradale. Ogni nodo ha gli archi uscenti negli adiacenti
   /// </summary>
   public class Nodo
   {  public int id;
      public double X;
      public double Y;
      public List<Adj> adiacenti;

      public Nodo()
      {  this.id= int.MinValue;
         this.X = 0.0;
         this.Y = 0.0;
         adiacenti = new List<Adj>();
      }
      public Nodo(int id, double x, double y)
      {  this.id= id;
         this.X = x;
         this.Y = y;
         adiacenti = new List<Adj>();
      }
   }

Then, Dijkstra is:
   
   public class Dijkstra_heap
   {

      public struct node
      {  public double dist;  // tentative shortest path length
         public int parent;   // pointer to parent
         public int heap_pos; // number of position in heap
      }
      public node[] nodes;    // questi sono i nodi di dijkstra, interni

      public Nodo[] grafo;    // questo e' il grafo, come collection di nodi (!)

      #region definitions for heap
      struct heap
      {  public int size;     // the number of the last heap element 
         public int[] nd;     // heap of the pointers to nodes       
      }

      heap d_heap;

      int h_current_pos, h_new_pos, h_pos, h_last_pos, h_degree;
      int node_j, node_k;
      double dist_k, dist_min;

      int HEAP_POWER = 2;
      int NILL = -1;

      bool NODE_IN_HEAP(int node_i ) 
      {  return(nodes[node_i].heap_pos != NILL);
      }

      bool NONEMPTY_HEAP()
      {  return(d_heap.size > 0);
      }

      void PUT_TO_POS_IN_HEAP(int node_i, int pos )
      {  d_heap.nd[pos] = node_i;
         nodes[node_i].heap_pos = pos;
      }

      void Init_heap (int n, int source)
      {  h_degree = 1 << HEAP_POWER;
         d_heap.size = 1;
         d_heap.nd = new int[n+1];
         PUT_TO_POS_IN_HEAP( source, 0 );
      }

      void Heap_decrease_key (int node_i, double dist_i)
      {  for ( h_current_pos =  nodes[node_i].heap_pos;
               h_current_pos > 0;
               h_current_pos = h_new_pos
             )
         {  h_new_pos = ( h_current_pos - 1 ) >> HEAP_POWER;

            node_j = d_heap.nd[h_new_pos];
            if ( dist_i  >=  nodes[node_j].dist ) 
               break;

            PUT_TO_POS_IN_HEAP ( node_j, h_current_pos );
         }

         PUT_TO_POS_IN_HEAP ( node_i, h_current_pos );
      }

      void Insert_to_heap (int node_i)
      {  PUT_TO_POS_IN_HEAP ( node_i, d_heap.size );
         d_heap.size++;
         Heap_decrease_key( node_i, nodes[node_i].dist);
      }

      int Extract_min ()
      {  int node_0;

         node_0 = d_heap.nd[0];
         nodes[node_0].heap_pos = NILL;

         d_heap.size-- ;

         if ( d_heap.size > 0 )
         {  node_k =  d_heap.nd [ d_heap.size ];
            dist_k =  nodes[node_k].dist;

            h_current_pos = 0;

            while ( true )
            {  h_new_pos = ( h_current_pos << HEAP_POWER ) +  1;
               if ( h_new_pos >= d_heap.size ) 
                  break;

               dist_min  = nodes[d_heap.nd[h_new_pos]].dist;

               h_last_pos  = h_new_pos + h_degree;
               if ( h_last_pos > d_heap.size ) 
                  h_last_pos = d_heap.size;

               for ( h_pos = h_new_pos + 1; h_pos < h_last_pos; h_pos ++ )
               {  if ( nodes[d_heap.nd[h_pos]].dist < dist_min )
                  {  h_new_pos = h_pos;
                     dist_min  = nodes[d_heap.nd[h_pos]].dist;
                  }
               }

               if ( dist_k <= dist_min ) 
                  break;

               PUT_TO_POS_IN_HEAP ( d_heap.nd[h_new_pos], h_current_pos );

               h_current_pos = h_new_pos;
            }  // end while

            PUT_TO_POS_IN_HEAP ( node_k, h_current_pos );
         }     // end if
         return(node_0);
      }

      #endregion

      /// <summary>
      /// Algoritmo di Dijkstra, PQ come heap, calcola tutto l'albero dei predecessori
      /// </summary>
      /// <param name="source">Sorgente</param>
      public void dikh (int source)
      {  double dist_new, dist_from;
         int n,node_from, node_to, node_last, i, arc_ij;
         int num_scans = 0;

         // initialization 

         node_last = n = grafo.Length;
         nodes     = new node[n];

         for ( i = 0; i != node_last; i ++ )
         {  nodes[i].parent   = -1;
            nodes[i].dist     = Int32.MaxValue;
            nodes[i].heap_pos = NILL;
         }

         nodes[source].parent = source;
         nodes[source].dist   = 0;

         Init_heap (n, source);

         while ( NONEMPTY_HEAP() )
         {  node_from = Extract_min();

            dist_from = nodes[node_from].dist;
            num_scans ++;
           
            //for (arc_ij = index[node_from]; arc_ij < index[node_from+1]; arc_ij++)
            for (arc_ij = 0; arc_ij < grafo[node_from].adiacenti.Count; arc_ij++)
            {  node_to  = grafo[node_from].adiacenti[arc_ij].id;
               dist_new = dist_from  + grafo[node_from].adiacenti[arc_ij].d;

               if ( dist_new < nodes[node_to].dist)
               {  nodes[node_to].dist   = dist_new;
                  nodes[node_to].parent = node_from;
                  if ( !NODE_IN_HEAP(node_to) )
      	            Insert_to_heap ( node_to );
                  Heap_decrease_key ( node_to, dist_new );
               }
            }
         }
      }  // end dikh

      /// <summary>
      /// Algoritmo di Dijkstra, PQ come heap, calcola un percorso da un nodo ad un altro
      /// </summary>
      /// <param name="source">id nodo origine (pos in grafo)</param>
      /// <param name="dest">id nodo destinazione (pos in grafo)</param>
      /// <returns>distanza fra orgine e dest.</returns>
      public double dikh (int source, int dest)
      {  double dist_new, dist_from, dist=double.MaxValue;
         int n,node_from, node_to, node_last, i, arc_ij;
         int num_scans = 0;

         // initialization 

         node_last = n = grafo.Length;
         nodes     = new node[n];

         for ( i = 0; i != node_last; i ++ )
         {  nodes[i].parent   = -1;
            nodes[i].dist     = Int32.MaxValue;
            nodes[i].heap_pos = NILL;
         }

         nodes[source].parent = source;
         nodes[source].dist   = 0;

         Init_heap (n, source);

         while ( NONEMPTY_HEAP() )
         {  node_from = Extract_min();
            if(node_from == dest)
            {  dist = nodes[node_from].dist;
               goto end;
            }

            dist_from = nodes[node_from].dist;
            num_scans ++;
           
            //for (arc_ij = index[node_from]; arc_ij < index[node_from+1]; arc_ij++)
            for (arc_ij = 0; arc_ij < grafo[node_from].adiacenti.Count; arc_ij++)
            {  node_to  = grafo[node_from].adiacenti[arc_ij].id;
               dist_new = dist_from  + grafo[node_from].adiacenti[arc_ij].d;

               if ( dist_new < nodes[node_to].dist)
               {  nodes[node_to].dist   = dist_new;
                  nodes[node_to].parent = node_from;
                  if ( !NODE_IN_HEAP(node_to) )
      	            Insert_to_heap ( node_to );
                  Heap_decrease_key ( node_to, dist_new );
               }
            }
         }
end:     return(dist);
      }  // end dikh
   }