/*
 * Copyright (C) 2024 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.
 *
*/

#include "KinematicLinkFeatures.hh"

namespace gz {
namespace physics {
namespace bullet_featherstone {

#if BT_BULLET_VERSION >= 307
/////////////////////////////////////////////////
void KinematicLinkFeatures::SetLinkKinematic(
    const Identity &_id, bool _kinematic)
{
  auto *link = this->ReferenceInterface<LinkInfo>(_id);
  auto *model = this->ReferenceInterface<ModelInfo>(link->model);

  int collisionFlags = _kinematic ? btCollisionObject::CF_KINEMATIC_OBJECT :
      btCollisionObject::CF_DYNAMIC_OBJECT;

  if (link->indexInModel.has_value())
  {
    model->body->setLinkDynamicType(link->indexInModel.value(), collisionFlags);
  }
  else
  {
    model->body->setBaseDynamicType(collisionFlags);
  }
}

/////////////////////////////////////////////////
bool KinematicLinkFeatures::GetLinkKinematic(const Identity &_id) const
{
  auto *link = this->ReferenceInterface<LinkInfo>(_id);
  auto *model = this->ReferenceInterface<ModelInfo>(link->model);

  int indexInModel = link->indexInModel.value_or(-1);
  return model->body->isLinkKinematic(indexInModel);
}
#endif

}  // namespace bullet_featherstone
}  // namespace physics
}  // namespace gz
