// 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.

///|
pub struct PhysicsPipeline {
  prev_collisions : Array[
    (
      @collision.ColliderHandle,
      @collision.ColliderHandle,
      @collision.CollisionEventFlags,
      Bool,
    ),
  ]
  prev_intersections : Array[
    (@collision.ColliderHandle, @collision.ColliderHandle, Bool),
  ]
  contact_solver_cache : Array[ContactImpulseCacheEntry]
  counters : @counters.Counters
}

///|
pub fn PhysicsPipeline::PhysicsPipeline() -> PhysicsPipeline {
  {
    prev_collisions: [],
    prev_intersections: [],
    contact_solver_cache: [],
    counters: @counters.Counters::default(),
  }
}

///|
fn min_value(a : @core.Real, b : @core.Real) -> @core.Real {
  if a < b {
    a
  } else {
    b
  }
}

///|
fn max_value(a : @core.Real, b : @core.Real) -> @core.Real {
  if a > b {
    a
  } else {
    b
  }
}

///|
fn clamp_value(
  value : @core.Real,
  min_val : @core.Real,
  max_val : @core.Real,
) -> @core.Real {
  max_value(min_val, min_value(max_val, value))
}

///|
fn separation_ball_ball(
  pos1 : @core.Vec2,
  r1 : @core.Real,
  pos2 : @core.Vec2,
  r2 : @core.Real,
) -> @core.Vec2? {
  let delta = pos1.sub(pos2)
  let dist = delta.length()
  let radius = r1 + r2
  if dist > radius {
    None
  } else {
    let dir = if dist <= 1.0e-6F {
      @core.Vec2(1.0F, 0.0F)
    } else {
      delta.normalize()
    }
    let depth = radius - dist
    Some(Vec2(dir.x * depth, dir.y * depth))
  }
}

///|
fn vec2_scale(v : @core.Vec2, s : @core.Real) -> @core.Vec2 {
  Vec2(v.x * s, v.y * s)
}

///|
fn cuboid_axes(rotation : @core.Real) -> (@core.Vec2, @core.Vec2) {
  let rot = @core.Rot2::from_angle(rotation)
  (Vec2(rot.cos, rot.sin), Vec2(-rot.sin, rot.cos))
}

///|
fn projection_radius(
  axis : @core.Vec2,
  cuboid_axis_x : @core.Vec2,
  cuboid_axis_y : @core.Vec2,
  half_width : @core.Real,
  half_height : @core.Real,
) -> @core.Real {
  @core.abs(axis.dot(cuboid_axis_x)) * half_width +
  @core.abs(axis.dot(cuboid_axis_y)) * half_height
}

///|
fn separation_cuboid_cuboid(
  pos1 : @core.Vec2,
  rot1 : @core.Real,
  hw1 : @core.Real,
  hh1 : @core.Real,
  pos2 : @core.Vec2,
  rot2 : @core.Real,
  hw2 : @core.Real,
  hh2 : @core.Real,
) -> @core.Vec2? {
  let (ax1, ay1) = cuboid_axes(rot1)
  let (ax2, ay2) = cuboid_axes(rot2)
  let delta = pos1.sub(pos2)
  let mut best_overlap = 0.0F
  let mut best_axis = @core.Vec2(1.0F, 0.0F)
  let mut best_sign = 1.0F
  let axes = [ax1, ay1, ax2, ay2]
  for i in 0.. @core.Vec2? {
  let rot = @core.Rot2::from_angle(cuboid_rot)
  let inv = rot.inverse()
  let local_ball = inv.rotate_vec2(ball_pos.sub(cuboid_pos))
  let clamped = @core.Vec2(
    clamp_value(local_ball.x, -half_width, half_width),
    clamp_value(local_ball.y, -half_height, half_height),
  )
  let local_delta = local_ball.sub(clamped)
  let dist = local_delta.length()
  if dist > radius {
    return None
  }
  if dist > 1.0e-6F {
    let dir_world = rot.rotate_vec2(local_delta.normalize())
    let depth = radius - dist
    Some(vec2_scale(dir_world, depth))
  } else {
    // Circle center is inside (or extremely close to) the cuboid. Pick the closest face.
    let dx = half_width - @core.abs(local_ball.x)
    let dy = half_height - @core.abs(local_ball.y)
    if dx < dy {
      let sign = if local_ball.x < 0.0F { -1.0F } else { 1.0F }
      let dir_world = rot.rotate_vec2(Vec2(sign, 0.0F))
      Some(vec2_scale(dir_world, radius + dx))
    } else {
      let sign = if local_ball.y < 0.0F { -1.0F } else { 1.0F }
      let dir_world = rot.rotate_vec2(Vec2(0.0F, sign))
      Some(vec2_scale(dir_world, radius + dy))
    }
  }
}

///|
fn separation_vector(
  shape1 : @collision.Shape,
  pos1 : @core.Vec2,
  rot1 : @core.Real,
  shape2 : @collision.Shape,
  pos2 : @core.Vec2,
  rot2 : @core.Real,
) -> @core.Vec2? {
  fn segment_world_endpoints(
    center : @core.Vec2,
    rotation : @core.Real,
    a : @core.Vec2,
    b : @core.Vec2,
  ) -> (@core.Vec2, @core.Vec2) {
    let rot = @core.Rot2::from_angle(rotation)
    (center.add(rot.rotate_vec2(a)), center.add(rot.rotate_vec2(b)))
  }

  fn closest_point_on_segment(
    a : @core.Vec2,
    b : @core.Vec2,
    p : @core.Vec2,
  ) -> @core.Vec2 {
    let ab = b.sub(a)
    let denom = ab.dot(ab)
    if denom <= 1.0e-12F {
      a
    } else {
      let t = clamp_value(p.sub(a).dot(ab) / denom, 0.0F, 1.0F)
      a.add(vec2_scale(ab, t))
    }
  }

  fn segment_aabb_intersect(
    a : @core.Vec2,
    b : @core.Vec2,
    hw : @core.Real,
    hh : @core.Real,
  ) -> Bool {
    let d = b.sub(a)
    let mut t0 = 0.0F
    let mut t1 = 1.0F
    let p = [-d.x, d.x, -d.y, d.y]
    let q = [a.x + hw, hw - a.x, a.y + hh, hh - a.y]
    for i in 0..<4 {
      let pi = p[i]
      let qi = q[i]
      if @core.abs(pi) <= 1.0e-12F {
        if qi < 0.0F {
          return false
        }
      } else {
        let r = qi / pi
        if pi < 0.0F {
          if r > t1 {
            return false
          }
          if r > t0 {
            t0 = r
          }
        } else {
          if r < t0 {
            return false
          }
          if r < t1 {
            t1 = r
          }
        }
      }
    }
    true
  }

  fn separation_ball_segment(
    ball_center : @core.Vec2,
    radius : @core.Real,
    seg_center : @core.Vec2,
    seg_rot : @core.Real,
    a : @core.Vec2,
    b : @core.Vec2,
  ) -> @core.Vec2? {
    let (sa, sb) = segment_world_endpoints(seg_center, seg_rot, a, b)
    let q = closest_point_on_segment(sa, sb, ball_center)
    let delta = ball_center.sub(q)
    let dist = delta.length()
    if dist >= radius {
      None
    } else {
      let dir = if dist <= 1.0e-6F {
        // Fallback: use segment normal.
        let d = sb.sub(sa)
        let n = @core.Vec2(-d.y, d.x)
        if n.length_squared() <= 1.0e-12F {
          @core.Vec2(0.0F, 1.0F)
        } else {
          n.normalize()
        }
      } else {
        vec2_scale(delta, 1.0F / dist)
      }
      let depth = radius - dist
      Some(vec2_scale(dir, depth))
    }
  }

  fn separation_cuboid_segment(
    cuboid_center : @core.Vec2,
    cuboid_rot : @core.Real,
    half_width : @core.Real,
    half_height : @core.Real,
    seg_center : @core.Vec2,
    seg_rot : @core.Real,
    a : @core.Vec2,
    b : @core.Vec2,
  ) -> @core.Vec2? {
    let (sa, sb) = segment_world_endpoints(seg_center, seg_rot, a, b)
    // Early-out if they don't intersect.
    let rot = @core.Rot2::from_angle(cuboid_rot)
    let inv = rot.inverse()
    let la = inv.rotate_vec2(sa.sub(cuboid_center))
    let lb = inv.rotate_vec2(sb.sub(cuboid_center))
    if !segment_aabb_intersect(la, lb, half_width, half_height) {
      return None
    }
    fn project_cuboid(
      center : @core.Vec2,
      rot : @core.Real,
      hw : @core.Real,
      hh : @core.Real,
      axis : @core.Vec2,
    ) -> (@core.Real, @core.Real) {
      let (ax, ay) = cuboid_axes(rot)
      let c = center.dot(axis)
      let r = @core.abs(axis.dot(ax)) * hw + @core.abs(axis.dot(ay)) * hh
      (c - r, c + r)
    }

    fn project_segment(
      a : @core.Vec2,
      b : @core.Vec2,
      axis : @core.Vec2,
    ) -> (@core.Real, @core.Real) {
      let da = a.dot(axis)
      let db = b.dot(axis)
      if da < db {
        (da, db)
      } else {
        (db, da)
      }
    }

    fn support_point_cuboid_toward(
      center : @core.Vec2,
      rot : @core.Real,
      hw : @core.Real,
      hh : @core.Real,
      dir : @core.Vec2,
    ) -> @core.Vec2 {
      let (ax, ay) = cuboid_axes(rot)
      let sx = if dir.dot(ax) >= 0.0F { 1.0F } else { -1.0F }
      let sy = if dir.dot(ay) >= 0.0F { 1.0F } else { -1.0F }
      center.add(vec2_scale(ax, sx * hw)).add(vec2_scale(ay, sy * hh))
    }

    // SAT-style minimum translation vector (MTV) to separate the cuboid from the segment.
    //
    // Note: a segment is degenerate. Projecting it on its own normal produces a single point,
    // yielding zero overlap even when the segment crosses the cuboid. For that axis we use the
    // "point + normal" separation test (parry's segment_cuboid_find_local_separating_normal_oneway).
    let margin = 1.0e-6F
    let mut best : @core.Vec2? = None
    let mut best_len2 = 1.0e30F
    let (cub_ax, cub_ay) = cuboid_axes(cuboid_rot)
    let cub_axes = [cub_ax, cub_ay]
    for i in 0.. 1.0e-12F {
      let seg_n = seg_n_raw.normalize()
      let axis = if cuboid_center.sub(sa).dot(seg_n) >= 0.0F {
        seg_n
      } else {
        Vec2(-seg_n.x, -seg_n.y)
      }
      let support = support_point_cuboid_toward(
        cuboid_center,
        cuboid_rot,
        half_width,
        half_height,
        Vec2(-axis.x, -axis.y),
      )
      let separation = support.sub(sa).dot(axis)
      if separation > 0.0F {
        return None
      }
      let cand = vec2_scale(axis, -separation + margin)
      let l2 = cand.length_squared()
      if l2 < best_len2 {
        best_len2 = l2
        best = Some(cand)
      }
    }
    best
  }

  fn separation_basic(
    s1 : @collision.Shape,
    p1 : @core.Vec2,
    r1 : @core.Real,
    s2 : @collision.Shape,
    p2 : @core.Vec2,
    r2 : @core.Real,
  ) -> @core.Vec2? {
    match (s1, s2) {
      (Ball(br1), Ball(br2)) => separation_ball_ball(p1, br1, p2, br2)
      (Cuboid(hw1, hh1), Cuboid(hw2, hh2)) =>
        separation_cuboid_cuboid(p1, r1, hw1, hh1, p2, r2, hw2, hh2)
      (Ball(r), Cuboid(hw, hh)) => separation_ball_cuboid(p1, r, p2, r2, hw, hh)
      (Cuboid(hw, hh), Ball(r)) =>
        if separation_ball_cuboid(p2, r, p1, r1, hw, hh) is Some(sep) {
          Some(Vec2(-sep.x, -sep.y))
        } else {
          None
        }
      (Ball(r), Segment(a, b)) => separation_ball_segment(p1, r, p2, r2, a, b)
      (Segment(a, b), Ball(r)) =>
        if separation_ball_segment(p2, r, p1, r1, a, b) is Some(sep) {
          Some(Vec2(-sep.x, -sep.y))
        } else {
          None
        }
      (Cuboid(hw, hh), Segment(a, b)) =>
        separation_cuboid_segment(p1, r1, hw, hh, p2, r2, a, b)
      (Segment(a, b), Cuboid(hw, hh)) =>
        if separation_cuboid_segment(p2, r2, hw, hh, p1, r1, a, b) is Some(sep) {
          Some(Vec2(-sep.x, -sep.y))
        } else {
          None
        }
      _ => None
    }
  }

  fn capsule_components(
    s : @collision.Shape,
    center : @core.Vec2,
    rotation : @core.Real,
  ) -> Array[(@collision.Shape, @core.Vec2, @core.Real)] {
    match s {
      CapsuleX(half_height, radius) => {
        let rot = @core.Rot2::from_angle(rotation)
        let offset = rot.rotate_vec2(Vec2(half_height, 0.0F))
        let a = center.add(offset)
        let b = center.sub(offset)
        [
          (Ball(radius), a, 0.0F),
          (Ball(radius), b, 0.0F),
          (Cuboid(half_height, radius), center, rotation),
        ]
      }
      CapsuleY(half_height, radius) => {
        let rot = @core.Rot2::from_angle(rotation)
        let offset = rot.rotate_vec2(Vec2(0.0F, half_height))
        let a = center.add(offset)
        let b = center.sub(offset)
        [
          (Ball(radius), a, 0.0F),
          (Ball(radius), b, 0.0F),
          (Cuboid(radius, half_height), center, rotation),
        ]
      }
      _ => []
    }
  }

  fn best_sep(a : @core.Vec2?, b : @core.Vec2?) -> @core.Vec2? {
    if b is None {
      return a
    }
    if a is None {
      return b
    }
    let av = a.unwrap()
    let bv = b.unwrap()
    if bv.length_squared() > av.length_squared() {
      b
    } else {
      a
    }
  }

  fn polyline_for_each_segment(
    vertices : Array[@core.Vec2],
    indices : Array[(Int, Int)]?,
    callback : (@core.Vec2, @core.Vec2) -> Unit,
  ) -> Unit {
    if indices is Some(idxs) {
      for i in 0..= vertices.length() || i1 >= vertices.length() {
          continue
        }
        callback(vertices[i0], vertices[i1])
      }
    } else {
      if vertices.length() < 2 {
        return
      }
      for i in 0..<(vertices.length() - 1) {
        callback(vertices[i], vertices[i + 1])
      }
    }
  }

  fn separation_polyline_other(
    vertices : Array[@core.Vec2],
    indices : Array[(Int, Int)]?,
    poly_pos : @core.Vec2,
    poly_rot : @core.Real,
    other : @collision.Shape,
    other_pos : @core.Vec2,
    other_rot : @core.Real,
  ) -> @core.Vec2? {
    let mut best : @core.Vec2? = None
    polyline_for_each_segment(vertices, indices, fn(
      a : @core.Vec2,
      b : @core.Vec2,
    ) -> Unit {
      best = best_sep(
        best,
        separation_vector(
          Segment(a, b),
          poly_pos,
          poly_rot,
          other,
          other_pos,
          other_rot,
        ),
      )
    })
    best
  }

  fn separation_other_polyline(
    other : @collision.Shape,
    other_pos : @core.Vec2,
    other_rot : @core.Real,
    vertices : Array[@core.Vec2],
    indices : Array[(Int, Int)]?,
    poly_pos : @core.Vec2,
    poly_rot : @core.Real,
  ) -> @core.Vec2? {
    let mut best : @core.Vec2? = None
    polyline_for_each_segment(vertices, indices, fn(
      a : @core.Vec2,
      b : @core.Vec2,
    ) -> Unit {
      best = best_sep(
        best,
        separation_vector(
          other,
          other_pos,
          other_rot,
          Segment(a, b),
          poly_pos,
          poly_rot,
        ),
      )
    })
    best
  }

  match (shape1, shape2) {
    (Polyline(vertices, indices), other) =>
      separation_polyline_other(
        vertices, indices, pos1, rot1, other, pos2, rot2,
      )
    (other, Polyline(vertices, indices)) =>
      separation_other_polyline(
        other, pos1, rot1, vertices, indices, pos2, rot2,
      )
    (Ball(_), Ball(_))
    | (Ball(_), Cuboid(_, _))
    | (Cuboid(_, _), Ball(_))
    | (Cuboid(_, _), Cuboid(_, _))
    | (Ball(_), Segment(_, _))
    | (Segment(_, _), Ball(_))
    | (Cuboid(_, _), Segment(_, _))
    | (Segment(_, _), Cuboid(_, _))
    | (Segment(_, _), Segment(_, _)) =>
      separation_basic(shape1, pos1, rot1, shape2, pos2, rot2)
    (Ball(_), CapsuleX(_, _))
    | (Ball(_), CapsuleY(_, _))
    | (Cuboid(_, _), CapsuleX(_, _))
    | (Cuboid(_, _), CapsuleY(_, _))
    | (Segment(_, _), CapsuleX(_, _))
    | (Segment(_, _), CapsuleY(_, _)) => {
      let comps2 = capsule_components(shape2, pos2, rot2)
      let mut best : @core.Vec2? = None
      for i in 0.. {
      let comps1 = capsule_components(shape1, pos1, rot1)
      let mut best : @core.Vec2? = None
      for i in 0.. {
            let comps2 = capsule_components(shape2, pos2, rot2)
            let mut best2 : @core.Vec2? = None
            for j in 0.. separation_basic(c1.0, c1.1, c1.2, shape2, pos2, rot2)
        }
        best = best_sep(best, cand)
      }
      best
    }
    _ => separation_basic(shape1, pos1, rot1, shape2, pos2, rot2)
  }
}

///|
fn handle_user_changes_to_bodies(
  islands : @dynamics.IslandManager,
  bodies : @dynamics.RigidBodySet,
  colliders : @collision.ColliderSet,
  impulse_joints : @dynamics.ImpulseJointSet,
  multibody_joints : @dynamics.MultibodyJointSet,
  modified_bodies : Array[@dynamics.RigidBodyHandle],
) -> Unit {
  for i in 0.. {
        parent |> ignore
        child |> ignore
        joint_handle |> ignore
        joint.set_enabled_by_attached_body(body.is_enabled()) |> ignore
      })
      multibody_joints |> ignore
    }
  }
}

///|
fn update_body_mass_properties_from_colliders(
  bodies : @dynamics.RigidBodySet,
  colliders : @collision.ColliderSet,
  modified_bodies : Array[@dynamics.RigidBodyHandle],
) -> Unit {
  for i in 0.. ccd_max_dist {
              ccd_max_dist = dist
            }
          }
        }
      }
      body.set_local_mass_properties(mprops) |> ignore
      body.recompute_mass_properties_from_colliders() |> ignore
      body.set_ccd_properties(ccd_thickness, ccd_max_dist) |> ignore
    }
  }
}

///|
fn clear_modified_bodies(
  bodies : @dynamics.RigidBodySet,
  modified_bodies : Array[@dynamics.RigidBodyHandle],
) -> Unit {
  for i in 0.. ignore
    }
  }
}

///|
fn clear_modified_colliders(
  colliders : @collision.ColliderSet,
  modified_colliders : Array[@collision.ColliderHandle],
) -> Unit {
  colliders.clear_changes_for(modified_colliders)
}

///|
fn collider_handle_equals(
  a : @collision.ColliderHandle,
  b : @collision.ColliderHandle,
) -> Bool {
  @collision.ColliderHandle::equals(a, b)
}

///|
fn collision_pair_equals(
  a : (@collision.ColliderHandle, @collision.ColliderHandle),
  b : (@collision.ColliderHandle, @collision.ColliderHandle),
) -> Bool {
  collider_handle_equals(a.0, b.0) && collider_handle_equals(a.1, b.1)
}

///|
fn collision_pair_in_list(
  pair : (@collision.ColliderHandle, @collision.ColliderHandle),
  list : Array[(@collision.ColliderHandle, @collision.ColliderHandle)],
) -> Bool {
  for i in 0.. Unit {
  fn feature_id_equals(
    a : @collision.FeatureId,
    b : @collision.FeatureId,
  ) -> Bool {
    match (a, b) {
      (Unknown, Unknown) => true
      (Vertex(i), Vertex(j)) => i == j
      (Edge(i), Edge(j)) => i == j
      (Face(i), Face(j)) => i == j
      _ => false
    }
  }

  // Reset impulses so removed contacts don't keep stale values.
  for ci in 0.. @collision.CollisionEventFlags {
  let mut flags = @collision.CollisionEventFlags::empty()
  let mut sensor = false
  if colliders.get(h1) is Some(c1) {
    sensor = sensor || c1.is_sensor()
  } else {
    flags = flags.insert(@collision.CollisionEventFlags::removed())
  }
  if colliders.get(h2) is Some(c2) {
    sensor = sensor || c2.is_sensor()
  } else {
    flags = flags.insert(@collision.CollisionEventFlags::removed())
  }
  if sensor {
    flags = flags.insert(@collision.CollisionEventFlags::sensor())
  }
  flags
}

///|
fn should_emit_collision_events(
  colliders : @collision.ColliderSet,
  h1 : @collision.ColliderHandle,
  h2 : @collision.ColliderHandle,
) -> Bool {
  fn has_collision_events(collider : @collision.Collider) -> Bool {
    collider
    .active_events()
    .contains(@collision.ActiveEvents::collision_events())
  }

  let mut enabled = false
  if colliders.get(h1) is Some(c1) {
    enabled = enabled || has_collision_events(c1)
  }
  if colliders.get(h2) is Some(c2) {
    enabled = enabled || has_collision_events(c2)
  }
  enabled
}

///|
fn should_emit_intersection_events(
  colliders : @collision.ColliderSet,
  h1 : @collision.ColliderHandle,
  h2 : @collision.ColliderHandle,
) -> Bool {
  fn has_intersection_events(collider : @collision.Collider) -> Bool {
    collider
    .active_events()
    .contains(@collision.ActiveEvents::intersection_events())
  }

  let mut enabled = false
  if colliders.get(h1) is Some(c1) {
    enabled = enabled || has_intersection_events(c1)
  }
  if colliders.get(h2) is Some(c2) {
    enabled = enabled || has_intersection_events(c2)
  }
  enabled
}

///|
fn collect_current_collisions(
  narrow_phase : @collision.NarrowPhase,
  colliders : @collision.ColliderSet,
) -> Array[
  (
    @collision.ColliderHandle,
    @collision.ColliderHandle,
    @collision.CollisionEventFlags,
    Bool,
  ),
] {
  let result : Array[
    (
      @collision.ColliderHandle,
      @collision.ColliderHandle,
      @collision.CollisionEventFlags,
      Bool,
    ),
  ] = []
  let intersections = narrow_phase.intersection_pairs()
  for i in 0.. Array[(@collision.ColliderHandle, @collision.ColliderHandle, Bool)] {
  let result : Array[
    (@collision.ColliderHandle, @collision.ColliderHandle, Bool),
  ] = []
  let intersections = narrow_phase.intersection_pairs()
  for i in 0.. Unit {
  let current = collect_current_collisions(narrow_phase, colliders)
  let prev_pairs : Array[(@collision.ColliderHandle, @collision.ColliderHandle)] = []
  for i in 0.. Unit {
  let current = collect_current_intersections(narrow_phase, colliders)
  let prev_pairs : Array[(@collision.ColliderHandle, @collision.ColliderHandle)] = []
  for i in 0.. Unit {
  let wake_from_impulse = impulse_joints.take_wake_up()
  for i in 0.. 0
  if !ccd_is_enabled {
    remaining_substeps = 1
  }
  let min_ccd_dt = parameters.min_ccd_dt
  let collision_pipeline = CollisionPipeline()
  while remaining_substeps > 0 && remaining_time > 0.0F {
    let mut dt = remaining_time
    if ccd_is_enabled && remaining_substeps > 1 {
      // Take forces (gravity) into account for the initial TOI estimate.
      // NOTE: CCD activation uses `RigidBodyForces.force/torque`, so compute them first.
      for i in 0.. ignore
          }
        }
      }
      let ccd_active = ccd_solver.update_ccd_active_flags(
        islands, bodies, remaining_time, true,
      )
      let first_impact = if ccd_active {
        ccd_solver.find_first_impact(
          remaining_time, islands, bodies, colliders, broad_phase, narrow_phase,
        )
      } else {
        None
      }
      if first_impact is Some(toi) {
        let steps_real = Float::from_double(remaining_substeps.to_double())
        let original_interval = remaining_time / steps_real
        dt = if toi < original_interval {
          original_interval
        } else {
          toi + (remaining_time - toi) / steps_real
        }
        remaining_substeps = remaining_substeps - 1
      } else {
        dt = remaining_time
        remaining_substeps = 0
      }
      remaining_time = remaining_time - dt
      if remaining_time <= min_ccd_dt {
        dt = dt + remaining_time
        remaining_time = 0.0F
        remaining_substeps = 0
      }
    } else {
      dt = remaining_time
      remaining_time = 0.0F
      remaining_substeps = 0
    }
    // Compute contacts based on the current (start-of-substep) poses.
    collision_pipeline.step(
      prediction_distance, islands, broad_phase, narrow_phase, bodies, colliders,
      impulse_joints, multibody_joints, physics_hooks, event_handler,
    )
    for i in 0.. body.update_effective_force_and_torque(gravity) |> ignore
          KinematicPositionBased => {
            // Rapier parity: derive velocities from position -> next_position using the
            // current substep dt.
            let inv_dt = 1.0F / dt
            let derived_vels = @dynamics.RigidBodyPosition(
              body.position(),
              body.next_position(),
            ).interpolate_velocity(inv_dt, body.center_of_mass())
            body.set_kinematic_position_based_vels(derived_vels) |> ignore
          }
          KinematicVelocityBased =>
            // Velocity-based kinematics will be integrated into `next_position` after the solver.
            ()
          _ => ()
        }
      }
    }
    let island_solver = IslandSolver()
    // The per-pipeline impulse cache is used for warmstarting across frames. When solving
    // per-island we need to rebuild it incrementally; keep a snapshot of the previous cache.
    let contact_cache_prev : Array[ContactImpulseCacheEntry] = []
    for i in 0.. ignore
        }
      }
    }

    // Update narrow-phase based on the final poses (needed for the next substep and for
    // the final collision events / island update at the end of this timestep).
    collision_pipeline.step(
      prediction_distance, islands, broad_phase, narrow_phase, bodies, colliders,
      impulse_joints, multibody_joints, physics_hooks, event_handler,
    )
  }
  // Only emit collision events after the final collision detection pass.
  populate_contact_impulses_from_cache(narrow_phase, self.contact_solver_cache)
  PhysicsPipeline::emit_collision_events(
    self, bodies, colliders, narrow_phase, event_handler,
  )
  PhysicsPipeline::emit_intersection_events(
    self, bodies, colliders, narrow_phase, event_handler,
  )
  let contacts = @dynamics.ContactGraph()
  for i in 0.. Unit {
  populate_contact_impulses_from_cache(narrow_phase, cache)
}

///|
fn PhysicsPipeline::emit_collision_events3(
  self : PhysicsPipeline,
  bodies : @dynamics.RigidBodySet3,
  colliders : @collision.ColliderSet3,
  narrow_phase : @collision.NarrowPhase,
  event_handler : EventHandler,
) -> Unit {
  PhysicsPipeline::emit_collision_events(
    self,
    bodies.inner,
    colliders.inner,
    narrow_phase,
    event_handler,
  )
}

///|
fn PhysicsPipeline::emit_intersection_events3(
  self : PhysicsPipeline,
  bodies : @dynamics.RigidBodySet3,
  colliders : @collision.ColliderSet3,
  narrow_phase : @collision.NarrowPhase,
  event_handler : EventHandler,
) -> Unit {
  PhysicsPipeline::emit_intersection_events(
    self,
    bodies.inner,
    colliders.inner,
    narrow_phase,
    event_handler,
  )
}