#include "CRagDoll.hpp"
#include "Collision/CMetroidAreaCollider.hpp"
#include "Collision/CGameCollision.hpp"
#include "Collision/CMaterialFilter.hpp"
#include "Collision/CCollidableSphere.hpp"
#include "Collision/CCollisionInfo.hpp"
#include "CStateManager.hpp"

namespace urde {

void CRagDoll::CRagDollLengthConstraint::Update() {
  zeus::CVector3f delta = x4_p2->x4_curPos - x0_p1->x4_curPos;
  float magSq = delta.magSquared();
  float lenSq = x8_length * x8_length;
  bool doSolve = true;
  switch (xc_ineqType) {
  case 1: // Min
    doSolve = magSq < lenSq;
    break;
  case 2: // Max
    doSolve = magSq > lenSq;
    break;
  default:
    break;
  }
  if (!doSolve)
    return;
  zeus::CVector3f solveVec = delta * (lenSq / (magSq + lenSq) - 0.5f);
  x0_p1->x4_curPos -= solveVec;
  x4_p2->x4_curPos += solveVec;
}

void CRagDoll::CRagDollJointConstraint::Update() {
  // L_hip, R_shoulder, L_shoulder, L_hip, L_knee, L_ankle
  zeus::CVector3f P4ToP5 = x10_p5->x4_curPos - xc_p4->x4_curPos; // L_hip->L_knee
  zeus::CVector3f cross =
      P4ToP5.cross((x8_p3->x4_curPos - x0_p1->x4_curPos).cross(x4_p2->x4_curPos - x0_p1->x4_curPos));
  // L_hip->L_knee X (L_hip->L_shoulder X L_hip->R_shoulder)
  if (cross.canBeNormalized()) {
    zeus::CVector3f hipUp = cross.cross(P4ToP5).normalized();
    float dot = (x14_p6->x4_curPos - x10_p5->x4_curPos).dot(hipUp);
    if (dot > 0.f) {
      zeus::CVector3f solveVec = 0.5f * dot * hipUp;
      x14_p6->x4_curPos -= solveVec;
      x10_p5->x4_curPos += solveVec;
    }
  }
}

void CRagDoll::CRagDollPlaneConstraint::Update() {
  zeus::CVector3f P1ToP2 = (x4_p2->x4_curPos - x0_p1->x4_curPos).normalized();
  float dot = P1ToP2.dot(xc_p4->x4_curPos - x8_p3->x4_curPos);
  if (dot < 0.f) {
    zeus::CVector3f solveVec = 0.5f * dot * P1ToP2;
    xc_p4->x4_curPos -= solveVec;
    x10_p5->x4_curPos += solveVec;
  }
}

CRagDoll::CRagDoll(float normalGravity, float floatingGravity, float overTime, u32 flags)
: x44_normalGravity(normalGravity), x48_floatingGravity(floatingGravity), x50_overTimer(overTime) {
  x68_27_continueSmallMovements = bool(flags & 0x1);
  x68_28_noOverTimer = bool(flags & 0x2);
  x68_29_noAiCollision = bool(flags & 0x4);
}

void CRagDoll::AccumulateForces(float dt, float waterTop) {
  float fps = 1.f / dt;
  x64_angTimer += dt;
  if (x64_angTimer > 4.f)
    x64_angTimer -= 4.f;
  float targetZ = std::sin(zeus::degToRad(90.f) * x64_angTimer) * 0.1f + (waterTop - 0.2f);
  zeus::CVector3f centerOfVolume;
  float totalVolume = 0.f;
  for (auto& particle : x4_particles) {
    float volume = particle.x10_radius * particle.x10_radius * particle.x10_radius;
    totalVolume += volume;
    centerOfVolume += particle.x4_curPos * volume;
    float fromTargetZ = particle.x4_curPos.z() - targetZ;
    float verticalAcc = x48_floatingGravity;
    float termVelCoefficient = 0.f;
    if (std::fabs(fromTargetZ) < 0.5f) {
      termVelCoefficient = 0.5f * fromTargetZ / 0.5f + 0.5f;
      verticalAcc = x48_floatingGravity * -fromTargetZ / 0.5f;
    } else if (fromTargetZ > 0.f) {
      verticalAcc = x44_normalGravity;
      termVelCoefficient = 1.f;
    }
    particle.x20_velocity.z() += verticalAcc;
    zeus::CVector3f vel = (particle.x4_curPos - particle.x14_prevPos) * fps;
    float velMag = vel.magnitude();
    if (velMag > FLT_EPSILON) {
      particle.x20_velocity -=
          vel * (1.f / velMag) *
          ((velMag * velMag * 0.75f * (1.2f * termVelCoefficient + 1000.f * (1.f - termVelCoefficient))) /
           (8000.f * particle.x10_radius));
    }
  }
  zeus::CVector3f averageTorque;
  centerOfVolume = centerOfVolume / totalVolume;
  for (const auto& particle : x4_particles) {
    float volume = particle.x10_radius * particle.x10_radius * particle.x10_radius;
    averageTorque += (particle.x4_curPos - centerOfVolume).cross(particle.x4_curPos - particle.x14_prevPos) * volume;
  }
  averageTorque = averageTorque * (fps / totalVolume);
  if (averageTorque.canBeNormalized())
    for (auto& particle : x4_particles)
      particle.x20_velocity -= averageTorque.cross(particle.x4_curPos - centerOfVolume) * 25.f;
}

void CRagDoll::AddParticle(CSegId id, const zeus::CVector3f& prevPos, const zeus::CVector3f& curPos, float radius) {
  x4_particles.emplace_back(id, curPos, radius, prevPos);
}

void CRagDoll::AddLengthConstraint(int i1, int i2) {
  x14_lengthConstraints.emplace_back(&x4_particles[i1], &x4_particles[i2],
                                     (x4_particles[i1].x4_curPos - x4_particles[i2].x4_curPos).magnitude(), 0);
}

void CRagDoll::AddMaxLengthConstraint(int i1, int i2, float length) {
  x14_lengthConstraints.emplace_back(&x4_particles[i1], &x4_particles[i2], length, 2);
}

void CRagDoll::AddMinLengthConstraint(int i1, int i2, float length) {
  x14_lengthConstraints.emplace_back(&x4_particles[i1], &x4_particles[i2], length, 1);
}

void CRagDoll::AddJointConstraint(int i1, int i2, int i3, int i4, int i5, int i6) {
  x24_jointConstraints.emplace_back(&x4_particles[i1], &x4_particles[i2], &x4_particles[i3], &x4_particles[i4],
                                    &x4_particles[i5], &x4_particles[i6]);
}

zeus::CQuaternion CRagDoll::BoneAlign(CHierarchyPoseBuilder& pb, const CCharLayoutInfo& charInfo, int i1, int i2,
                                      const zeus::CQuaternion& q) {
  zeus::CVector3f fromParent = charInfo.GetFromParentUnrotated(x4_particles[i2].x0_id);
  zeus::CVector3f delta = x4_particles[i2].x4_curPos - x4_particles[i1].x4_curPos;
  delta = q.inverse().transform(delta);
  zeus::CQuaternion ret = zeus::CQuaternion::shortestRotationArc(fromParent, delta);
  pb.GetTreeMap()[x4_particles[i1].x0_id].x4_rotation = ret;
  return ret;
}

zeus::CAABox CRagDoll::CalculateRenderBounds() const {
  zeus::CAABox aabb;
  for (const auto& particle : x4_particles) {
    aabb.accumulateBounds(
        zeus::CAABox(particle.x4_curPos - particle.x10_radius, particle.x4_curPos + particle.x10_radius));
  }
  return aabb;
}

void CRagDoll::CheckStatic(float dt) {
  x4c_impactCount = 0;
  x54_impactVel = 0.f;
  float halfDt = 0.5f * dt;
  float halfDeltaUnitSq = halfDt * halfDt;
  x58_averageVel = zeus::CVector3f::skZero;
  bool movingSlowly = true;
  for (auto& particle : x4_particles) {
    zeus::CVector3f delta = particle.x4_curPos - particle.x14_prevPos;
    x58_averageVel += delta;
    if (delta.magSquared() > halfDeltaUnitSq)
      movingSlowly = false;
    if (particle.x3c_24_impactPending) {
      x4c_impactCount += 1;
      x54_impactVel = std::max(particle.x38_impactFrameVel, x54_impactVel);
    }
  }
  if (!x4_particles.empty())
    x58_averageVel = x58_averageVel * (1.f / (dt * x4_particles.size()));
  x54_impactVel /= dt;
  if (!x68_28_noOverTimer) {
    x50_overTimer -= dt;
    if (x50_overTimer <= 0.f)
      x68_25_over = true;
  }
  if (movingSlowly && x68_24_prevMovingSlowly)
    x68_25_over = true;
  x68_24_prevMovingSlowly = movingSlowly;
}

void CRagDoll::ClearForces() {
  for (auto& particle : x4_particles)
    particle.x20_velocity = zeus::CVector3f::skZero;
}

void CRagDoll::SatisfyConstraints(CStateManager& mgr) {
  for (auto& length : x14_lengthConstraints)
    length.Update();
  for (auto& joint : x24_jointConstraints)
    joint.Update();
  for (auto& plane : x34_planeConstraints)
    plane.Update();
  if (SatisfyWorldConstraints(mgr, 1))
    SatisfyWorldConstraints(mgr, 2);
}

bool CRagDoll::SatisfyWorldConstraints(CStateManager& mgr, int pass) {
  zeus::CAABox aabb;
  for (const auto& particle : x4_particles) {
    if (pass == 1 || particle.x3c_24_impactPending) {
      aabb.accumulateBounds(particle.x14_prevPos - particle.x10_radius);
      aabb.accumulateBounds(particle.x14_prevPos + particle.x10_radius);
      aabb.accumulateBounds(particle.x4_curPos - particle.x10_radius);
      aabb.accumulateBounds(particle.x4_curPos + particle.x10_radius);
    }
  }

  CAreaCollisionCache ccache(aabb);
  CGameCollision::BuildAreaCollisionCache(mgr, ccache);
  bool needs2ndPass = false;

  TUniqueId bestId = kInvalidUniqueId;
  CMaterialList include;
  if (x68_29_noAiCollision)
    include = CMaterialList(EMaterialTypes::Solid);
  else
    include = CMaterialList(EMaterialTypes::Solid, EMaterialTypes::AIBlock);

  CMaterialList exclude;
  if (x68_29_noAiCollision)
    exclude = CMaterialList(EMaterialTypes::Character, EMaterialTypes::Player, EMaterialTypes::AIBlock,
                            EMaterialTypes::Occluder);
  else
    exclude = CMaterialList(EMaterialTypes::Character, EMaterialTypes::Player);

  rstl::reserved_vector<TUniqueId, 1024> nearList;
  CMaterialFilter filter = CMaterialFilter::MakeIncludeExclude(include, exclude);
  mgr.BuildNearList(nearList, aabb, filter, nullptr);

  for (auto& particle : x4_particles) {
    if (pass == 1 || particle.x3c_24_impactPending) {
      zeus::CVector3f delta = particle.x4_curPos - particle.x14_prevPos;
      float deltaMag = delta.magnitude();
      if (deltaMag > 0.0001f) {
        delta = delta * (1.f / deltaMag);
        double d = deltaMag;
        CCollidableSphere sphere(zeus::CSphere(particle.x14_prevPos, particle.x10_radius), include);
        CCollisionInfo info;
        CGameCollision::DetectCollision_Cached_Moving(mgr, ccache, sphere, {}, filter, nearList, delta, bestId, info,
                                                      d);
        if (info.IsValid()) {
          needs2ndPass = true;
          switch (pass) {
          case 1: {
            particle.x3c_24_impactPending = true;
            float dot = delta.dot(info.GetNormalLeft());
            particle.x2c_impactResponseDelta = -0.125f * dot * deltaMag * info.GetNormalLeft();
            particle.x38_impactFrameVel = -dot * deltaMag;
            particle.x4_curPos += (0.0001f - (deltaMag - float(d)) * dot) * info.GetNormalLeft();
            break;
          }
          case 2:
            particle.x4_curPos = float(d - 0.0001) * delta + particle.x14_prevPos;
            break;
          default:
            break;
          }
        }
      } else if (!x68_27_continueSmallMovements) {
        particle.x4_curPos = particle.x14_prevPos;
      }
    }
  }

  return needs2ndPass;
}

void CRagDoll::SatisfyWorldConstraintsOnConstruction(CStateManager& mgr) {
  for (auto& particle : x4_particles)
    particle.x3c_24_impactPending = true;
  SatisfyWorldConstraints(mgr, 2);
  for (auto& particle : x4_particles)
    particle.x14_prevPos = particle.x4_curPos;
}

void CRagDoll::Verlet(float dt) {
  for (auto& particle : x4_particles) {
    zeus::CVector3f oldPos = particle.x4_curPos;
    particle.x4_curPos += (particle.x4_curPos - particle.x14_prevPos) * (particle.x3c_24_impactPending ? 0.9f : 1.f);
    particle.x4_curPos += particle.x20_velocity * (dt * dt);
    particle.x4_curPos += particle.x2c_impactResponseDelta;
    particle.x14_prevPos = oldPos;
    zeus::CVector3f deltaPos = particle.x4_curPos - particle.x14_prevPos;
    if (deltaPos.magSquared() > 4.f)
      particle.x4_curPos = deltaPos.normalized() * 2.f + particle.x14_prevPos;
    particle.x3c_24_impactPending = false;
    particle.x2c_impactResponseDelta = zeus::CVector3f::skZero;
  }
}

void CRagDoll::PreRender(const zeus::CVector3f& v, CModelData& mData) {
  // Empty
}

void CRagDoll::Update(CStateManager& mgr, float dt, float waterTop) {
  if (!x68_25_over || x68_27_continueSmallMovements) {
    AccumulateForces(dt, waterTop);
    Verlet(dt);
    SatisfyConstraints(mgr);
    ClearForces();
    CheckStatic(dt);
  }
}

void CRagDoll::Prime(CStateManager& mgr, const zeus::CTransform& xf, CModelData& mData) {
  zeus::CVector3f scale = mData.GetScale();
  CAnimData* aData = mData.AnimationData();
  aData->BuildPose();
  for (auto& particle : x4_particles)
    if (particle.x0_id != 0xff)
      particle.x4_curPos = xf * (aData->GetPose().GetOffset(particle.x0_id) * scale);
  SatisfyWorldConstraints(mgr, 2);
  for (auto& particle : x4_particles)
    particle.x3c_24_impactPending = false;
  x68_26_primed = true;
}

} // namespace urde