// Copyright 2025 International Digital Economy Academy
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

///|
const MAX_SLEEP_TRAVERSAL_COST : Int = 1000

///|
const MIN_ISLAND_SIZE : Int = 1024

///|
const MAX_ISLAND_SIZE : Int = 4096

///|
struct Island {
  bodies : Array[RigidBodyHandle]
  mut id_in_awake_list : Int?
  mut additional_solver_iterations : Int
}

///|
fn Island::Island(
  handle : RigidBodyHandle,
  additional_solver_iterations : Int,
) -> Island {
  { bodies: [handle], id_in_awake_list: None, additional_solver_iterations }
}

///|
struct SleepCandidate {
  handle : RigidBodyHandle
}

///|
pub struct ContactGraph {
  pairs : Array[(RigidBodyHandle, RigidBodyHandle)]
}

///|
pub fn ContactGraph::ContactGraph() -> ContactGraph {
  { pairs: [] }
}

///|
pub fn ContactGraph::clear(self : ContactGraph) -> Unit {
  self.pairs.clear()
}

///|
pub fn ContactGraph::add_pair(
  self : ContactGraph,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
) -> Unit {
  self.pairs.push((body1, body2))
}

///|

///|
priv struct IslandsOptimizerMergeState {
  mut curr_awake_id : Int
}

///|
priv struct IslandsOptimizerSplitState {
  mut curr_awake_id : Int
}

///|
struct IslandsOptimizer {
  mut min_island_size : Int
  mut max_island_size : Int
  mut mode : Int
  merge_state : IslandsOptimizerMergeState
  split_state : IslandsOptimizerSplitState
}

///|
fn IslandsOptimizer::IslandsOptimizer() -> IslandsOptimizer {
  {
    min_island_size: MIN_ISLAND_SIZE,
    max_island_size: MAX_ISLAND_SIZE,
    mode: 0,
    merge_state: { curr_awake_id: 0 },
    split_state: { curr_awake_id: 0 },
  }
}

///|
pub struct IslandManager {
  islands : Array[Island?]
  awake_islands : Array[Int]
  free_islands : Array[Int]
  traversal_candidates : Array[SleepCandidate]
  mut traversal_timestamp : Int
  optimizer : IslandsOptimizer
  stack : Array[RigidBodyHandle]
  body_island_ids : Array[Int]
  body_island_generations : Array[Int]
  body_island_indices : Array[Int]
  body_state_generations : Array[Int]
}

///|
pub fn IslandManager::IslandManager() -> IslandManager {
  let manager = {
    islands: [],
    awake_islands: [],
    free_islands: [],
    traversal_candidates: [],
    traversal_timestamp: 0,
    optimizer: IslandsOptimizer(),
    stack: [],
    body_island_ids: [],
    body_island_generations: [],
    body_island_indices: [],
    body_state_generations: [],
  }
  manager.set_island_size_limits(MIN_ISLAND_SIZE, MAX_ISLAND_SIZE)
  manager
}

///|
fn IslandManager::set_island_size_limits(
  self : IslandManager,
  min_island_size : Int,
  max_island_size : Int,
) -> Unit {
  self.optimizer.min_island_size = min_island_size
  self.optimizer.max_island_size = max_island_size
}

///|
fn IslandManager::ensure_body_capacity(self : IslandManager, id : Int) -> Unit {
  if id < 0 {
    return
  }
  let mut i = self.body_island_ids.length()
  while i <= id {
    self.body_island_ids.push(-1)
    self.body_island_generations.push(0)
    self.body_island_indices.push(-1)
    self.body_state_generations.push(0)
    i = i + 1
  }
}

///|
fn IslandManager::reset_body_sleep_state(
  self : IslandManager,
  handle : RigidBodyHandle,
) -> Unit {
  if handle.id < 0 {
    return
  }
  self.ensure_body_capacity(handle.id)
  self.body_state_generations[handle.id] = handle.generation
}

///|
fn IslandManager::wake_up_body(
  self : IslandManager,
  bodies : RigidBodySet,
  handle : RigidBodyHandle,
  strong : Bool,
) -> Unit {
  if handle.id < 0 {
    return
  }
  self.sync_body_state_generation(handle)
  if bodies.get_mut(handle) is Some(body) {
    if body.is_fixed() {
      return
    }
    if body.activation.sleeping {
      body.changes = body.changes.insert(RIGID_BODY_CHANGES_SLEEP)
    }
    body.activation = body.activation.wake_up(strong)
  } else {
    return
  }
}

///|
fn IslandManager::sync_body_state_generation(
  self : IslandManager,
  handle : RigidBodyHandle,
) -> Unit {
  if handle.id < 0 {
    return
  }
  self.ensure_body_capacity(handle.id)
  if self.body_state_generations[handle.id] != handle.generation {
    self.reset_body_sleep_state(handle)
  }
}

///|
fn IslandManager::insert_island(self : IslandManager, island : Island) -> Int {
  if self.free_islands.pop() is Some(index) {
    self.islands[index] = Some(island)
    index
  } else {
    let index = self.islands.length()
    self.islands.push(Some(island))
    index
  }
}

///|
fn IslandManager::remove_from_awake_list(
  self : IslandManager,
  island_id : Int,
) -> Unit {
  let island = self.islands[island_id]
  if island is Some(value) {
    if value.id_in_awake_list is Some(awake_index) {
      let island_value = value
      let last_index = self.awake_islands.length() - 1
      if awake_index != last_index {
        let moved_id = self.awake_islands[last_index]
        self.awake_islands[awake_index] = moved_id
        if self.islands[moved_id] is Some(moved_island) {
          let updated_island = moved_island
          updated_island.id_in_awake_list = Some(awake_index)
          self.islands[moved_id] = Some(updated_island)
        }
      }
      self.awake_islands.pop() |> ignore
      island_value.id_in_awake_list = None
      self.islands[island_id] = Some(island_value)
    }
  }
}

///|
fn IslandManager::insert_body(
  self : IslandManager,
  bodies : RigidBodySet,
  handle : RigidBodyHandle,
  sleeping : Bool,
) -> Unit {
  self.reset_body_sleep_state(handle)
  let mut additional_solver_iterations = 0
  if bodies.get(handle) is Some(body) {
    additional_solver_iterations = body.additional_solver_iterations
  }
  let island = Island(handle, additional_solver_iterations)
  if !sleeping {
    let index = self.awake_islands.length()
    island.id_in_awake_list = Some(index)
  }
  let island_id = self.insert_island(island)
  if !sleeping {
    self.awake_islands.push(island_id)
  }
  self.ensure_body_capacity(handle.id)
  self.body_island_ids[handle.id] = island_id
  self.body_island_generations[handle.id] = handle.generation
  self.body_island_indices[handle.id] = 0
  if bodies.get_mut(handle) is Some(body) {
    body.ids.active_island_id = island_id
    body.ids.active_set_id = 0
    body.ids.active_set_timestamp = 0
  }
}

///|
fn IslandManager::remove_body(
  self : IslandManager,
  bodies : RigidBodySet,
  handle : RigidBodyHandle,
) -> Unit {
  if self.island_id_for(handle) is Some(island_id) {
    if self.islands[island_id] is Some(island) {
      self.ensure_body_capacity(handle.id)
      let mut body_index = self.body_island_indices[handle.id]
      if body_index < 0 ||
        body_index >= island.bodies.length() ||
        !handle_equals(island.bodies[body_index], handle) {
        body_index = -1
        for i in 0..= 0 &&
          moved_handle.id < self.body_island_indices.length() {
          self.body_island_ids[moved_handle.id] = island_id
          self.body_island_generations[moved_handle.id] = moved_handle.generation
          self.body_island_indices[moved_handle.id] = body_index
          if bodies.get_mut(moved_handle) is Some(body) {
            body.ids.active_island_id = island_id
            body.ids.active_set_id = body_index
          }
        }
      }
      island.bodies.pop() |> ignore
      if handle.id >= 0 && handle.id < self.body_island_ids.length() {
        self.body_island_ids[handle.id] = -1
        self.body_island_indices[handle.id] = -1
        self.body_island_generations[handle.id] = handle.generation
        self.reset_body_sleep_state(handle)
        if bodies.get_mut(handle) is Some(body) {
          body.ids.active_island_id = -1
          body.ids.active_set_id = -1
        }
      }
      if island.bodies.length() == 0 {
        self.remove_from_awake_list(island_id)
        self.islands[island_id] = None
        self.free_islands.push(island_id)
      } else {
        self.islands[island_id] = Some(island)
      }
    }
  }
}

///|
fn IslandManager::remove_body_with_ids(
  self : IslandManager,
  bodies : RigidBodySet,
  handle : RigidBodyHandle,
  ids : RigidBodyIds,
) -> Unit {
  let island_id = ids.active_island_id
  if island_id < 0 || island_id >= self.islands.length() {
    return
  }
  if self.islands[island_id] is Some(island) {
    self.ensure_body_capacity(handle.id)
    let mut body_index = ids.active_set_id
    if body_index < 0 ||
      body_index >= island.bodies.length() ||
      !handle_equals(island.bodies[body_index], handle) {
      body_index = -1
      for i in 0..= 0 &&
        moved_handle.id < self.body_island_indices.length() {
        self.body_island_ids[moved_handle.id] = island_id
        self.body_island_generations[moved_handle.id] = moved_handle.generation
        self.body_island_indices[moved_handle.id] = body_index
        if bodies.get_mut(moved_handle) is Some(body) {
          body.ids.active_island_id = island_id
          body.ids.active_set_id = body_index
        }
      }
    }
    island.bodies.pop() |> ignore
    if handle.id >= 0 && handle.id < self.body_island_ids.length() {
      self.body_island_ids[handle.id] = -1
      self.body_island_indices[handle.id] = -1
      self.body_island_generations[handle.id] = handle.generation
      self.reset_body_sleep_state(handle)
    }
    if island.bodies.length() == 0 {
      self.remove_from_awake_list(island_id)
      self.islands[island_id] = None
      self.free_islands.push(island_id)
    } else {
      self.islands[island_id] = Some(island)
    }
  }
}

///|
fn IslandManager::island_id_for(
  self : IslandManager,
  handle : RigidBodyHandle,
) -> Int? {
  if handle.id < 0 || handle.id >= self.body_island_ids.length() {
    return None
  }
  if self.body_island_generations[handle.id] != handle.generation {
    return None
  }
  let island_id = self.body_island_ids[handle.id]
  if island_id < 0 || island_id >= self.islands.length() {
    return None
  }
  if self.islands[island_id] is Some(_) {
    Some(island_id)
  } else {
    None
  }
}

///|
fn truncate_handles(values : Array[RigidBodyHandle], len : Int) -> Unit {
  while values.length() > len {
    values.pop() |> ignore
  }
}

///|
fn IslandManager::push_contacting_bodies(
  self : IslandManager,
  bodies : RigidBodySet,
  contacts : ContactGraph,
  handle : RigidBodyHandle,
) -> Unit {
  bodies |> ignore
  for i in 0.. Unit {
  bodies |> ignore
  let attached = impulse_joints.attached_enabled_joints(handle)
  for i in 0.. Array[RigidBodyHandle] {
  let result : Array[RigidBodyHandle] = []
  for i in 0..= 0 && island_id < self.islands.length() {
      if self.islands[island_id] is Some(island) {
        for j in 0.. Array[Int] {
  let result : Array[Int] = []
  for i in 0.. Array[RigidBodyHandle] {
  if island_id < 0 || island_id >= self.islands.length() {
    return []
  }
  if self.islands[island_id] is Some(island) {
    let result : Array[RigidBodyHandle] = []
    for i in 0.. Int {
  if island_id < 0 || island_id >= self.islands.length() {
    return 0
  }
  if self.islands[island_id] is Some(island) {
    island.additional_solver_iterations
  } else {
    0
  }
}

///|
fn IslandManager::wake_up_island(
  self : IslandManager,
  bodies : RigidBodySet,
  island_id : Int,
) -> Unit {
  if island_id < 0 || island_id >= self.islands.length() {
    return
  }
  if self.islands[island_id] is Some(island) {
    if island.id_in_awake_list is None {
      let index = self.awake_islands.length()
      island.id_in_awake_list = Some(index)
      self.awake_islands.push(island_id)
      for i in 0.. Unit {
  if bodies.get_mut(handle) is Some(body) {
    if body.is_fixed() {
      return
    }
    if body.activation.sleeping {
      body.changes = body.changes.insert(RIGID_BODY_CHANGES_SLEEP)
    }
    body.activation = body.activation.wake_up(strong)
    let island_id = body.ids.active_island_id
    self.wake_up_island(bodies, island_id)
  }
}

///|
fn IslandManager::merge_islands(
  self : IslandManager,
  bodies : RigidBodySet,
  island_id1 : Int,
  island_id2 : Int,
) -> Unit {
  if island_id1 == island_id2 {
    return
  }
  if island_id1 < 0 || island_id2 < 0 {
    return
  }
  if island_id1 >= self.islands.length() || island_id2 >= self.islands.length() {
    return
  }
  if self.islands[island_id1] is Some(island1) &&
    self.islands[island_id2] is Some(island2) {
    let sleeping1 = island1.id_in_awake_list is None
    let sleeping2 = island2.id_in_awake_list is None
    if sleeping1 != sleeping2 {
      return
    }
    let (keep_id, remove_id) = if island1.bodies.length() <
      island2.bodies.length() {
      (island_id2, island_id1)
    } else {
      (island_id1, island_id2)
    }
    let keep = if keep_id == island_id1 { island1 } else { island2 }
    let remove = if remove_id == island_id1 { island1 } else { island2 }
    let mut next_index = keep.bodies.length()
    for i in 0.. keep.additional_solver_iterations {
          keep.additional_solver_iterations = body.additional_solver_iterations
        }
        self.ensure_body_capacity(handle.id)
        self.body_island_ids[handle.id] = keep_id
        self.body_island_generations[handle.id] = handle.generation
        self.body_island_indices[handle.id] = next_index
        body.ids.active_island_id = keep_id
        body.ids.active_set_id = next_index
        next_index = next_index + 1
      }
    }
    if remove.id_in_awake_list is Some(_) {
      self.remove_from_awake_list(remove_id)
    }
    if sleeping1 {
      keep.id_in_awake_list = None
    } else if self.islands[keep_id] is Some(updated_keep) {
      keep.id_in_awake_list = updated_keep.id_in_awake_list
    }
    self.islands[remove_id] = None
    self.free_islands.push(remove_id)
    self.islands[keep_id] = Some(keep)
  }
}

///|
pub fn IslandManager::interaction_started_or_stopped(
  self : IslandManager,
  bodies : RigidBodySet,
  handle1 : RigidBodyHandle?,
  handle2 : RigidBodyHandle?,
  started : Bool,
  wake_up : Bool,
) -> Unit {
  if wake_up {
    if handle1 is Some(value1) {
      self.wake_up(bodies, value1, false)
    }
    if handle2 is Some(value2) {
      self.wake_up(bodies, value2, false)
    }
  }
  if !started {
    return
  }
  if handle1 is Some(value1) && handle2 is Some(value2) {
    if bodies.get(value1) is Some(body1) && bodies.get(value2) is Some(body2) {
      if body1.is_dynamic_or_kinematic() && body2.is_dynamic_or_kinematic() {
        if self.island_id_for(value1) is Some(id1) &&
          self.island_id_for(value2) is Some(id2) {
          if id1 != id2 {
            self.merge_islands(bodies, id1, id2)
          }
        }
      }
    }
  }
}

///|
pub fn IslandManager::rigid_body_updated(
  self : IslandManager,
  handle : RigidBodyHandle,
  bodies : RigidBodySet,
) -> Unit {
  if bodies.get_mut(handle) is Some(body) {
    self.sync_body_state_generation(handle)
    if body.is_fixed() {
      return
    }
    let island_id = body.ids.active_island_id
    let mut island_exists = false
    if island_id >= 0 && island_id < self.islands.length() {
      if self.islands[island_id] is Some(island) {
        island_exists = true
        if !body.is_sleeping() && island.id_in_awake_list is None {
          self.wake_up(bodies, handle, false)
        }
      }
    }
    if !island_exists {
      if body.is_sleeping() {
        self.insert_body(bodies, handle, true)
      } else {
        let mut inserted = false
        if self.awake_islands.length() > 0 {
          let last_id = self.awake_islands[self.awake_islands.length() - 1]
          if self.islands[last_id] is Some(island) {
            if island.bodies.length() < self.optimizer.min_island_size &&
              island.id_in_awake_list is Some(_) {
              let index = island.bodies.length()
              island.bodies.push(handle)
              self.islands[last_id] = Some(island)
              self.ensure_body_capacity(handle.id)
              self.body_island_ids[handle.id] = last_id
              self.body_island_generations[handle.id] = handle.generation
              self.body_island_indices[handle.id] = index
              body.ids.active_island_id = last_id
              body.ids.active_set_id = index
              body.ids.active_set_timestamp = 0
              inserted = true
            }
          }
        }
        if !inserted {
          self.insert_body(bodies, handle, false)
        }
      }
    }
    if (
        body.changes.contains(RIGID_BODY_CHANGES_SLEEP) ||
        body.changes.contains(RIGID_BODY_CHANGES_TYPE)
      ) &&
      body.is_enabled() &&
      !body.activation.sleeping {
      self.wake_up(bodies, handle, false)
    }
  }
}

///|
pub fn IslandManager::rigid_body_removed_or_disabled(
  self : IslandManager,
  handle : RigidBodyHandle,
  bodies : RigidBodySet,
) -> Unit {
  if bodies.get_mut(handle) is Some(body) {
    let ids = body.ids
    body.ids.active_island_id = -1
    body.ids.active_set_id = -1
    body.ids.active_set_timestamp = 0
    self.remove_body_with_ids(bodies, handle, ids)
  } else {
    self.remove_body(bodies, handle)
  }
}

///|
fn IslandManager::extract_sub_island(
  self : IslandManager,
  bodies : RigidBodySet,
  island_id : Int,
  extracted : Array[RigidBodyHandle],
  sleeping : Bool,
) -> Unit {
  if island_id < 0 || island_id >= self.islands.length() {
    return
  }
  if extracted.length() == 0 {
    return
  }
  if self.islands[island_id] is Some(island) {
    let new_island_id = self.insert_island({
      bodies: [],
      id_in_awake_list: None,
      additional_solver_iterations: 0,
    })
    let new_island_bodies : Array[RigidBodyHandle] = []
    let mut additional_solver_iterations = 0
    for i in 0..= 0 && handle.id < self.body_island_ids.length() {
        if self.body_island_generations[handle.id] == handle.generation &&
          self.body_island_ids[handle.id] == island_id {
          body_index = self.body_island_indices[handle.id]
        }
      }
      if body_index < 0 ||
        body_index >= island.bodies.length() ||
        !handle_equals(island.bodies[body_index], handle) {
        body_index = -1
        for j in 0..= 0 &&
          moved_handle.id < self.body_island_indices.length() {
          self.body_island_ids[moved_handle.id] = island_id
          self.body_island_generations[moved_handle.id] = moved_handle.generation
          self.body_island_indices[moved_handle.id] = body_index
        }
      }
      island.bodies.pop() |> ignore
      let new_index = new_island_bodies.length()
      new_island_bodies.push(handle)
      self.ensure_body_capacity(handle.id)
      self.body_island_ids[handle.id] = new_island_id
      self.body_island_generations[handle.id] = handle.generation
      self.body_island_indices[handle.id] = new_index
      if bodies.get_mut(handle) is Some(body) {
        body.ids.active_island_id = new_island_id
        body.ids.active_set_id = new_index
        if sleeping {
          body.activation = body.activation.sleep()
          body.vels = RigidBodyVelocity::zero()
        }
        if body.additional_solver_iterations > additional_solver_iterations {
          additional_solver_iterations = body.additional_solver_iterations
        }
      }
    }
    if island.bodies.length() == 0 {
      self.remove_from_awake_list(island_id)
      self.islands[island_id] = None
      self.free_islands.push(island_id)
    } else {
      self.islands[island_id] = Some(island)
    }
    let created_island = Island::{
      bodies: new_island_bodies,
      id_in_awake_list: None,
      additional_solver_iterations,
    }
    if !sleeping {
      let index = self.awake_islands.length()
      created_island.id_in_awake_list = Some(index)
      self.awake_islands.push(new_island_id)
    }
    self.islands[new_island_id] = Some(created_island)
  }
}

///|
fn IslandManager::extract_sleeping_island(
  self : IslandManager,
  bodies : RigidBodySet,
  contacts : ContactGraph,
  impulse_joints : ImpulseJointSet,
  multibody_joints : MultibodyJointSet,
  sleep_root : RigidBodyHandle,
) -> Int {
  if sleep_root.id < 0 {
    return 0
  }
  if bodies.get(sleep_root) is None {
    return 0
  }
  if bodies.get_mut(sleep_root) is Some(root_body) {
    if !(root_body.activation.sleep_root_state is TraversalPending) {
      return 0
    }
    root_body.activation.sleep_root_state = Traversed
  } else {
    return 0
  }
  if self.island_id_for(sleep_root) is Some(island_id) {
    if self.islands[island_id] is Some(active_island) {
      if active_island.id_in_awake_list is None {
        return 0
      }
      let new_island_bodies : Array[RigidBodyHandle] = []
      self.stack.clear()
      self.stack.push(sleep_root)
      self.traversal_timestamp = self.traversal_timestamp + 1
      let mut visited_count = 0
      while self.stack.pop() is Some(handle) {
        if bodies.get_mut(handle) is Some(body) {
          if body.is_fixed() {
            continue
          }
          if body.ids.active_set_timestamp == self.traversal_timestamp {
            continue
          }
          body.ids.active_set_timestamp = self.traversal_timestamp
          visited_count = visited_count + 1
          if body.activation.is_eligible_for_sleep() {
            body.activation.sleep_root_state = Traversed
          } else {
            self.stack.clear()
            return visited_count
          }
          self.push_contacting_bodies(bodies, contacts, handle)
          self.push_linked_bodies(
            bodies, impulse_joints, multibody_joints, handle,
          )
          new_island_bodies.push(handle)
        }
      }
      if active_island.bodies.length() == new_island_bodies.length() {
        for i in 0.. Unit {
  if self.optimizer.mode % 2 == 0 {
    self.incremental_merge(bodies)
  } else {
    self.incremental_split(bodies, contacts, impulse_joints, multibody_joints)
  }
  self.optimizer.mode = self.optimizer.mode + 1
}

///|
fn IslandManager::incremental_merge(
  self : IslandManager,
  bodies : RigidBodySet,
) -> Unit {
  if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.length() {
    self.optimizer.merge_state.curr_awake_id = 0
  }
  let mut first_awake_id = -1
  let mut first_island_id = -1
  let mut first_solver_iterations = 0
  for
    awake_id in self.optimizer.merge_state.curr_awake_id.. Unit {
  if self.optimizer.split_state.curr_awake_id >= self.awake_islands.length() {
    self.optimizer.split_state.curr_awake_id = 0
  }
  for
    awake_id in self.optimizer.split_state.curr_awake_id.. self.optimizer.max_island_size {
        self.stack.clear()
        let new_island_bodies : Array[RigidBodyHandle] = []
        self.traversal_timestamp = self.traversal_timestamp + 1
        for root_index in 0.. self.optimizer.max_island_size {
                truncate_handles(new_island_bodies, len_before)
                self.stack.clear()
                break
              }
            }
          }
          if new_island_bodies.length() == 0 {
            return
          } else if new_island_bodies.length() >= self.optimizer.min_island_size {
            self.extract_sub_island(bodies, island_id, new_island_bodies, false)
            return
          }
        }
      }
      self.optimizer.split_state.curr_awake_id = awake_id + 1
    }
  }
}

///|
fn IslandManager::pop_traversal_candidate(
  self : IslandManager,
) -> SleepCandidate? {
  if self.traversal_candidates.length() == 0 {
    return None
  }
  let candidate = self.traversal_candidates[0]
  let mut i = 1
  while i < self.traversal_candidates.length() {
    self.traversal_candidates[i - 1] = self.traversal_candidates[i]
    i = i + 1
  }
  self.traversal_candidates.pop() |> ignore
  Some(candidate)
}

///|
pub fn IslandManager::update_islands(
  self : IslandManager,
  dt : @core.Real,
  length_unit : @core.Real,
  bodies : RigidBodySet,
  contacts : ContactGraph,
  impulse_joints : ImpulseJointSet,
  multibody_joints : MultibodyJointSet,
) -> Unit {
  for handle in self.active_bodies() {
    if handle.id < 0 {
      continue
    }
    if bodies.get_mut(handle) is Some(body) {
      self.sync_body_state_generation(handle)
      let linvel = body.vels.linvel
      let sq_linvel = linvel.x * linvel.x + linvel.y * linvel.y
      let angvel = body.vels.angvel
      let sq_angvel = angvel * angvel
      body.activation = body.activation.update_energy(
        body.body_type,
        length_unit,
        sq_linvel,
        sq_angvel,
        dt,
      )
      let eligible = body.activation.is_eligible_for_sleep()
      if eligible {
        if body.activation.sleep_root_state is Unknown {
          self.traversal_candidates.push({ handle, })
          body.activation.sleep_root_state = TraversalPending
        }
      } else {
        body.activation.sleep_root_state = Unknown
      }
    }
  }
  let mut cost = 0
  while true {
    if self.pop_traversal_candidate() is Some(candidate) {
      cost = cost +
        self.extract_sleeping_island(
          bodies,
          contacts,
          impulse_joints,
          multibody_joints,
          candidate.handle,
        )
      if cost > MAX_SLEEP_TRAVERSAL_COST {
        break
      }
    } else {
      break
    }
  }
  self.update_optimizer(bodies, contacts, impulse_joints, multibody_joints)
}

///|
pub struct ImpulseJoint {
  body1 : RigidBodyHandle
  body2 : RigidBodyHandle
  mut data : GenericJoint
  mut impulses : @core.Vec2
  mut ang_impulse : @core.Real
  handle : ImpulseJointHandle
}

///|
pub fn ImpulseJoint::is_enabled(self : ImpulseJoint) -> Bool {
  self.data.is_enabled()
}

///|
pub fn ImpulseJoint::set_enabled(
  self : ImpulseJoint,
  enabled : Bool,
) -> ImpulseJoint {
  self.data = self.data.set_enabled(enabled)
  self
}

///|
pub fn ImpulseJoint::set_enabled_by_attached_body(
  self : ImpulseJoint,
  attached_enabled : Bool,
) -> ImpulseJoint {
  self.data = self.data.set_enabled_by_attached_body(attached_enabled)
  self
}

///|
pub struct ImpulseJointGraph {
  nodes : Array[RigidBodyHandle]
  edges : Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJoint)]
}

///|
pub fn ImpulseJointGraph::nodes(
  self : ImpulseJointGraph,
) -> Array[RigidBodyHandle] {
  let result : Array[RigidBodyHandle] = []
  for i in 0.. Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJoint)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJoint)] = []
  for i in 0.. ImpulseJointHandle {
  { id, generation }
}

///|
pub fn ImpulseJointHandle::from_raw_parts(
  id : Int,
  generation : Int,
) -> ImpulseJointHandle {
  ImpulseJointHandle(id, generation)
}

///|
pub fn ImpulseJointHandle::into_raw_parts(
  self : ImpulseJointHandle,
) -> (Int, Int) {
  (self.id, self.generation)
}

///|
pub fn ImpulseJointHandle::invalid() -> ImpulseJointHandle {
  ImpulseJointHandle(-1, -1)
}

///|
pub struct GenericJoint {
  mut local_frame1 : @core.Isometry2
  mut local_frame2 : @core.Isometry2
  mut axis3 : @core.Vec3
  mut local_anchor1_3 : @core.Vec3
  mut local_anchor2_3 : @core.Vec3
  mut local_frame1_rotation : @core.Quat
  mut local_frame2_rotation : @core.Quat
  mut locked_axes : JointAxesMask
  mut limit_axes : JointAxesMask
  mut motor_axes : JointAxesMask
  mut coupled_axes : JointAxesMask
  mut limits : JointLimits3
  mut motors : JointMotors3
  mut softness : SpringCoefficients
  mut contacts_enabled : Bool
  mut enabled : JointEnabled
  user_data : Int
}

///|
pub fn GenericJoint::default() -> GenericJoint {
  {
    local_frame1: @core.Isometry2::identity(),
    local_frame2: @core.Isometry2::identity(),
    axis3: Vec3(0.0F, 0.0F, 1.0F),
    local_anchor1_3: @core.Vec3::zero(),
    local_anchor2_3: @core.Vec3::zero(),
    local_frame1_rotation: @core.Quat::identity(),
    local_frame2_rotation: @core.Quat::identity(),
    locked_axes: JointAxesMask::empty(),
    limit_axes: JointAxesMask::empty(),
    motor_axes: JointAxesMask::empty(),
    coupled_axes: JointAxesMask::empty(),
    limits: JointLimits3::default(),
    motors: JointMotors3::default(),
    softness: SpringCoefficients::joint_defaults(),
    contacts_enabled: true,
    enabled: Enabled,
    user_data: 0,
  }
}

///|
pub fn GenericJoint::GenericJoint(locked_axes : JointAxesMask) -> GenericJoint {
  let j = GenericJoint::default()
  j.locked_axes = locked_axes
  j
}

///|
pub fn GenericJoint::lock_axes(
  self : GenericJoint,
  axes : JointAxesMask,
) -> GenericJoint {
  self.locked_axes = self.locked_axes.or(axes)
  self
}

///|
pub fn GenericJoint::as_revolute(self : GenericJoint) -> GenericJoint? {
  let bits = self.locked_axes.bits()
  if bits == JointAxesMask::locked_revolute_axes().bits() ||
    bits == JointAxesMask::locked_revolute_axes3().bits() {
    Some(self)
  } else {
    None
  }
}

///|
pub fn GenericJoint::as_revolute_mut(self : GenericJoint) -> GenericJoint? {
  self.as_revolute()
}

///|
pub fn GenericJoint::as_fixed(self : GenericJoint) -> GenericJoint? {
  let bits = self.locked_axes.bits()
  if bits == JointAxesMask::locked_fixed_axes().bits() ||
    bits == JointAxesMask::locked_fixed_axes3().bits() {
    Some(self)
  } else {
    None
  }
}

///|
pub fn GenericJoint::as_fixed_mut(self : GenericJoint) -> GenericJoint? {
  self.as_fixed()
}

///|
pub fn GenericJoint::as_prismatic(self : GenericJoint) -> GenericJoint? {
  let bits = self.locked_axes.bits()
  if bits == JointAxesMask::locked_prismatic_axes().bits() ||
    bits == JointAxesMask::locked_prismatic_axes3().bits() {
    Some(self)
  } else {
    None
  }
}

///|
pub fn GenericJoint::as_prismatic_mut(self : GenericJoint) -> GenericJoint? {
  self.as_prismatic()
}

///|
pub fn GenericJoint::as_rope(self : GenericJoint) -> GenericJoint? {
  if self.locked_axes.bits() == 0 {
    Some(self)
  } else {
    None
  }
}

///|
pub fn GenericJoint::as_rope_mut(self : GenericJoint) -> GenericJoint? {
  self.as_rope()
}

///|
pub fn GenericJoint::as_spherical(self : GenericJoint) -> GenericJoint? {
  if self.locked_axes.bits() == JointAxesMask::locked_spherical_axes().bits() {
    Some(self)
  } else {
    None
  }
}

///|
pub fn GenericJoint::as_spherical_mut(self : GenericJoint) -> GenericJoint? {
  self.as_spherical()
}

///|
pub fn GenericJoint::from_revolute(joint : RevoluteJoint) -> GenericJoint {
  let anchor1 = joint.local_anchor1()
  let anchor2 = joint.local_anchor2()
  let frame1 = @core.Isometry2::from_translation(anchor1)
  let frame2 = @core.Isometry2::from_translation(anchor2)
  let motor_axis = if joint.is_spatial { JointAxis::AngX } else { AngZ }
  let result = GenericJoint::{
    local_frame1: frame1,
    local_frame2: frame2,
    axis3: joint.axis3,
    local_anchor1_3: joint.local_anchor1_3,
    local_anchor2_3: joint.local_anchor2_3,
    local_frame1_rotation: joint.local_frame1_rotation,
    local_frame2_rotation: joint.local_frame2_rotation,
    locked_axes: if joint.is_spatial {
      JointAxesMask::locked_revolute_axes3()
    } else {
      JointAxesMask::locked_revolute_axes()
    },
    limit_axes: JointAxesMask::empty(),
    motor_axes: JointAxesMask::empty(),
    coupled_axes: JointAxesMask::empty(),
    limits: JointLimits3::default(),
    motors: JointMotors3::default(),
    softness: joint.softness(),
    contacts_enabled: true,
    enabled: Enabled,
    user_data: 0,
  }
  if joint.motor() is Some(motor) {
    result.motor_axes = joint_axis_mask(motor_axis)
    result.motors = joint_motors_set_axis(result.motors, motor_axis, motor)
  }
  if joint.limits() is Some(limits) {
    result.limit_axes = joint_axis_mask(motor_axis)
    result.limits = joint_limits_set_axis(result.limits, motor_axis, limits)
  }
  result
}

///|
pub fn GenericJoint::is_enabled(self : GenericJoint) -> Bool {
  self.enabled is Enabled
}

///|
pub fn GenericJoint::local_frame1(self : GenericJoint) -> @core.Isometry2 {
  self.local_frame1
}

///|
pub fn GenericJoint::local_frame2(self : GenericJoint) -> @core.Isometry2 {
  self.local_frame2
}

///|
pub fn GenericJoint::local_frame1_3(self : GenericJoint) -> @core.Isometry3 {
  Isometry3(self.local_anchor1_3, self.local_frame1_rotation)
}

///|
pub fn GenericJoint::local_frame2_3(self : GenericJoint) -> @core.Isometry3 {
  Isometry3(self.local_anchor2_3, self.local_frame2_rotation)
}

///|
pub fn GenericJoint::set_local_frame1(
  self : GenericJoint,
  frame : @core.Isometry2,
) -> GenericJoint {
  self.local_frame1 = frame
  self.local_anchor1_3 = Vec3(
    frame.translation.x,
    frame.translation.y,
    self.local_anchor1_3.z,
  )
  self.local_frame1_rotation = @core.rotation_from_scaled_axis(
    Vec3(0.0F, 0.0F, frame.rotation.angle()),
  )
  self
}

///|
pub fn GenericJoint::set_local_frame1_3(
  self : GenericJoint,
  frame : @core.Isometry3,
) -> GenericJoint {
  self.local_anchor1_3 = frame.translation
  self.local_frame1_rotation = frame.rotation
  self.axis3 = frame.rotation.rotate_vec3(Vec3(1.0F, 0.0F, 0.0F))
  self.local_frame1 = Isometry2(
    Vec2(frame.translation.x, frame.translation.y),
    @core.Rot2::from_angle(frame.rotation.to_scaled_axis().z),
  )
  self
}

///|
pub fn GenericJoint::set_local_frame2(
  self : GenericJoint,
  frame : @core.Isometry2,
) -> GenericJoint {
  self.local_frame2 = frame
  self.local_anchor2_3 = Vec3(
    frame.translation.x,
    frame.translation.y,
    self.local_anchor2_3.z,
  )
  self.local_frame2_rotation = @core.rotation_from_scaled_axis(
    Vec3(0.0F, 0.0F, frame.rotation.angle()),
  )
  self
}

///|
pub fn GenericJoint::set_local_frame2_3(
  self : GenericJoint,
  frame : @core.Isometry3,
) -> GenericJoint {
  self.local_anchor2_3 = frame.translation
  self.local_frame2_rotation = frame.rotation
  self.local_frame2 = Isometry2(
    Vec2(frame.translation.x, frame.translation.y),
    @core.Rot2::from_angle(frame.rotation.to_scaled_axis().z),
  )
  self
}

///|
pub fn GenericJoint::local_anchor1(self : GenericJoint) -> @core.Vec2 {
  self.local_frame1.translation
}

///|
pub fn GenericJoint::local_anchor2(self : GenericJoint) -> @core.Vec2 {
  self.local_frame2.translation
}

///|
pub fn GenericJoint::local_anchor1_3(self : GenericJoint) -> @core.Vec3 {
  self.local_anchor1_3
}

///|
pub fn GenericJoint::local_anchor2_3(self : GenericJoint) -> @core.Vec3 {
  self.local_anchor2_3
}

///|
pub fn GenericJoint::set_local_anchor1(
  self : GenericJoint,
  anchor : @core.Vec2,
) -> GenericJoint {
  self.local_frame1 = Isometry2(anchor, self.local_frame1.rotation)
  self.local_anchor1_3 = Vec3(anchor.x, anchor.y, self.local_anchor1_3.z)
  self
}

///|
pub fn GenericJoint::set_local_anchor1_3(
  self : GenericJoint,
  anchor : @core.Vec3,
) -> GenericJoint {
  self.local_anchor1_3 = anchor
  self.local_frame1 = Isometry2(
    Vec2(anchor.x, anchor.y),
    self.local_frame1.rotation,
  )
  self
}

///|
pub fn GenericJoint::set_local_anchor2(
  self : GenericJoint,
  anchor : @core.Vec2,
) -> GenericJoint {
  self.local_frame2 = Isometry2(anchor, self.local_frame2.rotation)
  self.local_anchor2_3 = Vec3(anchor.x, anchor.y, self.local_anchor2_3.z)
  self
}

///|
pub fn GenericJoint::set_local_anchor2_3(
  self : GenericJoint,
  anchor : @core.Vec3,
) -> GenericJoint {
  self.local_anchor2_3 = anchor
  self.local_frame2 = Isometry2(
    Vec2(anchor.x, anchor.y),
    self.local_frame2.rotation,
  )
  self
}

///|
pub fn GenericJoint::local_frame1_rotation(self : GenericJoint) -> @core.Quat {
  self.local_frame1_rotation
}

///|
pub fn GenericJoint::local_frame2_rotation(self : GenericJoint) -> @core.Quat {
  self.local_frame2_rotation
}

///|
pub fn GenericJoint::set_local_frame1_rotation(
  self : GenericJoint,
  rotation : @core.Quat,
) -> GenericJoint {
  self.local_frame1_rotation = rotation
  self.axis3 = rotation.rotate_vec3(Vec3(1.0F, 0.0F, 0.0F))
  self.local_frame1 = Isometry2(
    Vec2(self.local_anchor1_3.x, self.local_anchor1_3.y),
    @core.Rot2::from_angle(rotation.to_scaled_axis().z),
  )
  self
}

///|
pub fn GenericJoint::set_local_frame2_rotation(
  self : GenericJoint,
  rotation : @core.Quat,
) -> GenericJoint {
  self.local_frame2_rotation = rotation
  self.local_frame2 = Isometry2(
    Vec2(self.local_anchor2_3.x, self.local_anchor2_3.y),
    @core.Rot2::from_angle(rotation.to_scaled_axis().z),
  )
  self
}

///|
pub fn GenericJoint::locked_axes(self : GenericJoint) -> JointAxesMask {
  self.locked_axes
}

///|
pub fn GenericJoint::limit_axes(self : GenericJoint) -> JointAxesMask {
  self.limit_axes
}

///|
pub fn GenericJoint::motor_axes(self : GenericJoint) -> JointAxesMask {
  self.motor_axes
}

///|
pub fn GenericJoint::coupled_axes(self : GenericJoint) -> JointAxesMask {
  self.coupled_axes
}

///|
pub fn GenericJoint::set_locked_axes(
  self : GenericJoint,
  axes : JointAxesMask,
) -> GenericJoint {
  self.locked_axes = axes
  self
}

///|
pub fn GenericJoint::set_limit_axes(
  self : GenericJoint,
  axes : JointAxesMask,
) -> GenericJoint {
  self.limit_axes = axes
  self
}

///|
pub fn GenericJoint::set_motor_axes(
  self : GenericJoint,
  axes : JointAxesMask,
) -> GenericJoint {
  self.motor_axes = axes
  self
}

///|
pub fn GenericJoint::set_coupled_axes(
  self : GenericJoint,
  axes : JointAxesMask,
) -> GenericJoint {
  self.coupled_axes = axes
  self
}

///|
fn joint_limits_for_axis(
  limits : JointLimits3,
  axis : JointAxis,
) -> JointLimits {
  match axis {
    LinX => limits.lin_x
    LinY => limits.lin_y
    LinZ => limits.lin_z
    AngX => limits.ang_x
    AngY => limits.ang_y
    AngZ => limits.ang_z
  }
}

///|
fn joint_limits_set_axis(
  limits : JointLimits3,
  axis : JointAxis,
  value : JointLimits,
) -> JointLimits3 {
  match axis {
    LinX =>
      {
        lin_x: value,
        lin_y: limits.lin_y,
        lin_z: limits.lin_z,
        ang_x: limits.ang_x,
        ang_y: limits.ang_y,
        ang_z: limits.ang_z,
      }
    LinY =>
      {
        lin_x: limits.lin_x,
        lin_y: value,
        lin_z: limits.lin_z,
        ang_x: limits.ang_x,
        ang_y: limits.ang_y,
        ang_z: limits.ang_z,
      }
    LinZ =>
      {
        lin_x: limits.lin_x,
        lin_y: limits.lin_y,
        lin_z: value,
        ang_x: limits.ang_x,
        ang_y: limits.ang_y,
        ang_z: limits.ang_z,
      }
    AngX =>
      {
        lin_x: limits.lin_x,
        lin_y: limits.lin_y,
        lin_z: limits.lin_z,
        ang_x: value,
        ang_y: limits.ang_y,
        ang_z: limits.ang_z,
      }
    AngY =>
      {
        lin_x: limits.lin_x,
        lin_y: limits.lin_y,
        lin_z: limits.lin_z,
        ang_x: limits.ang_x,
        ang_y: value,
        ang_z: limits.ang_z,
      }
    AngZ =>
      {
        lin_x: limits.lin_x,
        lin_y: limits.lin_y,
        lin_z: limits.lin_z,
        ang_x: limits.ang_x,
        ang_y: limits.ang_y,
        ang_z: value,
      }
  }
}

///|
fn joint_motor_for_axis(motors : JointMotors3, axis : JointAxis) -> JointMotor {
  match axis {
    LinX => motors.lin_x
    LinY => motors.lin_y
    LinZ => motors.lin_z
    AngX => motors.ang_x
    AngY => motors.ang_y
    AngZ => motors.ang_z
  }
}

///|
fn joint_motors_set_axis(
  motors : JointMotors3,
  axis : JointAxis,
  value : JointMotor,
) -> JointMotors3 {
  match axis {
    LinX =>
      {
        lin_x: value,
        lin_y: motors.lin_y,
        lin_z: motors.lin_z,
        ang_x: motors.ang_x,
        ang_y: motors.ang_y,
        ang_z: motors.ang_z,
      }
    LinY =>
      {
        lin_x: motors.lin_x,
        lin_y: value,
        lin_z: motors.lin_z,
        ang_x: motors.ang_x,
        ang_y: motors.ang_y,
        ang_z: motors.ang_z,
      }
    LinZ =>
      {
        lin_x: motors.lin_x,
        lin_y: motors.lin_y,
        lin_z: value,
        ang_x: motors.ang_x,
        ang_y: motors.ang_y,
        ang_z: motors.ang_z,
      }
    AngX =>
      {
        lin_x: motors.lin_x,
        lin_y: motors.lin_y,
        lin_z: motors.lin_z,
        ang_x: value,
        ang_y: motors.ang_y,
        ang_z: motors.ang_z,
      }
    AngY =>
      {
        lin_x: motors.lin_x,
        lin_y: motors.lin_y,
        lin_z: motors.lin_z,
        ang_x: motors.ang_x,
        ang_y: value,
        ang_z: motors.ang_z,
      }
    AngZ =>
      {
        lin_x: motors.lin_x,
        lin_y: motors.lin_y,
        lin_z: motors.lin_z,
        ang_x: motors.ang_x,
        ang_y: motors.ang_y,
        ang_z: value,
      }
  }
}

///|
/// Flips the orientation of the joint, including limits and motors.
///
/// Ported from rapier-reference `GenericJoint::flip`.
pub fn GenericJoint::flip(self : GenericJoint) -> GenericJoint {
  let tmp = self.local_frame1
  self.local_frame1 = self.local_frame2
  self.local_frame2 = tmp
  let axis3 = self.axis3
  self.axis3 = axis3
  let anchor1_3 = self.local_anchor1_3
  self.local_anchor1_3 = self.local_anchor2_3
  self.local_anchor2_3 = anchor1_3
  let frame1_3 = self.local_frame1_rotation
  self.local_frame1_rotation = self.local_frame2_rotation
  self.local_frame2_rotation = frame1_3
  let coupled_bits = self.coupled_axes.bits()
  let axes : Array[JointAxis] = [LinX, LinY, LinZ, AngX, AngY, AngZ]
  for index in 0.. JointLimits3 {
  self.limits
}

///|
pub fn GenericJoint::motors(self : GenericJoint) -> JointMotors3 {
  self.motors
}

///|
pub fn GenericJoint::set_limits(
  self : GenericJoint,
  limits : JointLimits3,
) -> GenericJoint {
  self.limits = limits
  self
}

///|
pub fn GenericJoint::set_motors(
  self : GenericJoint,
  motors : JointMotors3,
) -> GenericJoint {
  self.motors = motors
  self
}

///|
fn joint_axis_mask(axis : JointAxis) -> JointAxesMask {
  match axis {
    LinX => JointAxesMask::lin_x()
    LinY => JointAxesMask::lin_y()
    LinZ => JointAxesMask::lin_z()
    AngX => JointAxesMask::ang_x()
    AngY => JointAxesMask::ang_y()
    AngZ => JointAxesMask::ang_z()
  }
}

///|
pub fn GenericJoint::set_motor(
  self : GenericJoint,
  axis : JointAxis,
  target_pos : @core.Real,
  target_vel : @core.Real,
  stiffness : @core.Real,
  damping : @core.Real,
) -> GenericJoint {
  self.motor_axes = self.motor_axes.or(joint_axis_mask(axis))
  let motor = joint_motor_for_axis(self.motors, axis)
    .set_target_vel(target_vel)
    .set_target_pos(target_pos)
    .set_stiffness(stiffness)
    .set_damping(damping)
  self.motors = joint_motors_set_axis(self.motors, axis, motor)
  self
}

///|
pub fn GenericJoint::set_motor_velocity(
  self : GenericJoint,
  axis : JointAxis,
  target_vel : @core.Real,
  factor : @core.Real,
) -> GenericJoint {
  let current_pos = joint_motor_for_axis(self.motors, axis).target_pos
  self.set_motor(axis, current_pos, target_vel, 0.0F, factor)
}

///|
pub fn GenericJoint::set_motor_position(
  self : GenericJoint,
  axis : JointAxis,
  target_pos : @core.Real,
  stiffness : @core.Real,
  damping : @core.Real,
) -> GenericJoint {
  self.set_motor(axis, target_pos, 0.0F, stiffness, damping)
}

///|
pub fn GenericJoint::set_motor_max_force(
  self : GenericJoint,
  axis : JointAxis,
  max_force : @core.Real,
) -> GenericJoint {
  self.motors = joint_motors_set_axis(
    self.motors,
    axis,
    joint_motor_for_axis(self.motors, axis).set_max_force(max_force),
  )
  self
}

///|
pub fn GenericJoint::set_motor_model(
  self : GenericJoint,
  axis : JointAxis,
  model : MotorModel,
) -> GenericJoint {
  self.motors = joint_motors_set_axis(
    self.motors,
    axis,
    joint_motor_for_axis(self.motors, axis).set_model(model),
  )
  self
}

///|
pub fn GenericJoint::motor(
  self : GenericJoint,
  axis : JointAxis,
) -> JointMotor? {
  let mask = joint_axis_mask(axis)
  if !self.motor_axes.contains(mask) {
    return None
  }
  Some(joint_motor_for_axis(self.motors, axis))
}

///|
pub fn GenericJoint::motor_model(
  self : GenericJoint,
  axis : JointAxis,
) -> MotorModel? {
  if self.motor(axis) is Some(motor) {
    Some(motor.model)
  } else {
    None
  }
}

///|
pub fn GenericJoint::limits_axis(
  self : GenericJoint,
  axis : JointAxis,
) -> JointLimits? {
  let mask = joint_axis_mask(axis)
  if !self.limit_axes.contains(mask) {
    return None
  }
  Some(joint_limits_for_axis(self.limits, axis))
}

///|
pub fn GenericJoint::set_limits_axis(
  self : GenericJoint,
  axis : JointAxis,
  min : @core.Real,
  max : @core.Real,
) -> GenericJoint {
  self.limit_axes = self.limit_axes.or(joint_axis_mask(axis))
  self.limits = joint_limits_set_axis(
    self.limits,
    axis,
    joint_limits_for_axis(self.limits, axis).set_min(min).set_max(max),
  )
  self
}

///|
pub fn GenericJoint::softness(self : GenericJoint) -> SpringCoefficients {
  self.softness
}

///|
pub fn GenericJoint::set_softness(
  self : GenericJoint,
  softness : SpringCoefficients,
) -> GenericJoint {
  self.softness = softness
  self
}

///|
pub fn GenericJoint::contacts_enabled(self : GenericJoint) -> Bool {
  self.contacts_enabled
}

///|
pub fn GenericJoint::set_contacts_enabled(
  self : GenericJoint,
  enabled : Bool,
) -> GenericJoint {
  self.contacts_enabled = enabled
  self
}

///|
pub fn GenericJoint::set_enabled(
  self : GenericJoint,
  enabled : Bool,
) -> GenericJoint {
  match self.enabled {
    Enabled | DisabledByAttachedBody => if !enabled { self.enabled = Disabled }
    Disabled => if enabled { self.enabled = Enabled }
  }
  self
}

///|
pub fn GenericJoint::set_enabled_by_attached_body(
  self : GenericJoint,
  attached_enabled : Bool,
) -> GenericJoint {
  if attached_enabled {
    if self.enabled is DisabledByAttachedBody {
      self.enabled = Enabled
    }
  } else if self.enabled is Enabled {
    self.enabled = DisabledByAttachedBody
  }
  self
}

///|
fn handle_equals(left : RigidBodyHandle, right : RigidBodyHandle) -> Bool {
  left.id == right.id && left.generation == right.generation
}

///|
fn push_unique_handle(
  values : Array[RigidBodyHandle],
  handle : RigidBodyHandle,
) -> Unit {
  for i in 0.. Bool {
  handle_equals(left.0, right.0) && handle_equals(left.1, right.1)
}

///|
fn push_unique_pair(
  values : Array[(RigidBodyHandle, RigidBodyHandle)],
  pair : (RigidBodyHandle, RigidBodyHandle),
) -> Unit {
  for i in 0.. Unit {
  let mut index = -1
  for i in 0.. ignore
}

///|
fn replace_edge_index(
  values : Array[Int],
  old_index : Int,
  new_index : Int,
) -> Unit {
  for i in 0.. ImpulseJointSet {
  {
    rb_graph_ids: [],
    rb_graph_generations: [],
    graph_nodes: [],
    graph_edges: [],
    adjacency: [],
    joint_generations: [],
    joint_edge_ids: [],
    free_handles: [],
    to_wake_up: [],
    to_join: [],
  }
}

///|
fn ij_split_values(text : String, sep : String) -> Array[String] {
  let values : Array[String] = []
  for part in text.split(sep[:]) {
    let value = part.to_owned()
    if value.length() > 0 {
      values.push(value)
    }
  }
  values
}

///|
fn ij_parse_int_value(text : String) -> Int {
  let result = try @string.parse_int(text[:], base=10) catch {
    err => Err(err)
  } noraise {
    value => Ok(value)
  }
  if result is Ok(value) {
    value
  } else {
    0
  }
}

///|
fn ij_parse_real_value(text : String) -> @core.Real {
  let result = try @string.parse_double(text[:]) catch {
    err => Err(err)
  } noraise {
    value => Ok(value)
  }
  if result is Ok(value) {
    Float::from_double(value)
  } else {
    0.0F
  }
}

///|
fn ij_parse_bool_value(text : String) -> Bool {
  text == "1"
}

///|
fn motor_model_to_int(model : MotorModel) -> Int {
  match model {
    AccelerationBased => 0
    ForceBased => 1
  }
}

///|
fn motor_model_from_int(value : Int) -> MotorModel {
  if value == 1 {
    ForceBased
  } else {
    AccelerationBased
  }
}

///|
fn joint_enabled_to_int(value : JointEnabled) -> Int {
  match value {
    Enabled => 0
    Disabled => 1
    DisabledByAttachedBody => 2
  }
}

///|
fn joint_enabled_from_int(value : Int) -> JointEnabled {
  if value == 1 {
    Disabled
  } else if value == 2 {
    DisabledByAttachedBody
  } else {
    Enabled
  }
}

///|
fn parse_isometry2(fields : Array[String], start : Int) -> @core.Isometry2 {
  let tx = if fields.length() > start {
    ij_parse_real_value(fields[start])
  } else {
    0.0F
  }
  let ty = if fields.length() > start + 1 {
    ij_parse_real_value(fields[start + 1])
  } else {
    0.0F
  }
  let rot = if fields.length() > start + 2 {
    ij_parse_real_value(fields[start + 2])
  } else {
    0.0F
  }
  Isometry2(Vec2(tx, ty), @core.Rot2::from_angle(rot))
}

///|
fn write_motor_fields(sb : StringBuilder, motor : JointMotor) -> Unit {
  sb.write_object(motor.target_vel)
  sb.write_char('^')
  sb.write_object(motor.target_pos)
  sb.write_char('^')
  sb.write_object(motor.stiffness)
  sb.write_char('^')
  sb.write_object(motor.damping)
  sb.write_char('^')
  sb.write_object(motor.max_force)
  sb.write_char('^')
  sb.write_object(motor.impulse)
  sb.write_char('^')
  sb.write_object(motor_model_to_int(motor.model))
}

///|
fn write_limits_fields(sb : StringBuilder, limits : JointLimits) -> Unit {
  sb.write_object(limits.min)
  sb.write_char('^')
  sb.write_object(limits.max)
  sb.write_char('^')
  sb.write_object(limits.impulse)
}

///|
const SERIALIZED_GENERIC_JOINT_FIELD_COUNT : Int = 88

///|
fn write_generic_joint_fields(sb : StringBuilder, joint : GenericJoint) -> Unit {
  sb.write_object(joint.local_frame1.translation.x)
  sb.write_char('^')
  sb.write_object(joint.local_frame1.translation.y)
  sb.write_char('^')
  sb.write_object(joint.local_frame1.rotation.angle())
  sb.write_char('^')
  sb.write_object(joint.local_frame2.translation.x)
  sb.write_char('^')
  sb.write_object(joint.local_frame2.translation.y)
  sb.write_char('^')
  sb.write_object(joint.local_frame2.rotation.angle())
  sb.write_char('^')
  sb.write_object(joint.locked_axes.bits())
  sb.write_char('^')
  sb.write_object(joint.limit_axes.bits())
  sb.write_char('^')
  sb.write_object(joint.motor_axes.bits())
  sb.write_char('^')
  sb.write_object(joint.coupled_axes.bits())
  sb.write_char('^')
  // JointLimits3
  write_limits_fields(sb, joint.limits.lin_x)
  sb.write_char('^')
  write_limits_fields(sb, joint.limits.lin_y)
  sb.write_char('^')
  write_limits_fields(sb, joint.limits.lin_z)
  sb.write_char('^')
  write_limits_fields(sb, joint.limits.ang_x)
  sb.write_char('^')
  write_limits_fields(sb, joint.limits.ang_y)
  sb.write_char('^')
  write_limits_fields(sb, joint.limits.ang_z)
  sb.write_char('^')
  // JointMotors3
  write_motor_fields(sb, joint.motors.lin_x)
  sb.write_char('^')
  write_motor_fields(sb, joint.motors.lin_y)
  sb.write_char('^')
  write_motor_fields(sb, joint.motors.lin_z)
  sb.write_char('^')
  write_motor_fields(sb, joint.motors.ang_x)
  sb.write_char('^')
  write_motor_fields(sb, joint.motors.ang_y)
  sb.write_char('^')
  write_motor_fields(sb, joint.motors.ang_z)
  sb.write_char('^')
  sb.write_object(joint.softness.natural_frequency)
  sb.write_char('^')
  sb.write_object(joint.softness.damping_ratio)
  sb.write_char('^')
  sb.write_object(if joint.contacts_enabled { 1 } else { 0 })
  sb.write_char('^')
  sb.write_object(joint_enabled_to_int(joint.enabled))
  sb.write_char('^')
  sb.write_object(joint.user_data)
  sb.write_char('^')
  sb.write_object(joint.local_anchor1_3.z)
  sb.write_char('^')
  sb.write_object(joint.local_anchor2_3.z)
  sb.write_char('^')
  sb.write_object(joint.axis3.x)
  sb.write_char('^')
  sb.write_object(joint.axis3.y)
  sb.write_char('^')
  sb.write_object(joint.axis3.z)
  sb.write_char('^')
  sb.write_object(joint.local_frame1_rotation.x)
  sb.write_char('^')
  sb.write_object(joint.local_frame1_rotation.y)
  sb.write_char('^')
  sb.write_object(joint.local_frame1_rotation.z)
  sb.write_char('^')
  sb.write_object(joint.local_frame1_rotation.w)
  sb.write_char('^')
  sb.write_object(joint.local_frame2_rotation.x)
  sb.write_char('^')
  sb.write_object(joint.local_frame2_rotation.y)
  sb.write_char('^')
  sb.write_object(joint.local_frame2_rotation.z)
  sb.write_char('^')
  sb.write_object(joint.local_frame2_rotation.w)
}

///|
fn parse_motor(fields : Array[String], start : Int) -> JointMotor {
  let target_vel = if fields.length() > start {
    ij_parse_real_value(fields[start])
  } else {
    0.0F
  }
  let target_pos = if fields.length() > start + 1 {
    ij_parse_real_value(fields[start + 1])
  } else {
    0.0F
  }
  let stiffness = if fields.length() > start + 2 {
    ij_parse_real_value(fields[start + 2])
  } else {
    0.0F
  }
  let damping = if fields.length() > start + 3 {
    ij_parse_real_value(fields[start + 3])
  } else {
    0.0F
  }
  let max_force = if fields.length() > start + 4 {
    ij_parse_real_value(fields[start + 4])
  } else {
    REAL_MAX
  }
  let impulse = if fields.length() > start + 5 {
    ij_parse_real_value(fields[start + 5])
  } else {
    0.0F
  }
  let model = if fields.length() > start + 6 {
    motor_model_from_int(ij_parse_int_value(fields[start + 6]))
  } else {
    AccelerationBased
  }
  { target_vel, target_pos, stiffness, damping, max_force, impulse, model }
}

///|
fn parse_limits(fields : Array[String], start : Int) -> JointLimits {
  let min = if fields.length() > start {
    ij_parse_real_value(fields[start])
  } else {
    -REAL_MAX
  }
  let max = if fields.length() > start + 1 {
    ij_parse_real_value(fields[start + 1])
  } else {
    REAL_MAX
  }
  let impulse = if fields.length() > start + 2 {
    ij_parse_real_value(fields[start + 2])
  } else {
    0.0F
  }
  let limits = JointLimits(min, max)
  limits.impulse = impulse
  limits
}

///|
fn parse_generic_joint(fields : Array[String], start : Int) -> GenericJoint {
  let frame1 = parse_isometry2(fields, start)
  let frame2 = parse_isometry2(fields, start + 3)
  let locked_axes = if fields.length() > start + 6 {
    JointAxesMask(ij_parse_int_value(fields[start + 6]))
  } else {
    JointAxesMask::empty()
  }
  let limit_axes = if fields.length() > start + 7 {
    JointAxesMask(ij_parse_int_value(fields[start + 7]))
  } else {
    JointAxesMask::empty()
  }
  let motor_axes = if fields.length() > start + 8 {
    JointAxesMask(ij_parse_int_value(fields[start + 8]))
  } else {
    JointAxesMask::empty()
  }
  let coupled_axes = if fields.length() > start + 9 {
    JointAxesMask(ij_parse_int_value(fields[start + 9]))
  } else {
    JointAxesMask::empty()
  }
  let lx = parse_limits(fields, start + 10)
  let ly = parse_limits(fields, start + 13)
  let lz = parse_limits(fields, start + 16)
  let ax = parse_limits(fields, start + 19)
  let ay = parse_limits(fields, start + 22)
  let az = parse_limits(fields, start + 25)
  let mx = parse_motor(fields, start + 28)
  let my = parse_motor(fields, start + 35)
  let mz_lin = parse_motor(fields, start + 42)
  let max_ang = parse_motor(fields, start + 49)
  let may_ang = parse_motor(fields, start + 56)
  let maz_ang = parse_motor(fields, start + 63)
  let natural_frequency = if fields.length() > start + 70 {
    ij_parse_real_value(fields[start + 70])
  } else {
    SpringCoefficients::joint_defaults().natural_frequency
  }
  let damping_ratio = if fields.length() > start + 71 {
    ij_parse_real_value(fields[start + 71])
  } else {
    SpringCoefficients::joint_defaults().damping_ratio
  }
  let contacts_enabled = if fields.length() > start + 72 {
    ij_parse_bool_value(fields[start + 72])
  } else {
    true
  }
  let enabled = if fields.length() > start + 73 {
    joint_enabled_from_int(ij_parse_int_value(fields[start + 73]))
  } else {
    Enabled
  }
  let user_data = if fields.length() > start + 74 {
    ij_parse_int_value(fields[start + 74])
  } else {
    0
  }
  let anchor1_z = if fields.length() > start + 75 {
    ij_parse_real_value(fields[start + 75])
  } else {
    0.0F
  }
  let anchor2_z = if fields.length() > start + 76 {
    ij_parse_real_value(fields[start + 76])
  } else {
    0.0F
  }
  let axis3 = if fields.length() > start + 79 {
    @core.Vec3(
      ij_parse_real_value(fields[start + 77]),
      ij_parse_real_value(fields[start + 78]),
      ij_parse_real_value(fields[start + 79]),
    )
  } else {
    Vec3(0.0F, 0.0F, 1.0F)
  }
  let frame1_rotation = if fields.length() > start + 83 {
    @core.Quat(
      ij_parse_real_value(fields[start + 80]),
      ij_parse_real_value(fields[start + 81]),
      ij_parse_real_value(fields[start + 82]),
      ij_parse_real_value(fields[start + 83]),
    )
  } else {
    @core.rotation_from_scaled_axis(Vec3(0.0F, 0.0F, frame1.rotation.angle()))
  }
  let frame2_rotation = if fields.length() > start + 87 {
    @core.Quat(
      ij_parse_real_value(fields[start + 84]),
      ij_parse_real_value(fields[start + 85]),
      ij_parse_real_value(fields[start + 86]),
      ij_parse_real_value(fields[start + 87]),
    )
  } else {
    @core.rotation_from_scaled_axis(Vec3(0.0F, 0.0F, frame2.rotation.angle()))
  }
  {
    local_frame1: frame1,
    local_frame2: frame2,
    axis3,
    local_anchor1_3: Vec3(frame1.translation.x, frame1.translation.y, anchor1_z),
    local_anchor2_3: Vec3(frame2.translation.x, frame2.translation.y, anchor2_z),
    local_frame1_rotation: frame1_rotation,
    local_frame2_rotation: frame2_rotation,
    locked_axes,
    limit_axes,
    motor_axes,
    coupled_axes,
    limits: { lin_x: lx, lin_y: ly, lin_z: lz, ang_x: ax, ang_y: ay, ang_z: az },
    motors: {
      lin_x: mx,
      lin_y: my,
      lin_z: mz_lin,
      ang_x: max_ang,
      ang_y: may_ang,
      ang_z: maz_ang,
    },
    softness: SpringCoefficients(natural_frequency, damping_ratio),
    contacts_enabled,
    enabled,
    user_data,
  }
}

///|
pub fn ImpulseJointSet::serialize(self : ImpulseJointSet) -> String {
  let sb = StringBuilder()
  sb.write_string("len=")
  sb.write_object(self.joint_generations.length())
  sb.write_char(';')
  sb.write_string("gens=")
  for i in 0.. 0 {
      sb.write_char(',')
    }
    sb.write_object(self.joint_generations[i])
  }
  sb.write_char(';')
  sb.write_string("free=")
  for i in 0.. 0 {
      sb.write_char(',')
    }
    sb.write_object(self.free_handles[i])
  }
  sb.write_char(';')
  sb.write_string("joints=")
  for i in 0.. 0 {
      sb.write_char('|')
    }
    let edge_id = if i < self.joint_edge_ids.length() {
      self.joint_edge_ids[i]
    } else {
      -1
    }
    if edge_id >= 0 && edge_id < self.graph_edges.length() {
      let edge = self.graph_edges[edge_id]
      let joint = edge.joint
      let (b1id, b1gen) = joint.body1.into_raw_parts()
      let (b2id, b2gen) = joint.body2.into_raw_parts()
      sb.write_string("J^")
      sb.write_object(i)
      sb.write_char('^')
      sb.write_object(self.joint_generations[i])
      sb.write_char('^')
      sb.write_object(b1id)
      sb.write_char('^')
      sb.write_object(b1gen)
      sb.write_char('^')
      sb.write_object(b2id)
      sb.write_char('^')
      sb.write_object(b2gen)
      sb.write_char('^')
      write_generic_joint_fields(sb, joint.data)
      sb.write_char('^')
      sb.write_object(joint.impulses.x)
      sb.write_char('^')
      sb.write_object(joint.impulses.y)
      sb.write_char('^')
      sb.write_object(joint.ang_impulse)
    } else {
      sb.write_char('N')
    }
  }
  sb.to_string()
}

///|
pub fn ImpulseJointSet::deserialize(data : String) -> ImpulseJointSet {
  let mut len = 0
  let gens : Array[Int] = []
  let free_handles : Array[Int] = []
  let entries : Array[String] = []
  for part in data.split(";"[:]) {
    let value = part.to_owned()
    if value.strip_prefix("len="[:]) is Some(raw_view) {
      len = ij_parse_int_value(raw_view.to_owned())
    } else if value.strip_prefix("gens="[:]) is Some(raw_view) {
      for item in ij_split_values(raw_view.to_owned(), ",") {
        gens.push(ij_parse_int_value(item))
      }
    } else if value.strip_prefix("free="[:]) is Some(raw_view) {
      for item in ij_split_values(raw_view.to_owned(), ",") {
        free_handles.push(ij_parse_int_value(item))
      }
    } else if value.strip_prefix("joints="[:]) is Some(raw_view) {
      for item in ij_split_values(raw_view.to_owned(), "|") {
        entries.push(item)
      }
    }
  }
  let set = ImpulseJointSet()
  let target_len = if len > 0 { len } else { gens.length() }
  while gens.length() < target_len {
    gens.push(0)
  }
  set.joint_generations.clear()
  set.joint_generations.append(gens[:])
  set.joint_edge_ids.clear()
  for _ in 0.. 0 {
        ij_parse_int_value(fields[0])
      } else {
        i
      }
      if id < 0 || id >= target_len {
        continue
      }
      let gen = if fields.length() > 1 {
        ij_parse_int_value(fields[1])
      } else {
        set.joint_generations[id]
      }
      set.joint_generations[id] = gen
      let b1id = if fields.length() > 2 {
        ij_parse_int_value(fields[2])
      } else {
        -1
      }
      let b1gen = if fields.length() > 3 {
        ij_parse_int_value(fields[3])
      } else {
        -1
      }
      let b2id = if fields.length() > 4 {
        ij_parse_int_value(fields[4])
      } else {
        -1
      }
      let b2gen = if fields.length() > 5 {
        ij_parse_int_value(fields[5])
      } else {
        -1
      }
      let body1 = RigidBodyHandle(b1id, b1gen)
      let body2 = RigidBodyHandle(b2id, b2gen)
      let joint_data = parse_generic_joint(fields, 6)
      let impulse_x_index = 6 + SERIALIZED_GENERIC_JOINT_FIELD_COUNT
      let impulses_x = if fields.length() > impulse_x_index {
        ij_parse_real_value(fields[impulse_x_index])
      } else {
        0.0F
      }
      let impulses_y = if fields.length() > impulse_x_index + 1 {
        ij_parse_real_value(fields[impulse_x_index + 1])
      } else {
        0.0F
      }
      let ang_impulse = if fields.length() > impulse_x_index + 2 {
        ij_parse_real_value(fields[impulse_x_index + 2])
      } else {
        0.0F
      }
      let handle = ImpulseJointHandle(id, gen)
      let joint = ImpulseJoint::{
        body1,
        body2,
        data: joint_data,
        impulses: Vec2(impulses_x, impulses_y),
        ang_impulse,
        handle,
      }
      let node1 = set.ensure_graph_node(body1)
      let node2 = set.ensure_graph_node(body2)
      let edge_id = set.graph_edges.length()
      set.graph_edges.push({ node1, node2, joint })
      set.adjacency[node1].push(edge_id)
      set.adjacency[node2].push(edge_id)
      set.joint_edge_ids[id] = edge_id
    }
  }
  set
}

///|
pub fn ImpulseJointSet::take_wake_up(
  self : ImpulseJointSet,
) -> Array[RigidBodyHandle] {
  let result : Array[RigidBodyHandle] = []
  for i in 0.. Array[(RigidBodyHandle, RigidBodyHandle)] {
  let result : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  for i in 0.. Unit {
  if id < 0 {
    return
  }
  let mut i = self.rb_graph_ids.length()
  while i <= id {
    self.rb_graph_ids.push(-1)
    self.rb_graph_generations.push(0)
    i = i + 1
  }
}

///|
fn ImpulseJointSet::graph_node_for(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
) -> Int? {
  if handle.id < 0 || handle.id >= self.rb_graph_ids.length() {
    return None
  }
  let node_id = self.rb_graph_ids[handle.id]
  if node_id < 0 || node_id >= self.graph_nodes.length() {
    return None
  }
  if self.rb_graph_generations[handle.id] != handle.generation {
    return None
  }
  if !handle_equals(self.graph_nodes[node_id], handle) {
    return None
  }
  Some(node_id)
}

///|
fn ImpulseJointSet::ensure_graph_node(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
) -> Int {
  self.ensure_body_mapping_capacity(handle.id)
  if self.graph_node_for(handle) is Some(node_id) {
    node_id
  } else {
    let node_id = self.graph_nodes.length()
    self.graph_nodes.push(handle)
    self.adjacency.push([])
    self.rb_graph_ids[handle.id] = node_id
    self.rb_graph_generations[handle.id] = handle.generation
    node_id
  }
}

///|
fn ImpulseJointSet::allocate_joint_handle(
  self : ImpulseJointSet,
) -> ImpulseJointHandle {
  if self.free_handles.pop() is Some(index) {
    let generation = self.joint_generations[index]
    ImpulseJointHandle(index, generation)
  } else {
    let index = self.joint_generations.length()
    self.joint_generations.push(0)
    self.joint_edge_ids.push(-1)
    ImpulseJointHandle(index, 0)
  }
}

///|
fn ImpulseJointSet::is_handle_valid(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
) -> Bool {
  if handle.id < 0 || handle.id >= self.joint_generations.length() {
    return false
  }
  if self.joint_generations[handle.id] != handle.generation {
    return false
  }
  let edge_id = self.joint_edge_ids[handle.id]
  edge_id >= 0 && edge_id < self.graph_edges.length()
}

///|
fn ImpulseJointSet::remove_edge(
  self : ImpulseJointSet,
  edge_id : Int,
) -> ImpulseJoint? {
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let removed_edge = self.graph_edges[edge_id]
  remove_edge_index(self.adjacency[removed_edge.node1], edge_id)
  if removed_edge.node2 != removed_edge.node1 {
    remove_edge_index(self.adjacency[removed_edge.node2], edge_id)
  }
  let last_index = self.graph_edges.length() - 1
  if edge_id != last_index {
    let moved_edge = self.graph_edges[last_index]
    self.graph_edges[edge_id] = moved_edge
    replace_edge_index(self.adjacency[moved_edge.node1], last_index, edge_id)
    if moved_edge.node2 != moved_edge.node1 {
      replace_edge_index(self.adjacency[moved_edge.node2], last_index, edge_id)
    }
    let handle_id = moved_edge.joint.handle.id
    if handle_id >= 0 && handle_id < self.joint_edge_ids.length() {
      self.joint_edge_ids[handle_id] = edge_id
    }
  }
  self.graph_edges.pop() |> ignore
  Some(removed_edge.joint)
}

///|
fn ImpulseJointSet::remove_node(self : ImpulseJointSet, node_id : Int) -> Unit {
  if node_id < 0 || node_id >= self.graph_nodes.length() {
    return
  }
  let removed_handle = self.graph_nodes[node_id]
  if removed_handle.id >= 0 && removed_handle.id < self.rb_graph_ids.length() {
    self.rb_graph_ids[removed_handle.id] = -1
  }
  let last_index = self.graph_nodes.length() - 1
  if node_id != last_index {
    let moved_handle = self.graph_nodes[last_index]
    self.graph_nodes[node_id] = moved_handle
    self.adjacency[node_id] = self.adjacency[last_index]
    if moved_handle.id >= 0 && moved_handle.id < self.rb_graph_ids.length() {
      self.rb_graph_ids[moved_handle.id] = node_id
      self.rb_graph_generations[moved_handle.id] = moved_handle.generation
    }
    let edges = self.adjacency[node_id]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let edge = self.graph_edges[edge_id]
        if edge.node1 == last_index {
          edge.node1 = node_id
        }
        if edge.node2 == last_index {
          edge.node2 = node_id
        }
        self.graph_edges[edge_id] = edge
      }
    }
  }
  self.graph_nodes.pop() |> ignore
  self.adjacency.pop() |> ignore
}

///|
pub fn ImpulseJointSet::insert(
  self : ImpulseJointSet,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
  joint : GenericJoint,
  wake_up : Bool,
) -> ImpulseJointHandle {
  let handle = self.allocate_joint_handle()
  if body1.id < 0 || body2.id < 0 {
    return handle
  }
  let node1 = self.ensure_graph_node(body1)
  let node2 = self.ensure_graph_node(body2)
  // Make sure each stored joint has its own backing data, even if the caller reuses the
  // same GenericJoint value for multiple inserts.
  let joint_data = GenericJoint::{
    local_frame1: joint.local_frame1,
    local_frame2: joint.local_frame2,
    axis3: joint.axis3,
    local_anchor1_3: joint.local_anchor1_3,
    local_anchor2_3: joint.local_anchor2_3,
    local_frame1_rotation: joint.local_frame1_rotation,
    local_frame2_rotation: joint.local_frame2_rotation,
    locked_axes: joint.locked_axes,
    limit_axes: joint.limit_axes,
    motor_axes: joint.motor_axes,
    coupled_axes: joint.coupled_axes,
    limits: joint.limits,
    motors: joint.motors,
    softness: joint.softness,
    contacts_enabled: joint.contacts_enabled,
    enabled: joint.enabled,
    user_data: joint.user_data,
  }
  let impulse_joint = ImpulseJoint::{
    body1,
    body2,
    data: joint_data,
    impulses: @core.Vec2::zero(),
    ang_impulse: 0.0F,
    handle,
  }
  let edge_id = self.graph_edges.length()
  self.graph_edges.push({ node1, node2, joint: impulse_joint })
  self.adjacency[node1].push(edge_id)
  if node2 != node1 {
    self.adjacency[node2].push(edge_id)
  }
  self.joint_edge_ids[handle.id] = edge_id
  if wake_up {
    push_unique_handle(self.to_wake_up, body1)
    push_unique_handle(self.to_wake_up, body2)
  }
  push_unique_pair(self.to_join, (body1, body2))
  handle
}

///|
pub fn ImpulseJointSet::len(self : ImpulseJointSet) -> Int {
  self.graph_edges.length()
}

///|
pub fn ImpulseJointSet::is_empty(self : ImpulseJointSet) -> Bool {
  self.graph_edges.length() == 0
}

///|
pub fn ImpulseJointSet::joint_graph(
  self : ImpulseJointSet,
) -> ImpulseJointGraph {
  let nodes : Array[RigidBodyHandle] = []
  for i in 0..= 0 && edge.node1 < self.graph_nodes.length() {
      self.graph_nodes[edge.node1]
    } else {
      RigidBodyHandle::invalid()
    }
    let body2 = if edge.node2 >= 0 && edge.node2 < self.graph_nodes.length() {
      self.graph_nodes[edge.node2]
    } else {
      RigidBodyHandle::invalid()
    }
    edges.push((body1, body2, edge.joint))
  }
  { nodes, edges }
}

///|
pub fn ImpulseJointSet::contains(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
) -> Bool {
  self.is_handle_valid(handle)
}

///|
pub fn ImpulseJointSet::get(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  Some(self.graph_edges[edge_id].joint)
}

///|
pub fn ImpulseJointSet::get_mut(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
  wake_up : Bool,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let joint = self.graph_edges[edge_id].joint
  if wake_up {
    push_unique_handle(self.to_wake_up, joint.body1)
    push_unique_handle(self.to_wake_up, joint.body2)
  }
  Some(joint)
}

///|
pub fn ImpulseJointSet::set_enabled(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
  enabled : Bool,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  self.graph_edges[edge_id] = {
    node1: edge.node1,
    node2: edge.node2,
    joint: edge.joint.set_enabled(enabled),
  }
}

///|
pub fn ImpulseJointSet::set_impulses(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
  impulses : @core.Vec2,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  let joint = edge.joint
  joint.impulses = impulses
  self.graph_edges[edge_id] = { node1: edge.node1, node2: edge.node2, joint }
}

///|
fn impulse_joint_solver_angular_axis(data : GenericJoint) -> JointAxis {
  let spatial_axes = data
    .locked_axes()
    .or(data.limit_axes())
    .or(data.motor_axes())
    .or(data.coupled_axes())
    .intersection(
      JointAxesMask::lin_z().or(
        JointAxesMask::ang_x().or(JointAxesMask::ang_y()),
      ),
    )
  if spatial_axes.bits() == 0 {
    return AngZ
  }
  let locked_axes = data.locked_axes()
  if !locked_axes.contains(JointAxesMask::ang_x()) {
    return AngX
  }
  if !locked_axes.contains(JointAxesMask::ang_y()) {
    return AngY
  }
  if !locked_axes.contains(JointAxesMask::ang_z()) {
    return AngZ
  }
  let motor_axes = data.motor_axes()
  if motor_axes.contains(JointAxesMask::ang_x()) {
    return AngX
  }
  if motor_axes.contains(JointAxesMask::ang_y()) {
    return AngY
  }
  if motor_axes.contains(JointAxesMask::ang_z()) {
    return AngZ
  }
  let limit_axes = data.limit_axes()
  if limit_axes.contains(JointAxesMask::ang_x()) {
    return AngX
  }
  if limit_axes.contains(JointAxesMask::ang_y()) {
    return AngY
  }
  AngZ
}

///|
pub fn ImpulseJointSet::set_solver_impulses(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
  impulses : @core.Vec2,
  ang_impulse : @core.Real,
  motor_lin_x : @core.Real,
  motor_lin_y : @core.Real,
  motor_ang_z : @core.Real,
  limit_lin_x : @core.Real,
  limit_lin_y : @core.Real,
  limit_ang_z : @core.Real,
) -> Unit {
  if !self.is_handle_valid(handle) {
    return
  }
  let edge_id = self.joint_edge_ids[handle.id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return
  }
  let edge = self.graph_edges[edge_id]
  let joint = edge.joint
  let angular_axis = impulse_joint_solver_angular_axis(joint.data)
  joint.impulses = impulses
  joint.ang_impulse = ang_impulse
  let motors = joint.data.motors
  let updated_motors = JointMotors3::{
    lin_x: motors.lin_x.set_impulse(motor_lin_x),
    lin_y: motors.lin_y.set_impulse(motor_lin_y),
    lin_z: motors.lin_z,
    ang_x: motors.ang_x,
    ang_y: motors.ang_y,
    ang_z: motors.ang_z,
  }
  let updated_motor = joint_motor_for_axis(updated_motors, angular_axis).set_impulse(
    motor_ang_z,
  )
  joint.data.motors = joint_motors_set_axis(
    updated_motors, angular_axis, updated_motor,
  )
  let limits = joint.data.limits
  let updated_limits = JointLimits3::{
    lin_x: limits.lin_x.set_impulse(limit_lin_x),
    lin_y: limits.lin_y.set_impulse(limit_lin_y),
    lin_z: limits.lin_z,
    ang_x: limits.ang_x,
    ang_y: limits.ang_y,
    ang_z: limits.ang_z,
  }
  let updated_limit = joint_limits_for_axis(updated_limits, angular_axis).set_impulse(
    limit_ang_z,
  )
  joint.data.limits = joint_limits_set_axis(
    updated_limits, angular_axis, updated_limit,
  )
  self.graph_edges[edge_id] = { node1: edge.node1, node2: edge.node2, joint }
}

///|
pub fn ImpulseJointSet::get_unknown_gen(
  self : ImpulseJointSet,
  id : Int,
) -> (ImpulseJoint, ImpulseJointHandle)? {
  if id < 0 || id >= self.joint_edge_ids.length() {
    return None
  }
  let edge_id = self.joint_edge_ids[id]
  if edge_id < 0 || edge_id >= self.graph_edges.length() {
    return None
  }
  let handle = ImpulseJointHandle(id, self.joint_generations[id])
  Some((self.graph_edges[edge_id].joint, handle))
}

///|
pub fn ImpulseJointSet::get_unknown_gen_mut(
  self : ImpulseJointSet,
  id : Int,
) -> (ImpulseJoint, ImpulseJointHandle)? {
  self.get_unknown_gen(id)
}

///|
pub fn ImpulseJointSet::iter(
  self : ImpulseJointSet,
) -> Array[(ImpulseJointHandle, ImpulseJoint)] {
  let result : Array[(ImpulseJointHandle, ImpulseJoint)] = []
  for i in 0.. Array[(ImpulseJointHandle, ImpulseJoint)] {
  self.iter()
}

///|
pub fn ImpulseJointSet::remove(
  self : ImpulseJointSet,
  handle : ImpulseJointHandle,
  wake_up : Bool,
) -> ImpulseJoint? {
  if !self.is_handle_valid(handle) {
    return None
  }
  let edge_id = self.joint_edge_ids[handle.id]
  let removed = self.remove_edge(edge_id)
  if removed is Some(joint) {
    self.joint_edge_ids[handle.id] = -1
    self.joint_generations[handle.id] = self.joint_generations[handle.id] + 1
    self.free_handles.push(handle.id)
    if wake_up {
      push_unique_handle(self.to_wake_up, joint.body1)
      push_unique_handle(self.to_wake_up, joint.body2)
    }
    Some(joint)
  } else {
    None
  }
}

///|
pub fn ImpulseJointSet::remove_joints_attached_to_rigid_body(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
) -> Array[ImpulseJointHandle] {
  if self.graph_node_for(handle) is Some(node_index) {
    let to_remove : Array[ImpulseJointHandle] = []
    let edges_snapshot = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        to_remove.push(self.graph_edges[edge_id].joint.handle)
      }
    }
    let removed : Array[ImpulseJointHandle] = []
    for i in 0..= 0 &&
      node_index < self.adjacency.length() &&
      self.adjacency[node_index].length() == 0 {
      self.remove_node(node_index)
    }
    removed
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet::map_attached_joints_mut(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
  f : (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint) -> Unit,
) -> Unit {
  if self.graph_node_for(handle) is Some(node_index) {
    let edges = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let joint = self.graph_edges[edge_id].joint
        f(joint.body1, joint.body2, joint.handle, joint)
      }
    }
  }
}

///|
pub fn ImpulseJointSet::attached_joints(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint)] {
  if self.graph_node_for(handle) is Some(node_index) {
    let result : Array[
      (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint),
    ] = []
    let edges = self.adjacency[node_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let joint = self.graph_edges[edge_id].joint
        result.push((joint.body1, joint.body2, joint.handle, joint))
      }
    }
    result
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet::attached_enabled_joints(
  self : ImpulseJointSet,
  handle : RigidBodyHandle,
) -> Array[(RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint)] {
  let attached = self.attached_joints(handle)
  let result : Array[
    (RigidBodyHandle, RigidBodyHandle, ImpulseJointHandle, ImpulseJoint),
  ] = []
  for i in 0.. Array[(ImpulseJointHandle, ImpulseJoint)] {
  if self.graph_node_for(body1) is Some(node1_index) &&
    self.graph_node_for(body2) is Some(node2_index) {
    let result : Array[(ImpulseJointHandle, ImpulseJoint)] = []
    let edges = self.adjacency[node1_index]
    for i in 0..= 0 && edge_id < self.graph_edges.length() {
        let edge = self.graph_edges[edge_id]
        if edge.node1 == node2_index || edge.node2 == node2_index {
          result.push((edge.joint.handle, edge.joint))
        }
      }
    }
    result
  } else {
    []
  }
}

///|
pub fn ImpulseJointSet::select_active_interactions(
  self : ImpulseJointSet,
  islands : IslandManager,
  bodies : RigidBodySet,
  out : Array[Array[Int]],
) -> Unit {
  let awake_len = islands.awake_islands.length()
  for i in 0..= islands.islands.length() {
          continue
        }
        if islands.islands[island_id] is Some(island) {
          if island.id_in_awake_list is Some(awake_index) {
            island_awake_index = awake_index
          }
        }
      } else {
        let island_id = body1.ids.active_island_id
        if island_id < 0 || island_id >= islands.islands.length() {
          continue
        }
        if islands.islands[island_id] is Some(island) {
          if island.id_in_awake_list is Some(awake_index) {
            island_awake_index = awake_index
          }
        }
      }
      if island_awake_index < 0 {
        continue
      }
      if island_awake_index >= 0 && island_awake_index < out.length() {
        out[island_awake_index].push(i)
      }
    }
  }
}