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

///|
/// Internal solver-side views of rigid bodies.
///
/// Ported from `rapier-reference/src/dynamics/solver/solver_body.rs` (scalar, non-SIMD path).
///
/// Note: in Rapier these types are public. We keep them public as well to satisfy
/// crate-level pub parity, even though most users shouldn't need them directly.

///|
pub struct SolverVel {
  mut linear : @core.Vec2
  mut angular : @core.Real
}

///|
pub fn SolverVel::zero() -> SolverVel {
  { linear: @core.Vec2::zero(), angular: 0.0F }
}

///|
pub struct SolverPose {
  /// World-space translation of the rigid-body center of mass.
  mut translation : @core.Vec2
  /// World-space rotation.
  mut rotation : @core.Rot2
  /// Effective inverse mass (per-axis for dim2).
  mut im : @core.Vec2
  /// Effective world-space inverse inertia (scalar for dim2).
  mut ii : @core.Real
  /// Extra scalar used by Rapier's dim2 SIMD-friendly layout.
  padding : @core.Real
}

///|
pub fn SolverPose::zero() -> SolverPose {
  {
    translation: @core.Vec2::zero(),
    rotation: @core.Rot2::identity(),
    im: @core.Vec2::zero(),
    ii: 0.0F,
    padding: 0.0F,
  }
}

///|
pub struct SolverBodies {
  vels : Array[SolverVel]
  poses : Array[SolverPose]
}

///|
pub fn SolverBodies::default() -> SolverBodies {
  { vels: [], poses: [] }
}

///|
pub fn SolverBodies::clear(self : SolverBodies) -> Unit {
  self.vels.clear()
  self.poses.clear()
}

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

///|
pub fn SolverBodies::resize(self : SolverBodies, sz : Int) -> Unit {
  let target = if sz < 0 { 0 } else { sz }
  self.vels.clear()
  self.poses.clear()
  for _ in 0.. Unit {
  let n = self.vels.length()
  for i in 0.. true
          _ => false
        }
        body.set_linvel(v.linear, wake) |> ignore
        body.set_angvel(v.angular, wake) |> ignore
      }
    }
  }
}

///|
/// Copy the current rigid-body velocity/pose into this solver slot.
pub fn SolverBodies::copy_from(
  self : SolverBodies,
  i : Int,
  rb : @dynamics.RigidBody,
) -> Unit {
  if i < 0 || i >= self.len() {
    return
  }
  let v = self.vels[i]
  let p = self.poses[i]
  v.angular = rb.angvel()
  v.linear = rb.linvel()
  p.rotation = rb.rotation()
  p.translation = rb.world_com()
  if rb.is_dynamic_or_kinematic() {
    let mprops = rb.mass_props()
    p.im = mprops.effective_inv_mass
    p.ii = mprops.effective_world_inv_inertia
  } else {
    p.im = @core.Vec2::zero()
    p.ii = 0.0F
  }
  self.vels[i] = v
  self.poses[i] = p
}