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

///|
/// 3D-flavored physics pipeline.
///
/// This keeps the dim3 entrypoint wired through native 3D sets/pipelines while reusing the
/// shared solver core.
pub struct PhysicsPipeline3 {
  inner : PhysicsPipeline
}

///|
pub fn PhysicsPipeline3::PhysicsPipeline3() -> PhysicsPipeline3 {
  { inner: PhysicsPipeline() }
}

///|
fn handle_user_changes_to_bodies3(
  islands : @dynamics.IslandManager3,
  bodies : @dynamics.RigidBodySet3,
  colliders : @collision.ColliderSet3,
  impulse_joints : @dynamics.ImpulseJointSet3,
  multibody_joints : @dynamics.MultibodyJointSet3,
  modified_bodies : Array[@dynamics.RigidBodyHandle],
) -> Unit {
  multibody_joints |> ignore
  for i in 0.. {
        parent |> ignore
        child |> ignore
        joint_handle |> ignore
        joint.set_enabled_by_attached_body(body.is_enabled()) |> ignore
      })
    }
  }
}

///|
fn update_body_mass_properties_from_colliders3(
  bodies : @dynamics.RigidBodySet3,
  colliders : @collision.ColliderSet3,
  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_bodies3(
  bodies : @dynamics.RigidBodySet3,
  modified_bodies : Array[@dynamics.RigidBodyHandle],
) -> Unit {
  for i in 0.. ignore
    }
  }
}

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

///|
pub fn PhysicsPipeline3::step(
  self : PhysicsPipeline3,
  gravity : @core.Vec3,
  parameters : @dynamics.IntegrationParameters,
  islands : @dynamics.IslandManager3,
  broad_phase : @collision.BroadPhaseBvh,
  narrow_phase : @collision.NarrowPhase,
  bodies : @dynamics.RigidBodySet3,
  colliders : @collision.ColliderSet3,
  impulse_joints : @dynamics.ImpulseJointSet3,
  multibody_joints : @dynamics.MultibodyJointSet3,
  ccd_solver : @dynamics_ccd.CCDSolver,
  hooks : PhysicsHooks,
  events : EventHandler,
) -> Unit {
  let gravity2 = @core.Vec2(gravity.x, gravity.y)
  self.inner.counters.reset()
  self.inner.counters.step_started()

  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 = CollisionPipeline3()
  while remaining_substeps > 0 && remaining_time > 0.0F {
    let mut dt = remaining_time
    if ccd_is_enabled && remaining_substeps > 1 {
      let live_bodies = bodies.iter_mut()
      for i in 0.. ignore
        }
      }
      let ccd_active = ccd_solver.update_ccd_active_flags3(
        islands, bodies, remaining_time, true,
      )
      let first_impact = if ccd_active {
        ccd_solver.find_first_impact3(
          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())
        dt = {
          let original_interval = remaining_time / steps_real
          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
    }

    collision_pipeline.step(
      prediction_distance, islands, broad_phase, narrow_phase, bodies, colliders,
      hooks, events,
    )
    let integrating_bodies = bodies.iter_mut()
    for i in 0.. {
          body.update_effective_force_and_torque(gravity2) |> ignore
          let forces = body.forces()
          let raw_vels = forces.integrate(dt, body.vels(), body.mass_props())
          let damped_vels = raw_vels.apply_damping(dt, body.damping())
          let linvel = if body.locked_translations {
            @core.Vec2::zero()
          } else {
            damped_vels.linvel
          }
          let angvel = if body.locked_rotations {
            0.0F
          } else {
            damped_vels.angvel
          }
          body.set_linvel(linvel, false) |> ignore
          body.set_angvel(angvel, false) |> ignore
        }
        KinematicPositionBased => {
          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 => ()
        _ => ()
      }
    }
    let island_solver = IslandSolver()
    let contact_cache_prev : Array[ContactImpulseCacheEntry] = []
    for i in 0.. {
          let next_pos = body
            .vels()
            .integrate(dt, body.position(), body.center_of_mass())
          body.set_next_position_internal(next_pos) |> ignore
        }
        _ => ()
      }
    }
    if ccd_is_enabled {
      let ccd_active = ccd_solver.update_ccd_active_flags3(
        islands, bodies, dt, false,
      )
      if ccd_active {
        let impacts = ccd_solver.predict_impacts_at_next_positions3(
          dt, islands, bodies, colliders, broad_phase, narrow_phase,
        )
        ccd_solver.clamp_motions3(dt, bodies, impacts)
      }
    }

    let syncing_bodies = bodies.iter_mut()
    for i in 0.. ignore
      }
    }

    collision_pipeline.step(
      prediction_distance, islands, broad_phase, narrow_phase, bodies, colliders,
      hooks, events,
    )
    let (post_constraints, post_manifolds) = build_constraints3(
      parameters,
      bodies,
      colliders,
      hooks,
      narrow_phase,
      dt,
      self.inner.contact_solver_cache,
      0.0F,
      None,
      0,
    )
    if post_constraints.length() > 0 {
      position_correction3(
        bodies,
        colliders,
        post_constraints,
        post_manifolds,
        parameters.allowed_linear_error(),
      )
      position_correction_all3(
        bodies,
        colliders,
        post_constraints,
        post_manifolds,
        parameters.allowed_linear_error(),
      )
    }
    let post_joint_constraints = build_joint_constraints3(
      parameters,
      bodies,
      impulse_joints,
      multibody_joints,
      dt,
      None,
      0,
    )
    if post_joint_constraints.length() > 0 {
      position_correction_joints3(bodies, post_joint_constraints)
    }
    collision_pipeline.step(
      prediction_distance, islands, broad_phase, narrow_phase, bodies, colliders,
      hooks, events,
    )
  }

  populate_contact_impulses_from_cache3(
    narrow_phase,
    self.inner.contact_solver_cache,
  )
  PhysicsPipeline::emit_collision_events3(
    self.inner,
    bodies,
    colliders,
    narrow_phase,
    events,
  )
  PhysicsPipeline::emit_intersection_events3(
    self.inner,
    bodies,
    colliders,
    narrow_phase,
    events,
  )
  let contacts = @dynamics.ContactGraph()
  for i in 0..