# ダイクストラ(Dijkstra)法
# 印刷教材 p.57 の実装
# dijkstra(V, W)
# V: 点の集合
# W: 枝の重み（距離）の行列、枝がない場合は無限大(10^10で代替)
# 出力 list(p, d)
# p[i]: 最短路における点iの直前の点
# 終点をtとすると、p[t], p[p[t]], p[p[p[t]]], ... と辿ることで、
# 終点から始点までの最短路が分かる。
# d[i]: 始点から点iまでの最短距離
# p, dの取り出し方
# results <- dijkstra(V, W)
# p <- results[[1]]
# d <- results[[2]]

# barSに含まれる点のうち、始点からの距離が最小の点を求める関数
dmin <- function(d, barS) {
  mind <- inf
  m <- 0
  for (i in barS) {
    if (d[i] < mind) {
      mind <- d[i]
      m <- i
    }
  }
  return(m)
}

dijkstra <- function(V, W) {
  inf <- 1e10 # 10^10 無限大の代替
# (0) 初期化
  S <- numeric(0)
  barS <- V
  d <- numeric(length(V))
  for (i in 2:length(V)) {
    d[i] <- inf 
  }
  p <- numeric(length(V))

# (1)
  n <- 0
  while(setequal(V, S) == FALSE) { # S = V なら終了
    cat("ステップ", n, " S=", S, "barS=", barS, "d=", d, "p=", p, "\n")
    v <- dmin(d, barS) # barSに含まれる点のうち始点からの距離が最小の点をvとする

                                        # (2)    
    barS <- setdiff(barS, v) # barSからvを取り除く
    S <- union(S, v) # Sにvを加える
    for (j in barS) { # (v, j) ∈ E かつ j ∈ barS であるすべての枝に対し
      if ( d[v] + W[v, j] < d[j] ) { 
        d[j] <- d[v] + W[v, j]
        p[j] <- v
      }
    }
    n <- n + 1
  }
  cat("ステップ", n, " S=", S, "barS=", barS, "d=", d, "p=", p, "\n")
  return(list(p, d))
}


# 印刷教材 p.48 の例1
inf <- 1e10 # 10^10 ... 無限大の代替
V <- 1:6
W <- matrix(c(0, 1, 5, inf, inf, inf, 1, 0, 2, 2, inf, inf, 5, 4, 0, inf, 2, inf, inf, inf, 3, 0, inf, 3, inf, inf, 1, inf, 0, 4, inf, inf, inf, inf, inf, 0), ncol=6, byrow=T)
results <- dijkstra(V, W)
cat("p=", results[[1]], "d=", results[[2]], "\n")

# dijkstra(V, W)
# 
# [[1]] # p
# [1] 0 1 2 2 3 4
#
# [[2]] # d
# [1] 0 1 3 3 5 6
#
# 点1から点5への最短路は p[5] = 3, p[3] = 2, p[2] = 1 であるから、
# 1 → 2 → 3 → 5
# その距離は d[5] = 4