/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "dart/dynamics/Inertia.hpp"

#include "dart/common/Console.hpp"
#include "dart/math/Geometry.hpp"

namespace dart {
namespace dynamics {

//==============================================================================
Inertia::Inertia(
    double _mass,
    const Eigen::Vector3d& _com,
    const Eigen::Matrix3d& _momentOfInertia)
  : mMass(_mass), mCenterOfMass(_com)
{
  setMoment(_momentOfInertia);
}

//==============================================================================
Inertia::Inertia(const Eigen::Matrix6d& _spatialInertiaTensor)
{
  setSpatialTensor(_spatialInertiaTensor);
}

//==============================================================================
Inertia::Inertia(
    double _mass,
    double _comX,
    double _comY,
    double _comZ,
    double _Ixx,
    double _Iyy,
    double _Izz,
    double _Ixy,
    double _Ixz,
    double _Iyz)
  : mMass(_mass),
    mCenterOfMass(_comX, _comY, _comZ),
    mMoment({_Ixx, _Iyy, _Izz, _Ixy, _Ixz, _Iyz})
{
  computeSpatialTensor();
}

//==============================================================================
void Inertia::setParameter(Param _param, double _value)
{
  if (_param == MASS)
  {
    mMass = _value;
  }
  else if (_param <= COM_Z)
  {
    mCenterOfMass[_param - 1] = _value;
  }
  else if (_param <= I_YZ)
  {
    mCenterOfMass[_param - 4] = _value;
  }
  else
  {
    dtwarn << "[Inertia::setParameter] Attempting to set Param #" << _param
           << ", but inertial parameters only go up to " << I_YZ
           << ". Nothing will be set.\n";
    return;
  }

  computeSpatialTensor();
}

//==============================================================================
double Inertia::getParameter(Param _param) const
{
  if (_param == MASS)
    return mMass;
  else if (_param <= COM_Z)
    return mCenterOfMass[_param - 1];
  else if (_param <= I_YZ)
    return mMoment[_param - 4];

  dtwarn << "[Inertia::getParameter] Requested Param #" << _param
         << ", but inertial parameters only go up to " << I_YZ
         << ". Returning 0\n";

  return 0;
}

//==============================================================================
void Inertia::setMass(double _mass)
{
  mMass = _mass;
  computeSpatialTensor();
}

//==============================================================================
double Inertia::getMass() const
{
  return mMass;
}

//==============================================================================
void Inertia::setLocalCOM(const Eigen::Vector3d& _com)
{
  mCenterOfMass = _com;
  computeSpatialTensor();
}

//==============================================================================
const Eigen::Vector3d& Inertia::getLocalCOM() const
{
  return mCenterOfMass;
}

//==============================================================================
void Inertia::setMoment(const Eigen::Matrix3d& _moment)
{
  if (!verifyMoment(_moment, true))
    dtwarn << "[Inertia::setMoment] Passing in an invalid moment of inertia "
           << "matrix. Results might not by physically accurate or "
           << "meaningful.\n";

  for (std::size_t i = 0; i < 3; ++i)
    mMoment[i] = _moment(i, i);

  mMoment[I_XY - 4] = _moment(0, 1);
  mMoment[I_XZ - 4] = _moment(0, 2);
  mMoment[I_YZ - 4] = _moment(1, 2);

  computeSpatialTensor();
}

//==============================================================================
void Inertia::setMoment(
    double _Ixx,
    double _Iyy,
    double _Izz,
    double _Ixy,
    double _Ixz,
    double _Iyz)
{
  mMoment[I_XX - 4] = _Ixx;
  mMoment[I_YY - 4] = _Iyy;
  mMoment[I_ZZ - 4] = _Izz;
  mMoment[I_XY - 4] = _Ixy;
  mMoment[I_XZ - 4] = _Ixz;
  mMoment[I_YZ - 4] = _Iyz;

  computeSpatialTensor();
}

//==============================================================================
Eigen::Matrix3d Inertia::getMoment() const
{
  Eigen::Matrix3d I;
  for (int i = 0; i < 3; ++i)
    I(i, i) = mMoment[i];

  I(0, 1) = I(1, 0) = mMoment[I_XY - 4];
  I(0, 2) = I(2, 0) = mMoment[I_XZ - 4];
  I(1, 2) = I(2, 1) = mMoment[I_YZ - 4];

  return I;
}

//==============================================================================
void Inertia::setSpatialTensor(const Eigen::Matrix6d& _spatial)
{
  if (!verifySpatialTensor(_spatial, true))
    dtwarn << "[Inertia::setSpatialTensor] Passing in an invalid spatial "
           << "inertia tensor. Results might not be physically accurate or "
           << "meaningful.\n";

  mSpatialTensor = _spatial;
  computeParameters();
}

//==============================================================================
const Eigen::Matrix6d& Inertia::getSpatialTensor() const
{
  return mSpatialTensor;
}

//==============================================================================
bool Inertia::verifyMoment(
    const Eigen::Matrix3d& _moment, bool _printWarnings, double _tolerance)
{
  bool valid = true;
  for (int i = 0; i < 3; ++i)
  {
    if (_moment(i, i) <= 0)
    {
      valid = false;
      if (_printWarnings)
      {
        dtwarn << "[Inertia::verifyMoment] Invalid entry for (" << i << "," << i
               << "): " << _moment(i, i) << ". Value should be positive "
               << "and greater than zero.\n";
      }
    }
  }

  for (int i = 0; i < 3; ++i)
  {
    for (int j = i + 1; j < 3; ++j)
    {
      if (std::abs(_moment(i, j) - _moment(j, i)) > _tolerance)
      {
        valid = false;
        if (_printWarnings)
        {
          dtwarn << "[Inertia::verifyMoment] Values for entries (" << i << ","
                 << j << ") and (" << j << "," << i << ") differ by "
                 << _moment(i, j) - _moment(j, i) << " which is more than the "
                 << "permitted tolerance (" << _tolerance << ")\n";
        }
      }
    }
  }

  return valid;
}

//==============================================================================
bool Inertia::verifySpatialTensor(
    const Eigen::Matrix6d& _spatial, bool _printWarnings, double _tolerance)
{
  bool valid = true;

  for (std::size_t i = 0; i < 6; ++i)
  {
    if (_spatial(i, i) <= 0)
    {
      valid = false;
      if (_printWarnings)
      {
        std::string component = i < 3 ? "moment of inertia diagonal" : "mass";
        dtwarn << "[Inertia::verifySpatialTensor] Invalid entry for (" << i
               << "," << i << "): " << _spatial(i, i) << ". Value should be "
               << "positive and greater than zero because it corresponds to "
               << component << ".\n";
      }
    }
  }

  // Off-diagonals of top left block
  for (std::size_t i = 0; i < 3; ++i)
  {
    for (std::size_t j = i + 1; j < 3; ++j)
    {
      if (std::abs(_spatial(i, j) - _spatial(j, i)) > _tolerance)
      {
        valid = false;
        dtwarn << "[Inertia::verifySpatialTensor] Values for entries (" << i
               << "," << j << ") and (" << j << "," << i << ") differ by "
               << _spatial(i, j) - _spatial(j, i) << " which is more than the "
               << "permitted tolerance (" << _tolerance << ")\n";
      }
    }
  }

  // Off-diagonals of bottom right block
  for (std::size_t i = 3; i < 6; ++i)
  {
    for (std::size_t j = i + 1; j < 6; ++j)
    {
      if (_spatial(i, j) != 0)
      {
        valid = false;
        if (_printWarnings)
          dtwarn << "[Inertia::verifySpatialTensor] Invalid entry for (" << i
                 << "," << i << "): " << _spatial(i, j) << ". Value should be "
                 << "exactly zero.\n";
      }

      if (_spatial(j, i) != 0)
      {
        valid = false;
        if (_printWarnings)
          dtwarn << "[Inertia::verifySpatialTensor] Invalid entry for (" << j
                 << "," << i << "): " << _spatial(j, i) << ". Value should be "
                 << "exactly zero.\n";
      }
    }
  }

  // Diagonals of the bottom left and top right blocks
  for (std::size_t k = 0; k < 2; ++k)
  {
    for (std::size_t i = 0; i < 3; ++i)
    {
      std::size_t i1 = k == 0 ? i + 3 : i;
      std::size_t i2 = k == 0 ? i : i + 3;
      if (_spatial(i1, i2) != 0)
      {
        valid = false;
        if (_printWarnings)
          dtwarn << "[Inertia::verifySpatialTensor] Invalid entry for (" << i1
                 << "," << i2 << "): " << _spatial(i1, i2) << ". Value should "
                 << "be exactly zero.\n";
      }
    }
  }

  // Check skew-symmetry in bottom left and top right
  for (std::size_t k = 0; k < 2; ++k)
  {
    for (std::size_t i = 0; i < 3; ++i)
    {
      for (std::size_t j = i + 1; j < 3; ++j)
      {
        std::size_t i1 = k == 0 ? i + 3 : i;
        std::size_t j1 = k == 0 ? j : j + 3;

        std::size_t i2 = k == 0 ? j + 3 : j;
        std::size_t j2 = k == 0 ? i : i + 3;

        if (std::abs(_spatial(i1, j1) + _spatial(i2, j2)) > _tolerance)
        {
          valid = false;
          if (_printWarnings)
            dtwarn << "[Inertia::verifySpatialTensor] Mismatch between entries "
                   << "(" << i1 << "," << j1 << ") and (" << i2 << "," << j2
                   << "). They should sum to zero, but instead they sum to "
                   << _spatial(i1, j1) + _spatial(i2, j2)
                   << " which is outside "
                   << "of the permitted tolerance (" << _tolerance << ").\n";
        }
      }
    }
  }

  // Check that the bottom left block is the transpose of the top right block
  // Note that we only need to check three of the components from each block,
  // because the last test ensures that both blocks are skew-symmetric
  // themselves
  for (std::size_t i = 0; i < 3; ++i)
  {
    for (std::size_t j = i + 1; j < 3; ++j)
    {
      std::size_t i1 = i;
      std::size_t j1 = j + 3;

      std::size_t i2 = j1;
      std::size_t j2 = i1;

      if (std::abs(_spatial(i1, j1) - _spatial(i2, j2)) > _tolerance)
      {
        valid = false;
        if (_printWarnings)
          dtwarn << "[Inertia::verifySpatialTensor] Values for  entries "
                 << "(" << i1 << "," << j1 << ") and (" << i2 << "," << j2
                 << ") "
                 << "differ by " << _spatial(i1, j1) - _spatial(i1, j2)
                 << " which is more than the permitted tolerance ("
                 << _tolerance << "). "
                 << "The bottom-left block should be the transpose of the "
                 << "top-right block.\n";
      }
    }
  }

  return valid;
}

//==============================================================================
bool Inertia::verify(bool _printWarnings, double _tolerance) const
{
  return verifySpatialTensor(getSpatialTensor(), _printWarnings, _tolerance);
}

//==============================================================================
bool Inertia::operator==(const Inertia& other) const
{
  return (other.mSpatialTensor == mSpatialTensor);
}

//==============================================================================
// Note: Taken from Springer Handbook, chapter 2.2.11
void Inertia::computeSpatialTensor()
{
  Eigen::Matrix3d C = math::makeSkewSymmetric(mCenterOfMass);

  // Top left
  mSpatialTensor.block<3, 3>(0, 0) = getMoment() + mMass * C * C.transpose();

  // Bottom left
  mSpatialTensor.block<3, 3>(3, 0) = mMass * C.transpose();

  // Top right
  mSpatialTensor.block<3, 3>(0, 3) = mMass * C;

  // Bottom right
  mSpatialTensor.block<3, 3>(3, 3) = mMass * Eigen::Matrix3d::Identity();
}

//==============================================================================
void Inertia::computeParameters()
{
  mMass = mSpatialTensor(3, 3);
  Eigen::Matrix3d C = mSpatialTensor.block<3, 3>(0, 3) / mMass;
  mCenterOfMass[0] = -C(1, 2);
  mCenterOfMass[1] = C(0, 2);
  mCenterOfMass[2] = -C(0, 1);

  Eigen::Matrix3d I = mSpatialTensor.block<3, 3>(0, 0) + mMass * C * C;
  for (std::size_t i = 0; i < 3; ++i)
    mMoment[i] = I(i, i);

  mMoment[I_XY - 4] = I(0, 1);
  mMoment[I_XZ - 4] = I(0, 2);
  mMoment[I_YZ - 4] = I(1, 2);
}

} // namespace dynamics
} // namespace dart
