/*
 * Copyright (C) 2020 Open Source Robotics Foundation
 *
 * 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.
 *
*/
#ifndef GZ_MATH_DETAIL_CAPSULE_HH_
#define GZ_MATH_DETAIL_CAPSULE_HH_

#include <algorithm>
#include <cmath>
#include <limits>
#include <optional>
#include <gz/math/Capsule.hh>
#include <gz/math/Cylinder.hh>
#include <gz/math/Helpers.hh>
#include <gz/math/Inertial.hh>
#include <gz/math/Sphere.hh>

namespace gz::math
{
//////////////////////////////////////////////////
template<typename T>
Capsule<T>::Capsule(const T _length, const T _radius)
{
  this->length = _length;
  this->radius = _radius;
}

//////////////////////////////////////////////////
template<typename T>
Capsule<T>::Capsule(const T _length, const T _radius, const Material &_mat)
{
  this->length = _length;
  this->radius = _radius;
  this->material = _mat;
}

//////////////////////////////////////////////////
template<typename T>
T Capsule<T>::Radius() const
{
  return this->radius;
}

//////////////////////////////////////////////////
template<typename T>
void Capsule<T>::SetRadius(const T _radius)
{
  this->radius = _radius;
}

//////////////////////////////////////////////////
template<typename T>
T Capsule<T>::Length() const
{
  return this->length;
}

//////////////////////////////////////////////////
template<typename T>
void Capsule<T>::SetLength(const T _length)
{
  this->length = _length;
}

//////////////////////////////////////////////////
template<typename T>
const Material &Capsule<T>::Mat() const
{
  return this->material;
}

//////////////////////////////////////////////////
template<typename T>
void Capsule<T>::SetMat(const Material &_mat)
{
  this->material = _mat;
}

//////////////////////////////////////////////////
template<typename T>
bool Capsule<T>::operator==(const Capsule &_capsule) const
{
  return equal(this->radius, _capsule.Radius()) &&
    equal(this->length, _capsule.Length()) &&
    this->material == _capsule.Mat();
}

//////////////////////////////////////////////////
template<typename T>
std::optional< MassMatrix3<T> > Capsule<T>::MassMatrix() const
{
  // mass and moment of inertia of cylinder about centroid
  MassMatrix3<T> cylinder;
  cylinder.SetFromCylinderZ(this->material, this->length, this->radius);

  // mass and moment of inertia of hemisphere about centroid
  const T r2 = this->radius * this->radius;
  const T hemisphereMass = this->material.Density() *
      2. / 3. * GZ_PI * r2 * this->radius;
  // efunda.com/math/solids/solids_display.cfm?SolidName=Hemisphere
  const T ixx = 83. / 320. * hemisphereMass * r2;
  const T izz = 2. / 5.  * hemisphereMass * r2;
  MassMatrix3<T> hemisphere(hemisphereMass, Vector3<T>(ixx, ixx, izz),
                            Vector3<T>::Zero);;

  // Distance from centroid of cylinder to centroid of hemisphere,
  // since centroid of hemisphere is 3/8 radius from its flat base
  const T dz = this->length / 2. + this->radius * 3. / 8.;
  Inertial<T> upperCap(hemisphere, Pose3<T>(0, 0, dz, 0, 0, 0));
  Inertial<T> lowerCap(hemisphere, Pose3<T>(0, 0, -dz, 0, 0, 0));

  // Use Inertial class to add MassMatrix3 objects at different poses
  Inertial<T> capsule =
      Inertial<T>(cylinder, Pose3<T>::Zero) + upperCap + lowerCap;

  if (!capsule.MassMatrix().IsValid())
  {
    return std::nullopt;
  }

  return std::make_optional(capsule.MassMatrix());
}

//////////////////////////////////////////////////
template<typename T>
T Capsule<T>::Volume() const
{
  return GZ_PI * std::pow(this->radius, 2) *
         (this->length + 4. / 3. * this->radius);
}

//////////////////////////////////////////////////
template<typename T>
T Capsule<T>::VolumeBelow(const Plane<T> &_plane) const
{
  auto r = this->radius;
  auto halfLen = this->length / 2;

  if (r <= 0 || this->length <= 0)
    return 0;

  // Decompose into: bottom hemisphere at z = -halfLen,
  // cylinder from -halfLen to +halfLen, top hemisphere at z = +halfLen.
  // Translate the plane to each sub-shape's local frame.

  auto n = _plane.Normal();
  auto d = _plane.Offset();

  // Decompose into cylinder middle + bottom hemisphere + top hemisphere.
  // For a sub-shape centered at (0,0,zOff), the plane in its local
  // frame has the same normal but offset d' = d - n.Z()*zOff.

  Sphere<T> sphere(r);
  Cylinder<T> cyl(this->length, r);
  T vCyl = cyl.VolumeBelow(_plane);

  T halfSphereVol = sphere.Volume() / 2;

  // Bottom hemisphere: center at (0,0,-halfLen), hemisphere is z <= 0 local.
  // Translate plane to sphere's local frame.
  Plane<T> planeBot(n, d + n.Z() * halfLen);
  T vSphereBot = sphere.VolumeBelow(planeBot);
  // The hemisphere is {z <= 0}. The sphere volume below cutting plane
  // that lies within this hemisphere is min(vSphereBot, halfSphereVol).
  T vBot = std::min(vSphereBot, halfSphereVol);

  // Top hemisphere: center at (0,0,+halfLen), hemisphere is z >= 0 local.
  Plane<T> planeTop(n, d - n.Z() * halfLen);
  T vSphereTop = sphere.VolumeBelow(planeTop);
  // Volume in top hemisphere = volume below plane minus bottom hemisphere
  T vTop = std::max(static_cast<T>(0), vSphereTop - halfSphereVol);

  return vCyl + vBot + vTop;
}

//////////////////////////////////////////////////
template<typename T>
std::optional<Vector3<T>>
 Capsule<T>::CenterOfVolumeBelow(const Plane<T> &_plane) const
{
  auto r = this->radius;
  auto halfLen = this->length / 2;

  if (r <= 0 || this->length <= 0)
    return std::nullopt;

  auto n = _plane.Normal();
  auto d = _plane.Offset();

  Sphere<T> sphere(r);
  Cylinder<T> cyl(this->length, r);
  T halfSphereVol = sphere.Volume() / 2;

  // Cylinder contribution
  T vCyl = cyl.VolumeBelow(_plane);
  auto covCyl = cyl.CenterOfVolumeBelow(_plane);

  // Bottom hemisphere: center at (0,0,-halfLen)
  Plane<T> planeBot(n, d + n.Z() * halfLen);
  T vSphereBot = sphere.VolumeBelow(planeBot);
  T vBot = std::min(vSphereBot, halfSphereVol);

  // Top hemisphere: center at (0,0,+halfLen)
  Plane<T> planeTop(n, d - n.Z() * halfLen);
  T vSphereTop = sphere.VolumeBelow(planeTop);
  T vTop = std::max(static_cast<T>(0), vSphereTop - halfSphereVol);

  T totalVol = vCyl + vBot + vTop;
  if (totalVol <= 0)
    return std::nullopt;

  Vector3<T> moment(0, 0, 0);

  // Cylinder moment
  if (covCyl.has_value())
    moment += (*covCyl) * vCyl;

  // Bottom hemisphere moment
  // We need the centroid of {sphere below cutting plane} intersect {z <= 0}
  // in the sphere's local frame, then shift by (0,0,-halfLen).
  if (vBot > 0)
  {
    Vector3<T> botCenter(0, 0, -halfLen);
    if (vBot < 1e-12 * totalVol)
    {
      // Tiny slice: approximate centroid at hemisphere center to avoid
      // catastrophic cancellation.
      moment += botCenter * vBot;
    }
    else if (vBot >= halfSphereVol - 1e-15 * sphere.Volume())
    {
      // Entire hemisphere is below cutting plane
      // Centroid of hemisphere (z <= 0) of sphere of radius r is at
      // z = -3r/8
      moment += (botCenter + Vector3<T>(0, 0, -3 * r / 8)) * vBot;
    }
    else
    {
      // Cutting plane intersects the hemisphere below the equator.
      // The entire sphere volume below the cutting plane is within
      // the hemisphere (vSphereBot <= halfSphereVol).
      auto covSphBot = sphere.CenterOfVolumeBelow(planeBot);
      if (covSphBot.has_value())
        moment += (botCenter + *covSphBot) * vBot;
    }
  }

  // Top hemisphere moment
  if (vTop > 0)
  {
    Vector3<T> topCenter(0, 0, halfLen);
    if (vTop < 1e-12 * totalVol)
    {
      // Tiny slice: approximate centroid at hemisphere center to avoid
      // catastrophic cancellation in the subtraction formula.
      moment += topCenter * vTop;
    }
    else if (vTop >= halfSphereVol - 1e-15 * sphere.Volume())
    {
      // Entire top hemisphere is below cutting plane
      // Centroid of hemisphere (z >= 0) at z = +3r/8
      moment += (topCenter + Vector3<T>(0, 0, 3 * r / 8)) * vTop;
    }
    else
    {
      // Cutting plane intersects the top hemisphere.
      // Volume above equator and below cutting plane:
      // vTop = vSphereTop - halfSphereVol
      // This is the "annular" cap between the equator and the cutting plane.
      // Centroid = (vSphereTop * covSphTop - halfSphereVol * covEquator)
      //            / vTop
      auto covSphTop = sphere.CenterOfVolumeBelow(planeTop);
      // Centroid of bottom hemisphere (below equator) is at (0,0,-3r/8)
      Vector3<T> covEquator(0, 0, -3 * r / 8);
      if (covSphTop.has_value())
      {
        auto annularMoment = (*covSphTop) * vSphereTop
                           - covEquator * halfSphereVol;
        moment += (topCenter * vTop + annularMoment);
      }
    }
  }

  return moment / totalVol;
}

//////////////////////////////////////////////////
template<typename T>
bool Capsule<T>::SetDensityFromMass(const T _mass)
{
  T newDensity = this->DensityFromMass(_mass);
  if (isnan(newDensity))
    return false;

  this->material.SetDensity(newDensity);
  return true;
}

//////////////////////////////////////////////////
template<typename T>
T Capsule<T>::DensityFromMass(const T _mass) const
{
  if (this->radius <= 0 || this->length <=0 || _mass <= 0)
    return std::numeric_limits<T>::quiet_NaN();

  return _mass / this->Volume();
}
}  // namespace gz::math
#endif  // GZ_MATH_DETAIL_CAPSULE_HH_
