///|
/// Weighted graph of relative camera constraints.
pub(all) struct WeightedPoseGraph {
  vertices : Int
  mut weights : Array[Array[Double]]
}

///|
pub fn WeightedPoseGraph::new(vertices : Int) -> WeightedPoseGraph {
  let rows : Array[Array[Double]] = []
  for _ in 0.. Unit {
  if left >= 0 &&
    right >= 0 &&
    left < graph.vertices &&
    right < graph.vertices &&
    left != right {
    graph.weights[left][right] = weight.max(0.0)
    graph.weights[right][left] = weight.max(0.0)
  }
}

///|
pub fn WeightedPoseGraph::disconnect(
  graph : WeightedPoseGraph,
  left : Int,
  right : Int,
) -> Unit {
  graph.connect(left, right, 0.0)
}

///|
pub fn WeightedPoseGraph::weight(
  graph : WeightedPoseGraph,
  left : Int,
  right : Int,
) -> Double {
  if left < 0 || right < 0 || left >= graph.vertices || right >= graph.vertices {
    0.0
  } else {
    graph.weights[left][right]
  }
}

///|
pub fn WeightedPoseGraph::has_edge(
  graph : WeightedPoseGraph,
  left : Int,
  right : Int,
) -> Bool {
  graph.weight(left, right) > 0.0
}

///|
pub fn WeightedPoseGraph::neighbors(
  graph : WeightedPoseGraph,
  vertex : Int,
) -> Array[Int] {
  let result : Array[Int] = []
  if vertex < 0 || vertex >= graph.vertices {
    return result
  }
  for i in 0.. 0.0 {
      result.push(i)
    }
  }
  result
}

///|
pub fn WeightedPoseGraph::degree(
  graph : WeightedPoseGraph,
  vertex : Int,
) -> Int {
  graph.neighbors(vertex).length()
}

///|
pub fn WeightedPoseGraph::edge_count(graph : WeightedPoseGraph) -> Int {
  let mut count = 0
  for i in 0.. 0.0 {
        count += 1
      }
    }
  }
  count
}

///|
pub fn WeightedPoseGraph::weight_sum(graph : WeightedPoseGraph) -> Double {
  let mut result = 0.0
  for i in 0.. Bool {
  if start < 0 || goal < 0 || start >= graph.vertices || goal >= graph.vertices {
    return false
  }
  let visited : Array[Int] = [start]
  let queue : Array[Int] = [start]
  while queue.length() > 0 {
    let current = queue.remove(0)
    if current == goal {
      return true
    }
    for neighbor in graph.neighbors(current) {
      if !visited.contains(neighbor) {
        visited.push(neighbor)
        queue.push(neighbor)
      }
    }
  }
  false
}

///|
pub fn WeightedPoseGraph::component_count(graph : WeightedPoseGraph) -> Int {
  let mut count = 0
  for vertex in 0.. Array[Int] {
  let result : Array[Int] = []
  for vertex in 0.. Int? {
  let neighbors = graph.neighbors(vertex)
  if neighbors.length() == 0 {
    return None
  }
  let mut best = neighbors[0]
  for neighbor in neighbors[1:] {
    if graph.weight(vertex, neighbor) > graph.weight(vertex, best) {
      best = neighbor
    }
  }
  Some(best)
}

///|
pub fn WeightedPoseGraph::normalize_weights(
  graph : WeightedPoseGraph,
) -> WeightedPoseGraph {
  let sum = graph.weight_sum()
  if sum <= 0.0 {
    graph
  } else {
    for i in 0..