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

///|
/// Rope joint constraint for the real dim3 backend.
///
/// This only enforces a maximum distance between the bodies' centers of mass.

///|
pub struct RopeJoint3DReal {
  body1 : RigidBodyHandle
  body2 : RigidBodyHandle
  max_length : @core.Real
}

///|
pub struct RopeJointSet3DReal {
  joints : Array[RopeJoint3DReal]
}

///|
pub fn RopeJointSet3DReal::RopeJointSet3DReal() -> RopeJointSet3DReal {
  { joints: [] }
}

///|
pub fn RopeJointSet3DReal::insert(
  self : RopeJointSet3DReal,
  body1 : RigidBodyHandle,
  body2 : RigidBodyHandle,
  max_length : @core.Real,
) -> Unit {
  self.joints.push({ body1, body2, max_length })
}

///|
pub fn RopeJointSet3DReal::interaction_pairs(
  self : RopeJointSet3DReal,
) -> Array[(RigidBodyHandle, RigidBodyHandle)] {
  let out : Array[(RigidBodyHandle, RigidBodyHandle)] = []
  for j in self.joints {
    out.push((j.body1, j.body2))
  }
  out
}

///|
fn rope_joint_in_island(
  joint : RopeJoint3DReal,
  island_stamps : Array[Int]?,
  island_stamp : Int,
) -> Bool {
  if island_stamps is None {
    return true
  }
  if island_stamp <= 0 {
    return false
  }
  if island_stamps is Some(stamps) {
    let id1 = joint.body1.id
    let id2 = joint.body2.id
    let s1 = if id1 >= 0 && id1 < stamps.length() { stamps[id1] } else { 0 }
    let s2 = if id2 >= 0 && id2 < stamps.length() { stamps[id2] } else { 0 }
    if s1 > 0 && s2 > 0 && s1 != s2 {
      return false
    }
    return s1 == island_stamp || s2 == island_stamp
  }
  false
}

///|
pub fn RopeJointSet3DReal::solve(
  self : RopeJointSet3DReal,
  bodies : RigidBodySet3D,
  dt : @core.Real,
) -> Unit {
  RopeJointSet3DReal::solve_with_island_stamp(self, bodies, dt, None, 0)
}

///|
pub fn RopeJointSet3DReal::solve_with_island_stamp(
  self : RopeJointSet3DReal,
  bodies : RigidBodySet3D,
  dt : @core.Real,
  island_stamps : Array[Int]?,
  island_stamp : Int,
) -> Unit {
  dt |> ignore
  for i in 0.. 0.0F { inv_m1 / inv_sum } else { 0.0F }
      let w2 = if inv_m2 > 0.0F { inv_m2 / inv_sum } else { 0.0F }

      // Position correction.
      let corr = dir.scale(err)
      if dyn1 {
        b1.set_translation(b1.translation().add(corr.scale(w1)))
        b1.wake_up()
      }
      if dyn2 {
        b2.set_translation(b2.translation().sub(corr.scale(w2)))
        b2.wake_up()
      }

      // Velocity correction: prevent further separation along the rope direction.
      let rel = b2.linvel().sub(b1.linvel()).dot(dir)
      if rel > 0.0F {
        if dyn1 {
          b1.set_linvel(b1.linvel().add(dir.scale(rel * w1)))
        }
        if dyn2 {
          b2.set_linvel(b2.linvel().sub(dir.scale(rel * w2)))
        }
      }
    }
  }
}