/*
 * 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/constraint/SoftContactConstraint.hpp"

#include <iostream>

#include "dart/collision/CollisionObject.hpp"
#include "dart/common/Console.hpp"
#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/PointMass.hpp"
#include "dart/dynamics/Shape.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/dynamics/SoftBodyNode.hpp"
#include "dart/dynamics/SoftMeshShape.hpp"
#include "dart/external/odelcpsolver/lcp.h"

#define DART_EPSILON 1e-6
#define DART_ERROR_ALLOWANCE 0.0
#define DART_ERP 0.01
#define DART_MAX_ERV 1e+1
#define DART_CFM 1e-5
// #define DART_MAX_NUMBER_OF_CONTACTS 32

#define DART_RESTITUTION_COEFF_THRESHOLD 1e-3
#define DART_FRICTION_COEFF_THRESHOLD 1e-3
#define DART_BOUNCING_VELOCITY_THRESHOLD 1e-1
#define DART_MAX_BOUNCING_VELOCITY 1e+2
#define DART_CONTACT_CONSTRAINT_EPSILON 1e-6

namespace dart {
namespace constraint {

double SoftContactConstraint::mErrorAllowance = DART_ERROR_ALLOWANCE;
double SoftContactConstraint::mErrorReductionParameter = DART_ERP;
double SoftContactConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV;
double SoftContactConstraint::mConstraintForceMixing = DART_CFM;

constexpr double DART_DEFAULT_FRICTION_COEFF = 1.0;
constexpr double DART_DEFAULT_RESTITUTION_COEFF = 0.0;

//==============================================================================
SoftContactConstraint::SoftContactConstraint(
    collision::Contact& contact, double timeStep)
  : ConstraintBase(),
    mTimeStep(timeStep),
    mBodyNode1(const_cast<dynamics::ShapeFrame*>(
                   contact.collisionObject1->getShapeFrame())
                   ->asShapeNode()
                   ->getBodyNodePtr()
                   .get()),
    mBodyNode2(const_cast<dynamics::ShapeFrame*>(
                   contact.collisionObject2->getShapeFrame())
                   ->asShapeNode()
                   ->getBodyNodePtr()
                   .get()),
    mSoftBodyNode1(dynamic_cast<dynamics::SoftBodyNode*>(mBodyNode1)),
    mSoftBodyNode2(dynamic_cast<dynamics::SoftBodyNode*>(mBodyNode2)),
    mPointMass1(nullptr),
    mPointMass2(nullptr),
    mSoftCollInfo(static_cast<collision::SoftCollisionInfo*>(contact.userData)),
    mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()),
    mIsFrictionOn(true),
    mAppliedImpulseIndex(-1),
    mIsBounceOn(false),
    mActive(false)
{
  // TODO(JS): Assumed single contact
  mContacts.push_back(&contact);

  // Set the colliding state of body nodes and point masses to false
  if (mSoftBodyNode1)
  {
    for (std::size_t i = 0; i < mSoftBodyNode1->getNumPointMasses(); ++i)
      mSoftBodyNode1->getPointMass(i)->setColliding(false);
  }
  if (mSoftBodyNode2)
  {
    for (std::size_t i = 0; i < mSoftBodyNode2->getNumPointMasses(); ++i)
      mSoftBodyNode2->getPointMass(i)->setColliding(false);
  }

  // Select colling point mass based on trimesh ID
  if (mSoftBodyNode1)
  {
    if (contact.collisionObject1->getShape()->getType()
        == dynamics::SoftMeshShape::getStaticType())
    {
      mPointMass1 = selectCollidingPointMass(
          mSoftBodyNode1, contact.point, contact.triID1);
      mPointMass1->setColliding(true);
    }
  }
  if (mSoftBodyNode2)
  {
    if (contact.collisionObject2->getShape()->getType()
        == dynamics::SoftMeshShape::getStaticType())
    {
      mPointMass2 = selectCollidingPointMass(
          mSoftBodyNode2, contact.point, contact.triID2);
      mPointMass2->setColliding(true);
    }
  }

  const auto* shapeNodeA = const_cast<dynamics::ShapeFrame*>(
                               contact.collisionObject1->getShapeFrame())
                               ->asShapeNode();
  const auto* shapeNodeB = const_cast<dynamics::ShapeFrame*>(
                               contact.collisionObject2->getShapeFrame())
                               ->asShapeNode();

  //----------------------------------------------------------------------------
  // Bounce
  //----------------------------------------------------------------------------
  const double restitutionCoeffA = computeRestitutionCoefficient(shapeNodeA);
  const double restitutionCoeffB = computeRestitutionCoefficient(shapeNodeB);
  mRestitutionCoeff = restitutionCoeffA * restitutionCoeffB;
  if (mRestitutionCoeff > DART_RESTITUTION_COEFF_THRESHOLD)
    mIsBounceOn = true;
  else
    mIsBounceOn = false;

  //----------------------------------------------------------------------------
  // Friction
  //----------------------------------------------------------------------------
  // TODO(JS): Assume the frictional coefficient can be changed during
  //           simulation steps.
  // Update mFrictionalCoff
  const double frictionCoeffA = computeFrictionCoefficient(shapeNodeA);
  const double frictionCoeffB = computeFrictionCoefficient(shapeNodeB);

  // TODO(JS): Consider providing various ways of the combined friction or
  // allowing to override this method by a custom method
  mFrictionCoeff = std::min(frictionCoeffA, frictionCoeffB);
  if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
  {
    mIsFrictionOn = true;

    // Update frictional direction
    updateFirstFrictionalDirection();
  }
  else
  {
    mIsFrictionOn = false;
  }

  // Compute local contact Jacobians expressed in body frame
  if (mIsFrictionOn)
  {
    // Set the dimension of this constraint. 1 is for Normal direction
    // constraint.
    // TODO(JS): Assumed that the number of contact is not static.
    // TODO(JS): Adjust following code once use of mNumFrictionConeBases is
    //           implemented.
    //  mDim = mContacts.size() * (1 + mNumFrictionConeBases);
    mDim = mContacts.size() * 3;

    mJacobians1.resize(mDim);
    mJacobians2.resize(mDim);

    // Intermediate variables
    int idx = 0;

    Eigen::Vector3d bodyDirection1;
    Eigen::Vector3d bodyDirection2;

    Eigen::Vector3d bodyPoint1;
    Eigen::Vector3d bodyPoint2;

    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      collision::Contact* ct = mContacts[i];

      // TODO(JS): Assumed that the number of tangent basis is 2.
      Eigen::MatrixXd D = getTangentBasisMatrixODE(ct->normal);

      assert(std::abs(ct->normal.dot(D.col(0))) < DART_EPSILON);
      assert(std::abs(ct->normal.dot(D.col(1))) < DART_EPSILON);
      //      if (D.col(0).dot(D.col(1)) > 0.0)
      //        std::cout << "D.col(0).dot(D.col(1): " << D.col(0).dot(D.col(1))
      //        << std::endl;
      assert(std::abs(D.col(0).dot(D.col(1))) < DART_EPSILON);

      //      std::cout << "D: " << std::endl << D << std::endl;

      // Jacobian for normal contact
      bodyDirection1.noalias()
          = mBodyNode1->getTransform().linear().transpose() * ct->normal;
      bodyDirection2.noalias()
          = mBodyNode2->getTransform().linear().transpose() * -ct->normal;

      bodyPoint1.noalias() = mBodyNode1->getTransform().inverse() * ct->point;
      bodyPoint2.noalias() = mBodyNode2->getTransform().inverse() * ct->point;

      mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
      mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);

      mJacobians1[idx].tail<3>() = bodyDirection1;
      mJacobians2[idx].tail<3>() = bodyDirection2;

      ++idx;

      // Jacobian for directional friction 1
      bodyDirection1.noalias()
          = mBodyNode1->getTransform().linear().transpose() * D.col(0);
      bodyDirection2.noalias()
          = mBodyNode2->getTransform().linear().transpose() * -D.col(0);

      //      bodyPoint1.noalias()
      //          = mBodyNode1->getWorldTransform().inverse() * ct->point;
      //      bodyPoint2.noalias()
      //          = mBodyNode2->getWorldTransform().inverse() * ct->point;

      //      std::cout << "bodyDirection2: " << std::endl << bodyDirection2 <<
      //      std::endl;

      mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
      mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);

      mJacobians1[idx].tail<3>() = bodyDirection1;
      mJacobians2[idx].tail<3>() = bodyDirection2;

      ++idx;

      // Jacobian for directional friction 2
      bodyDirection1.noalias()
          = mBodyNode1->getTransform().linear().transpose() * D.col(1);
      bodyDirection2.noalias()
          = mBodyNode2->getTransform().linear().transpose() * -D.col(1);

      //      bodyPoint1.noalias()
      //          = mBodyNode1->getWorldTransform().inverse() * ct->point;
      //      bodyPoint2.noalias()
      //          = mBodyNode2->getWorldTransform().inverse() * ct->point;

      //      std::cout << "bodyDirection2: " << std::endl << bodyDirection2 <<
      //      std::endl;

      mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
      mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);

      mJacobians1[idx].tail<3>() = bodyDirection1;
      mJacobians2[idx].tail<3>() = bodyDirection2;

      ++idx;
    }
  }
  else
  {
    // Set the dimension of this constraint.
    mDim = mContacts.size();

    mJacobians1.resize(mDim);
    mJacobians2.resize(mDim);

    Eigen::Vector3d bodyDirection1;
    Eigen::Vector3d bodyDirection2;

    Eigen::Vector3d bodyPoint1;
    Eigen::Vector3d bodyPoint2;

    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      collision::Contact* ct = mContacts[i];

      bodyDirection1.noalias()
          = mBodyNode1->getTransform().linear().transpose() * ct->normal;
      bodyDirection2.noalias()
          = mBodyNode2->getTransform().linear().transpose() * -ct->normal;

      bodyPoint1.noalias() = mBodyNode1->getTransform().inverse() * ct->point;
      bodyPoint2.noalias() = mBodyNode2->getTransform().inverse() * ct->point;

      mJacobians1[i].head<3>().noalias() = bodyPoint1.cross(bodyDirection1);
      mJacobians2[i].head<3>().noalias() = bodyPoint2.cross(bodyDirection2);

      mJacobians1[i].tail<3>().noalias() = bodyDirection1;
      mJacobians2[i].tail<3>().noalias() = bodyDirection2;
    }
  }

  //----------------------------------------------------------------------------
  // Union finding
  //----------------------------------------------------------------------------
  //  uniteSkeletons();
}

//==============================================================================
SoftContactConstraint::~SoftContactConstraint() {}

//==============================================================================
const std::string& SoftContactConstraint::getType() const
{
  return getStaticType();
}

//==============================================================================
const std::string& SoftContactConstraint::getStaticType()
{
  static const std::string name = "SoftContactConstraint";
  return name;
}

//==============================================================================
void SoftContactConstraint::setErrorAllowance(double _allowance)
{
  // Clamp error reduction parameter if it is out of the range
  if (_allowance < 0.0)
  {
    dtwarn << "Error reduction parameter[" << _allowance
           << "] is lower than 0.0. "
           << "It is set to 0.0." << std::endl;
    mErrorAllowance = 0.0;
  }

  mErrorAllowance = _allowance;
}

//==============================================================================
double SoftContactConstraint::getErrorAllowance()
{
  return mErrorAllowance;
}

//==============================================================================
void SoftContactConstraint::setErrorReductionParameter(double _erp)
{
  // Clamp error reduction parameter if it is out of the range [0, 1]
  if (_erp < 0.0)
  {
    dtwarn << "Error reduction parameter[" << _erp << "] is lower than 0.0. "
           << "It is set to 0.0." << std::endl;
    mErrorReductionParameter = 0.0;
  }
  if (_erp > 1.0)
  {
    dtwarn << "Error reduction parameter[" << _erp << "] is greater than 1.0. "
           << "It is set to 1.0." << std::endl;
    mErrorReductionParameter = 1.0;
  }

  mErrorReductionParameter = _erp;
}

//==============================================================================
double SoftContactConstraint::getErrorReductionParameter()
{
  return mErrorReductionParameter;
}

//==============================================================================
void SoftContactConstraint::setMaxErrorReductionVelocity(double _erv)
{
  // Clamp maximum error reduction velocity if it is out of the range
  if (_erv < 0.0)
  {
    dtwarn << "Maximum error reduction velocity[" << _erv
           << "] is lower than 0.0. "
           << "It is set to 0.0." << std::endl;
    mMaxErrorReductionVelocity = 0.0;
  }

  mMaxErrorReductionVelocity = _erv;
}

//==============================================================================
double SoftContactConstraint::getMaxErrorReductionVelocity()
{
  return mMaxErrorReductionVelocity;
}

//==============================================================================
void SoftContactConstraint::setConstraintForceMixing(double _cfm)
{
  // Clamp constraint force mixing parameter if it is out of the range
  if (_cfm < 1e-9)
  {
    dtwarn << "Constraint force mixing parameter[" << _cfm
           << "] is lower than 1e-9. "
           << "It is set to 1e-9." << std::endl;
    mConstraintForceMixing = 1e-9;
  }

  mConstraintForceMixing = _cfm;
}

//==============================================================================
double SoftContactConstraint::getConstraintForceMixing()
{
  return mConstraintForceMixing;
}

//==============================================================================
void SoftContactConstraint::setFrictionDirection(const Eigen::Vector3d& _dir)
{
  mFirstFrictionalDirection = _dir.normalized();
}

//==============================================================================
const Eigen::Vector3d& SoftContactConstraint::getFrictionDirection1() const
{
  return mFirstFrictionalDirection;
}

//==============================================================================
void SoftContactConstraint::update()
{
  assert(mSoftBodyNode1 || mSoftBodyNode2);

  // One of body node is soft body node and soft body node is always active
  mActive = true;
}

//==============================================================================
void SoftContactConstraint::getInformation(ConstraintInfo* _info)
{
  // Fill w, where the LCP form is Ax = b + w (x >= 0, w >= 0, x^T w = 0)
  getRelVelocity(_info->b);

  //----------------------------------------------------------------------------
  // Friction case
  //----------------------------------------------------------------------------
  if (mIsFrictionOn)
  {
    std::size_t index = 0;
    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      // Bias term, w, should be zero
      assert(_info->w[index] == 0.0);
      assert(_info->w[index + 1] == 0.0);
      assert(_info->w[index + 2] == 0.0);

      // Upper and lower bounds of normal impulsive force
      _info->lo[index] = 0.0;
      _info->hi[index] = dInfinity;
      assert(_info->findex[index] == -1);

      // Upper and lower bounds of tangential direction-1 impulsive force
      _info->lo[index + 1] = -mFrictionCoeff;
      _info->hi[index + 1] = mFrictionCoeff;
      _info->findex[index + 1] = index;

      // Upper and lower bounds of tangential direction-2 impulsive force
      _info->lo[index + 2] = -mFrictionCoeff;
      _info->hi[index + 2] = mFrictionCoeff;
      _info->findex[index + 2] = index;

      //      std::cout << "_frictionalCoff: " << _frictionalCoff << std::endl;

      //      std::cout << "_lcp->ub[_idx + 1]: " << _lcp->ub[_idx + 1] <<
      //      std::endl; std::cout << "_lcp->ub[_idx + 2]: " << _lcp->ub[_idx +
      //      2] << std::endl;

      //------------------------------------------------------------------------
      // Bouncing
      //------------------------------------------------------------------------
      // A. Penetration correction
      double bouncingVelocity
          = mContacts[i]->penetrationDepth - mErrorAllowance;
      if (bouncingVelocity < 0.0)
      {
        bouncingVelocity = 0.0;
      }
      else
      {
        bouncingVelocity *= mErrorReductionParameter * _info->invTimeStep;
        if (bouncingVelocity > mMaxErrorReductionVelocity)
          bouncingVelocity = mMaxErrorReductionVelocity;
      }

      // B. Restitution
      if (mIsBounceOn)
      {
        double& negativeRelativeVel = _info->b[index];
        double restitutionVel = negativeRelativeVel * mRestitutionCoeff;

        if (restitutionVel > DART_BOUNCING_VELOCITY_THRESHOLD)
        {
          if (restitutionVel > bouncingVelocity)
          {
            bouncingVelocity = restitutionVel;

            if (bouncingVelocity > DART_MAX_BOUNCING_VELOCITY)
            {
              bouncingVelocity = DART_MAX_BOUNCING_VELOCITY;
            }
          }
        }
      }

      //
      //      _lcp->b[_idx] = _lcp->b[_idx] * 1.1;
      //      std::cout << "_lcp->b[_idx]: " << _lcp->b[_idx] << std::endl;
      _info->b[index] += bouncingVelocity;
      //      std::cout << "_lcp->b[_idx]: " << _lcp->b[_idx] << std::endl;

      // TODO(JS): Initial guess
      // x
      _info->x[index] = 0.0;
      _info->x[index + 1] = 0.0;
      _info->x[index + 2] = 0.0;

      // Increase index
      index += 3;
    }
  }
  //----------------------------------------------------------------------------
  // Frictionless case
  //----------------------------------------------------------------------------
  else
  {
    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      // Bias term, w, should be zero
      _info->w[i] = 0.0;

      // Upper and lower bounds of normal impulsive force
      _info->lo[i] = 0.0;
      _info->hi[i] = dInfinity;
      assert(_info->findex[i] == -1);

      //------------------------------------------------------------------------
      // Bouncing
      //------------------------------------------------------------------------
      // A. Penetration correction
      double bouncingVelocity
          = mContacts[i]->penetrationDepth - DART_ERROR_ALLOWANCE;
      if (bouncingVelocity < 0.0)
      {
        bouncingVelocity = 0.0;
      }
      else
      {
        bouncingVelocity *= mErrorReductionParameter * _info->invTimeStep;
        if (bouncingVelocity > DART_MAX_ERV)
          bouncingVelocity = DART_MAX_ERV;
      }

      // B. Restitution
      if (mIsBounceOn)
      {
        double& negativeRelativeVel = _info->b[i];
        double restitutionVel = negativeRelativeVel * mRestitutionCoeff;

        if (restitutionVel > DART_BOUNCING_VELOCITY_THRESHOLD)
        {
          if (restitutionVel > bouncingVelocity)
          {
            bouncingVelocity = restitutionVel;

            if (bouncingVelocity > DART_MAX_BOUNCING_VELOCITY)
            {
              bouncingVelocity = DART_MAX_BOUNCING_VELOCITY;
            }
          }
        }
      }

      //
      //      _lcp->b[_idx] = _lcp->b[_idx] * 1.1;
      //      std::cout << "_lcp->b[_idx]: " << _lcp->b[_idx] << std::endl;
      _info->b[i] += bouncingVelocity;
      //      std::cout << "_lcp->b[_idx]: " << _lcp->b[_idx] << std::endl;

      // TODO(JS): Initial guess
      // x
      _info->x[i] = 0.0;

      // Increase index
    }
  }
}

//==============================================================================
void SoftContactConstraint::applyUnitImpulse(std::size_t _idx)
{
  assert(_idx < mDim && "Invalid Index.");
  assert(isActive());
  assert(mBodyNode1->isReactive() || mBodyNode2->isReactive());

  // Self collision case
  if (mBodyNode1->getSkeleton() == mBodyNode2->getSkeleton())
  {
    mBodyNode1->getSkeleton()->clearConstraintImpulses();

    if (mPointMass1)
    {
      mBodyNode1->getSkeleton()->updateBiasImpulse(
          mSoftBodyNode1, mPointMass1, mJacobians1[_idx].tail<3>());
    }
    else
    {
      if (mBodyNode1->isReactive())
      {
        mBodyNode1->getSkeleton()->updateBiasImpulse(
            mBodyNode1, mJacobians1[_idx]);
      }
    }

    if (mPointMass2)
    {
      mBodyNode2->getSkeleton()->updateBiasImpulse(
          mSoftBodyNode2, mPointMass2, mJacobians2[_idx].tail<3>());
    }
    else
    {
      if (mBodyNode2->isReactive())
      {
        mBodyNode2->getSkeleton()->updateBiasImpulse(
            mBodyNode2, mJacobians2[_idx]);
      }
    }

    mBodyNode1->getSkeleton()->updateVelocityChange();
  }
  // Colliding two distinct skeletons
  else
  {
    if (mPointMass1)
    {
      mBodyNode1->getSkeleton()->clearConstraintImpulses();
      mBodyNode1->getSkeleton()->updateBiasImpulse(
          mSoftBodyNode1, mPointMass1, mJacobians1[_idx].tail<3>());
      mBodyNode1->getSkeleton()->updateVelocityChange();
    }
    else
    {
      if (mBodyNode1->isReactive())
      {
        mBodyNode1->getSkeleton()->clearConstraintImpulses();
        mBodyNode1->getSkeleton()->updateBiasImpulse(
            mBodyNode1, mJacobians1[_idx]);
        mBodyNode1->getSkeleton()->updateVelocityChange();
      }
    }

    if (mPointMass2)
    {
      mBodyNode2->getSkeleton()->clearConstraintImpulses();
      mBodyNode2->getSkeleton()->updateBiasImpulse(
          mSoftBodyNode2, mPointMass2, mJacobians2[_idx].tail<3>());
      mBodyNode2->getSkeleton()->updateVelocityChange();
    }
    else
    {
      if (mBodyNode2->isReactive())
      {
        mBodyNode2->getSkeleton()->clearConstraintImpulses();
        mBodyNode2->getSkeleton()->updateBiasImpulse(
            mBodyNode2, mJacobians2[_idx]);
        mBodyNode2->getSkeleton()->updateVelocityChange();
      }
    }
  }

  mAppliedImpulseIndex = _idx;
}

//==============================================================================
void SoftContactConstraint::getVelocityChange(double* _vel, bool _withCfm)
{
  assert(_vel != nullptr && "Null pointer is not allowed.");

  for (std::size_t i = 0; i < mDim; ++i)
  {
    _vel[i] = 0.0;

    if (mBodyNode1->getSkeleton()->isImpulseApplied())
    {
      if (mPointMass1)
      {
        _vel[i] += mJacobians1[i].tail<3>().dot(
            mPointMass1->getBodyVelocityChange());
      }
      else
      {
        if (mBodyNode1->isReactive())
          _vel[i] += mJacobians1[i].dot(mBodyNode1->getBodyVelocityChange());
      }
    }

    if (mBodyNode2->getSkeleton()->isImpulseApplied())
    {
      if (mPointMass2)
      {
        _vel[i] += mJacobians2[i].tail<3>().dot(
            mPointMass2->getBodyVelocityChange());
      }
      else
      {
        if (mBodyNode2->isReactive())
          _vel[i] += mJacobians2[i].dot(mBodyNode2->getBodyVelocityChange());
      }
    }
  }

  // Add small values to diagnal to keep it away from singular, similar to cfm
  // varaible in ODE
  if (_withCfm)
  {
    _vel[mAppliedImpulseIndex]
        += _vel[mAppliedImpulseIndex] * mConstraintForceMixing;
  }
}

//==============================================================================
void SoftContactConstraint::excite()
{
  if (mBodyNode1->isReactive())
    mBodyNode1->getSkeleton()->setImpulseApplied(true);

  if (mBodyNode2->isReactive())
    mBodyNode2->getSkeleton()->setImpulseApplied(true);
}

//==============================================================================
void SoftContactConstraint::unexcite()
{
  if (mBodyNode1->isReactive())
    mBodyNode1->getSkeleton()->setImpulseApplied(false);

  if (mBodyNode2->isReactive())
    mBodyNode2->getSkeleton()->setImpulseApplied(false);
}

//==============================================================================
void SoftContactConstraint::applyImpulse(double* _lambda)
{
  // TODO(JS): Need to optimize code

  //----------------------------------------------------------------------------
  // Friction case
  //----------------------------------------------------------------------------
  if (mIsFrictionOn)
  {
    std::size_t index = 0;

    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      //      std::cout << "_lambda1: " << _lambda[_idx] << std::endl;
      //      std::cout << "_lambda2: " << _lambda[_idx + 1] << std::endl;
      //      std::cout << "_lambda3: " << _lambda[_idx + 2] << std::endl;

      //      std::cout << "imp1: " << mJacobians2[i * 3 + 0] * _lambda[_idx] <<
      //      std::endl; std::cout << "imp2: " << mJacobians2[i * 3 + 1] *
      //      _lambda[_idx + 1] << std::endl; std::cout << "imp3: " <<
      //      mJacobians2[i * 3 + 2] * _lambda[_idx + 2] << std::endl;

      assert(!math::isNan(_lambda[index]));

      // Store contact impulse (force) toward the normal w.r.t. world frame
      mContacts[i]->force = mContacts[i]->normal * _lambda[index] / mTimeStep;

      // Normal impulsive force
      //      mContacts[i]->lambda[0] = _lambda[_idx];
      if (mPointMass1)
      {
        mPointMass1->addConstraintImpulse(
            mJacobians1[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode1->isReactive())
          mBodyNode1->addConstraintImpulse(mJacobians1[index] * _lambda[index]);
      }

      if (mPointMass2)
      {
        mPointMass2->addConstraintImpulse(
            mJacobians2[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode2->isReactive())
          mBodyNode2->addConstraintImpulse(mJacobians2[index] * _lambda[index]);
      }
      //      std::cout << "_lambda: " << _lambda[_idx] << std::endl;
      index++;

      assert(!math::isNan(_lambda[index]));

      // Add contact impulse (force) toward the tangential w.r.t. world frame
      Eigen::MatrixXd D = getTangentBasisMatrixODE(mContacts[i]->normal);
      mContacts[i]->force += D.col(0) * _lambda[index] / mTimeStep;

      // Tangential direction-1 impulsive force
      //      mContacts[i]->lambda[1] = _lambda[_idx];
      if (mPointMass1)
      {
        mPointMass1->addConstraintImpulse(
            mJacobians1[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode1->isReactive())
          mBodyNode1->addConstraintImpulse(mJacobians1[index] * _lambda[index]);
      }

      if (mPointMass2)
      {
        mPointMass2->addConstraintImpulse(
            mJacobians2[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode2->isReactive())
          mBodyNode2->addConstraintImpulse(mJacobians2[index] * _lambda[index]);
      }
      //      std::cout << "_lambda: " << _lambda[_idx] << std::endl;
      index++;

      assert(!math::isNan(_lambda[index]));

      // Add contact impulse (force) toward the tangential w.r.t. world frame
      mContacts[i]->force += D.col(1) * _lambda[index] / mTimeStep;

      // Tangential direction-2 impulsive force
      //      mContacts[i]->lambda[2] = _lambda[_idx];
      if (mPointMass1)
      {
        mPointMass1->addConstraintImpulse(
            mJacobians1[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode1->isReactive())
          mBodyNode1->addConstraintImpulse(mJacobians1[index] * _lambda[index]);
      }

      if (mPointMass2)
      {
        mPointMass2->addConstraintImpulse(
            mJacobians2[index].tail<3>() * _lambda[index], true);
      }
      else
      {
        if (mBodyNode2->isReactive())
          mBodyNode2->addConstraintImpulse(mJacobians2[index] * _lambda[index]);
      }
      //      std::cout << "_lambda: " << _lambda[_idx] << std::endl;
      index++;
    }
  }
  //----------------------------------------------------------------------------
  // Frictionless case
  //----------------------------------------------------------------------------
  else
  {
    for (std::size_t i = 0; i < mContacts.size(); ++i)
    {
      // Normal impulsive force
      //			pContactPts[i]->lambda[0] = _lambda[i];

      if (mPointMass1)
      {
        mPointMass1->addConstraintImpulse(
            mJacobians1[i].tail<3>() * _lambda[i], true);
      }
      else
      {
        if (mBodyNode1->isReactive())
          mBodyNode1->addConstraintImpulse(mJacobians1[i] * _lambda[i]);
      }

      if (mPointMass2)
      {
        mPointMass2->addConstraintImpulse(
            mJacobians2[i].tail<3>() * _lambda[i], true);
      }
      else
      {
        if (mBodyNode2->isReactive())
          mBodyNode2->addConstraintImpulse(mJacobians2[i] * _lambda[i]);
      }

      // Store contact impulse (force) toward the normal w.r.t. world frame
      mContacts[i]->force = mContacts[i]->normal * _lambda[i] / mTimeStep;
    }
  }
}

//==============================================================================
void SoftContactConstraint::getRelVelocity(double* _vel)
{
  assert(_vel != nullptr && "Null pointer is not allowed.");

  for (std::size_t i = 0; i < mDim; ++i)
  {
    _vel[i] = 0.0;

    if (mPointMass1)
      _vel[i] -= mJacobians1[i].tail<3>().dot(mPointMass1->getBodyVelocity());
    else
      _vel[i] -= mJacobians1[i].dot(mBodyNode1->getSpatialVelocity());

    if (mPointMass2)
      _vel[i] -= mJacobians2[i].tail<3>().dot(mPointMass2->getBodyVelocity());
    else
      _vel[i] -= mJacobians2[i].dot(mBodyNode2->getSpatialVelocity());

    //    std::cout << "_relVel[i + _idx]: " << _relVel[i + _idx] << std::endl;
  }
}

//==============================================================================
bool SoftContactConstraint::isActive() const
{
  return mActive;
}

//==============================================================================
double SoftContactConstraint::computeFrictionCoefficient(
    const dynamics::ShapeNode* shapeNode)
{
  assert(shapeNode);

  auto dynamicAspect = shapeNode->getDynamicsAspect();

  if (dynamicAspect == nullptr)
  {
    dtwarn << "[ContactConstraint] Attempt to extract friction coefficient "
           << "from a ShapeNode that doesn't have DynamicAspect. The default "
           << "value (" << DART_DEFAULT_FRICTION_COEFF << ") will be used "
           << "instead.\n";
    return DART_DEFAULT_FRICTION_COEFF;
  }

  return dynamicAspect->getFrictionCoeff();
}

//==============================================================================
double SoftContactConstraint::computeRestitutionCoefficient(
    const dynamics::ShapeNode* shapeNode)
{
  assert(shapeNode);

  auto dynamicAspect = shapeNode->getDynamicsAspect();

  if (dynamicAspect == nullptr)
  {
    dtwarn << "[ContactConstraint] Attempt to extract restitution coefficient "
           << "from a ShapeNode that doesn't have DynamicAspect. The default "
           << "value (" << DART_DEFAULT_RESTITUTION_COEFF << ") will be used "
           << "instead.\n";
    return DART_DEFAULT_RESTITUTION_COEFF;
  }

  return dynamicAspect->getRestitutionCoeff();
}

//==============================================================================
dynamics::SkeletonPtr SoftContactConstraint::getRootSkeleton() const
{
  if (mSoftBodyNode1 || mBodyNode1->isReactive())
    return mBodyNode1->getSkeleton()->mUnionRootSkeleton.lock();
  else
    return mBodyNode2->getSkeleton()->mUnionRootSkeleton.lock();
}

//==============================================================================
void SoftContactConstraint::uniteSkeletons()
{
  if (!mBodyNode1->isReactive() || !mBodyNode2->isReactive())
    return;

  if (mBodyNode1->getSkeleton() == mBodyNode2->getSkeleton())
    return;

  const dynamics::SkeletonPtr& unionId1
      = ConstraintBase::compressPath(mBodyNode1->getSkeleton());
  const dynamics::SkeletonPtr& unionId2
      = ConstraintBase::compressPath(mBodyNode2->getSkeleton());

  if (unionId1 == unionId2)
    return;

  if (unionId1->mUnionSize < unionId2->mUnionSize)
  {
    // Merge root1 --> root2
    unionId1->mUnionRootSkeleton = unionId2;
    unionId2->mUnionSize += unionId1->mUnionSize;
  }
  else
  {
    // Merge root2 --> root1
    unionId2->mUnionRootSkeleton = unionId1;
    unionId1->mUnionSize += unionId2->mUnionSize;
  }
}

//==============================================================================
void SoftContactConstraint::updateFirstFrictionalDirection()
{
  //  std::cout <<
  //  "SoftContactConstraintTEST::_updateFirstFrictionalDirection(): "
  //            << "Not finished implementation."
  //            << std::endl;

  // TODO(JS): Not implemented
  // Refer to:
  // https://github.com/erwincoumans/bullet3/blob/master/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp#L910
  //  mFirstFrictionalDirection;
}

//==============================================================================
Eigen::MatrixXd SoftContactConstraint::getTangentBasisMatrixODE(
    const Eigen::Vector3d& _n)
{
  using namespace math::suffixes;

  // TODO(JS): Use mNumFrictionConeBases
  // Check if the number of bases is even number.
  //  bool isEvenNumBases = mNumFrictionConeBases % 2 ? true : false;

  Eigen::MatrixXd T(Eigen::MatrixXd::Zero(3, 2));

  // Pick an arbitrary vector to take the cross product of (in this case,
  // Z-axis)
  Eigen::Vector3d tangent = mFirstFrictionalDirection.cross(_n);

  // TODO(JS): Modify following lines once _updateFirstFrictionalDirection() is
  //           implemented.
  // If they're too close, pick another tangent (use X-axis as arbitrary vector)
  if (tangent.norm() < DART_CONTACT_CONSTRAINT_EPSILON)
    tangent = Eigen::Vector3d::UnitX().cross(_n);

  tangent.normalize();

  // Rotate the tangent around the normal to compute bases.
  // Note: a possible speedup is in place for mNumDir % 2 = 0
  // Each basis and its opposite belong in the matrix, so we iterate half as
  // many times
  T.col(0) = tangent;
  T.col(1) = Eigen::Quaterniond(Eigen::AngleAxisd(0.5_pi, _n)) * tangent;
  return T;
}

//==============================================================================
template <typename PointMassT, typename SoftBodyNodeT>
static PointMassT selectCollidingPointMassT(
    SoftBodyNodeT _softBodyNode, const Eigen::Vector3d& _point, int _faceId)
{
  PointMassT pointMass = nullptr;

  const Eigen::Vector3i& face = _softBodyNode->getFace(_faceId);

  PointMassT pm0 = _softBodyNode->getPointMass(face[0]);
  PointMassT pm1 = _softBodyNode->getPointMass(face[1]);
  PointMassT pm2 = _softBodyNode->getPointMass(face[2]);

  const Eigen::Vector3d& pos1 = pm0->getWorldPosition();
  const Eigen::Vector3d& pos2 = pm1->getWorldPosition();
  const Eigen::Vector3d& pos3 = pm2->getWorldPosition();

  double dist0 = (pos1 - _point).dot(pos1 - _point);
  double dist1 = (pos2 - _point).dot(pos2 - _point);
  double dist2 = (pos3 - _point).dot(pos3 - _point);

  if (dist0 > dist1)
  {
    if (dist1 > dist2)
      pointMass = pm2;
    else
      pointMass = pm1;
  }
  else
  {
    if (dist0 > dist2)
      pointMass = pm2;
    else
      pointMass = pm0;
  }

  return pointMass;
}

//==============================================================================
const dynamics::PointMass* SoftContactConstraint::selectCollidingPointMass(
    const dynamics::SoftBodyNode* _softBodyNode,
    const Eigen::Vector3d& _point,
    int _faceId) const
{
  return selectCollidingPointMassT<
      const dynamics::PointMass*,
      const dynamics::SoftBodyNode*>(_softBodyNode, _point, _faceId);
}

//==============================================================================
dynamics::PointMass* SoftContactConstraint::selectCollidingPointMass(
    dynamics::SoftBodyNode* _softBodyNode,
    const Eigen::Vector3d& _point,
    int _faceId) const
{
  return selectCollidingPointMassT<
      dynamics::PointMass*,
      dynamics::SoftBodyNode*>(_softBodyNode, _point, _faceId);
}

} // namespace constraint
} // namespace dart
