text: /*
   * 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.
   *
  */
  
  #include <gtest/gtest.h>
  
  #include "Collision.hh"
  #include "Link.hh"
  #include "Model.hh"
  #include "Shape.hh"
  
  using namespace ignition;
  using namespace physics;
  using namespace tpelib;
  
  /////////////////////////////////////////////////
  TEST(Model, BasicAPI)
  {
    Model model;
    model.SetId(1234u);
    EXPECT_EQ(1234u, model.GetId());
  
    model.SetName("model_1");
    EXPECT_EQ("model_1", model.GetName());
    EXPECT_EQ("model_1", model.GetNameRef());
  
    model.SetPose(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3));
    EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), model.GetPose());
    EXPECT_EQ(math::Pose3d(1, 2, 3, 0.1, 0.2, 0.3), model.GetWorldPose());
  
    model.SetLinearVelocity(math::Vector3d(.5, .5, .5));
    EXPECT_EQ(math::Vector3d(.5, .5, .5), model.GetLinearVelocity());
  
    model.SetAngularVelocity(math::Vector3d(1.0, 1.0, 1.0));
    EXPECT_EQ(math::Vector3d(1.0, 1.0, 1.0), model.GetAngularVelocity());
  
    model.UpdatePose(0.1);
    EXPECT_EQ(model.GetPose(), model.GetWorldPose());
  
    EXPECT_FALSE(model.GetStatic());
    model.SetStatic(true);
    EXPECT_TRUE(model.GetStatic());
  
    Model model2;
    EXPECT_NE(model.GetId(), model2.GetId());
  
    // test UpdatePose
    math::Pose3d originalPose = math::Pose3d::Zero;
    double timeStep = 0.1;
    model2.SetPose(originalPose);
    model2.SetLinearVelocity(math::Vector3d(0.1, 0.1, 0.1));
    model2.SetAngularVelocity(math::Vector3d(1.0, 0, 0));
    math::Pose3d expectedPose(
      originalPose.Pos() + math::Vector3d(0.1, 0.1, 0.1) * timeStep,
      originalPose.Rot().Integrate(math::Vector3d(1.0, 0, 0), timeStep));
    model2.UpdatePose(timeStep);
    EXPECT_EQ(expectedPose, model2.GetPose());
  }
  
  /////////////////////////////////////////////////
  TEST(Model, Link)
  {
    Model model;
    EXPECT_EQ(0u, model.GetChildCount());
  
    // add a child
    Entity &linkEnt = model.AddLink();
    linkEnt.SetName("link_1");
    linkEnt.SetPose(math::Pose3d(2, 3, 4, 0, 0, 1));
    EXPECT_EQ(1u, model.GetChildCount());
  
    std::size_t linkId = linkEnt.GetId();
    Entity ent = model.GetChildById(linkId);
    EXPECT_EQ(linkId, ent.GetId());
    EXPECT_EQ("link_1", ent.GetName());
    EXPECT_EQ(math::Pose3d(2, 3, 4, 0, 0, 1), ent.GetPose());
  
    Entity entByName = model.GetChildByName("link_1");
    EXPECT_EQ("link_1", entByName.GetName());
  
    Entity entByIdx = model.GetChildByIndex(0u);
    EXPECT_EQ("link_1", entByIdx.GetName());
  
    // test casting to link
    Link *link = static_cast<Link *>(&linkEnt);
    EXPECT_NE(nullptr, link);
    EXPECT_EQ(linkEnt.GetId(), link->GetId());
  
    // add another child
    Entity &linkEnt2 = model.AddLink();
    EXPECT_EQ(2u, model.GetChildCount());
  
    Entity ent2ByIdx = model.GetChildByIndex(1u);
    EXPECT_EQ(linkEnt2.GetId(), ent2ByIdx.GetId());
  
    Link *link2 = static_cast<Link *>(&linkEnt2);
    EXPECT_NE(nullptr, link2);
    EXPECT_EQ(linkEnt2.GetId(), link2->GetId());
  
    // test canonical link
    model.SetCanonicalLink(link->GetId());
    EXPECT_NE(Entity::kNullEntity.GetId(), model.GetCanonicalLink().GetId());
    EXPECT_EQ(link->GetId(), model.GetCanonicalLink().GetId());
  
    // test remove child by id
    model.RemoveChildById(linkId);
    EXPECT_EQ(1u, model.GetChildCount());
  
    Entity nullEnt = model.GetChildById(linkId);
    EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.GetId());
  }
  
  /////////////////////////////////////////////////
  TEST(Model, BoundingBox)
  {
    Model model;
    EXPECT_EQ(0u, model.GetChildCount());
    EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
  
    // add link with sphere collision shape
    Entity &linkEnt = model.AddLink();
    linkEnt.SetPose(math::Pose3d(0, 0, 1, 0, 0, 0));
    EXPECT_EQ(math::AxisAlignedBox(), linkEnt.GetBoundingBox());
    EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
  
    Link *link = static_cast<Link *>(&linkEnt);
    Entity &collisionEnt = link->AddCollision();
    Collision *collision = static_cast<Collision *>(&collisionEnt);
    SphereShape sphereShape;
    sphereShape.SetRadius(2.0);
    collision->SetShape(sphereShape);
  
    math::AxisAlignedBox expectedBoxLinkFrame(
        math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2));
    EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox());
  
    math::AxisAlignedBox expectedBoxModelFrame(
        math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 3));
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  
    // add another link with box collision shape
    Entity &linkEnt2 = model.AddLink();
    linkEnt2.SetPose(math::Pose3d(0, 1, 0, 0, 0, 0));
    EXPECT_EQ(math::AxisAlignedBox(), linkEnt2.GetBoundingBox());
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  
    Link *link2 = static_cast<Link *>(&linkEnt2);
    Entity &collisionEnt2 = link2->AddCollision();
    Collision *collision2 = static_cast<Collision *>(&collisionEnt2);
    BoxShape boxShape;
    boxShape.SetSize(math::Vector3d(3, 4, 5));
    collision2->SetShape(boxShape);
  
    expectedBoxLinkFrame = math::AxisAlignedBox(
        math::Vector3d(-1.5, -2, -2.5), math::Vector3d(1.5, 2, 2.5));
    EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox());
  
    expectedBoxModelFrame = math::AxisAlignedBox(
        math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3));
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  
    // add nested model with 1 link that has a cylinder collision shape
    Entity &nestedModelEnt = model.AddModel();
    nestedModelEnt.SetPose(math::Pose3d(1, 0, 0, 0, 0, 0));
    EXPECT_EQ(math::AxisAlignedBox(), nestedModelEnt.GetBoundingBox());
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  
    Model *nestedModel = static_cast<Model *>(&nestedModelEnt);
    Entity &nestedLinkEnt = nestedModel->AddLink();
    nestedLinkEnt.SetPose(math::Pose3d(1, 0, 0, 0, 0, 0));
    EXPECT_EQ(math::AxisAlignedBox(), nestedLinkEnt.GetBoundingBox());
    EXPECT_EQ(math::AxisAlignedBox(), nestedModelEnt.GetBoundingBox());
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  
    Link *nestedLink = static_cast<Link *>(&nestedLinkEnt);
    Entity &nestedCollisionEnt = nestedLink->AddCollision();
    Collision *nestedCollision = static_cast<Collision *>(&nestedCollisionEnt);
    CylinderShape cylinderShape;
    cylinderShape.SetRadius(2.0);
    cylinderShape.SetLength(2.0);
    nestedCollision->SetShape(cylinderShape);
  
    math::AxisAlignedBox expectedBoxNestedLinkFrame(
        math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 1));
    EXPECT_EQ(expectedBoxNestedLinkFrame, nestedLinkEnt.GetBoundingBox());
  
    math::AxisAlignedBox expectedBoxNestedModelFrame(
        math::Vector3d(-1, -2, -1), math::Vector3d(3, 2, 1));
    EXPECT_EQ(expectedBoxNestedModelFrame, nestedModelEnt.GetBoundingBox());
  
    expectedBoxModelFrame = math::AxisAlignedBox(
        math::Vector3d(-2, -2, -2.5), math::Vector3d(4, 3, 3));
    EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
  }
  
  /////////////////////////////////////////////////
  TEST(Model, CollideBitmask)
  {
    Model model;
    EXPECT_EQ(0x00, model.GetCollideBitmask());
  
    // add link and verify bitmask is still empty
    Entity &linkEnt = model.AddLink();
    EXPECT_EQ(0x00, linkEnt.GetCollideBitmask());
    EXPECT_EQ(0x00, model.GetCollideBitmask());
  
    // add a collision and verify the model has the same collision bitmask
    Link *link = static_cast<Link *>(&linkEnt);
    Entity &collisionEnt = link->AddCollision();
    Collision *collision = static_cast<Collision *>(&collisionEnt);
    collision->SetCollideBitmask(0x01);
    EXPECT_EQ(0x01, collision->GetCollideBitmask());
    EXPECT_EQ(0x01, linkEnt.GetCollideBitmask());
    EXPECT_EQ(0x01, model.GetCollideBitmask());
  
    // add another collision and verify bitmasks are bitwise OR'd.
    Entity &collisionEnt2 = link->AddCollision();
    Collision *collision2 = static_cast<Collision *>(&collisionEnt2);
    collision2->SetCollideBitmask(0x04);
    EXPECT_EQ(0x04, collision2->GetCollideBitmask());
    EXPECT_EQ(0x05, linkEnt.GetCollideBitmask());
    EXPECT_EQ(0x05, model.GetCollideBitmask());
  
    // add another link with collision and verify
    Entity &linkEnt2 = model.AddLink();
    Link *link2 = static_cast<Link *>(&linkEnt2);
    Entity &collisionEnt3 = link2->AddCollision();
    Collision *collision3 = static_cast<Collision *>(&collisionEnt3);
    collision3->SetCollideBitmask(0x09);
    EXPECT_EQ(0x09, collision3->GetCollideBitmask());
    EXPECT_EQ(0x09, linkEnt2.GetCollideBitmask());
    EXPECT_EQ(0x0D, model.GetCollideBitmask());
  
    // add nested model and verify bitmask is empty
    Entity &nestedModelEnt = model.AddModel();
    EXPECT_EQ(0x00, nestedModelEnt.GetCollideBitmask());
    EXPECT_EQ(0x0D, model.GetCollideBitmask());
  
    // add nested link and verify bitmask is still empty
    Model *nestedModel = static_cast<Model *>(&nestedModelEnt);
    Entity &nestedLinkEnt = nestedModel->AddLink();
    EXPECT_EQ(0x00, nestedLinkEnt.GetCollideBitmask());
    EXPECT_EQ(0x00, nestedModelEnt.GetCollideBitmask());
    EXPECT_EQ(0x0D, model.GetCollideBitmask());
  
    // add a nested collision and verify the nested model has the same bitmask
    // as the nested collision bitmask, and the top level model's bitmask now
    // includes the nested collision's bitmask
    Link *nestedLink = static_cast<Link *>(&nestedLinkEnt);
    Entity &nestedCollisionEnt = nestedLink->AddCollision();
    Collision *nestedCollision = static_cast<Collision *>(&nestedCollisionEnt);
    nestedCollision->SetCollideBitmask(0x02);
    EXPECT_EQ(0x02, nestedCollision->GetCollideBitmask());
    EXPECT_EQ(0x02, nestedLinkEnt.GetCollideBitmask());
    EXPECT_EQ(0x02, nestedModelEnt.GetCollideBitmask());
    EXPECT_EQ(0x0F, model.GetCollideBitmask());
  }
  
  /////////////////////////////////////////////////
  TEST(Model, NestedModel)
  {
    Model model;
    EXPECT_EQ(0u, model.GetChildCount());
  
    // add a child
    Entity &nestedModelEnt = model.AddModel();
    nestedModelEnt.SetName("model_1");
    nestedModelEnt.SetPose(math::Pose3d(2, 3, 4, 0, 0, 1));
    EXPECT_EQ(1u, model.GetChildCount());
  
    std::size_t modelId = nestedModelEnt.GetId();
    Entity ent = model.GetChildById(modelId);
    EXPECT_EQ(modelId, ent.GetId());
    EXPECT_EQ("model_1", ent.GetName());
    EXPECT_EQ(math::Pose3d(2, 3, 4, 0, 0, 1), ent.GetPose());
  
    Entity entByName = model.GetChildByName("model_1");
    EXPECT_EQ("model_1", entByName.GetName());
  
    Entity entByIdx = model.GetChildByIndex(0u);
    EXPECT_EQ("model_1", entByIdx.GetName());
  
    // test casting to model
    Model *nestedModel = static_cast<Model *>(&nestedModelEnt);
    EXPECT_NE(nullptr, nestedModel);
    EXPECT_EQ(nestedModelEnt.GetId(), nestedModel->GetId());
  
    // add another child
    Entity &nestedModelEnt2 = model.AddModel();
    EXPECT_EQ(2u, model.GetChildCount());
  
    Entity ent2ByIdx = model.GetChildByIndex(1u);
    EXPECT_EQ(nestedModelEnt2.GetId(), ent2ByIdx.GetId());
  
    Model *nestedModel2 = static_cast<Model *>(&nestedModelEnt2);
    EXPECT_NE(nullptr, nestedModel2);
    EXPECT_EQ(nestedModelEnt2.GetId(), nestedModel2->GetId());
  
    // test remove child by id
    model.RemoveChildById(modelId);
    EXPECT_EQ(1u, model.GetChildCount());
  
    Entity nullEnt = model.GetChildById(modelId);
    EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.GetId());
  
    // test canonical link within nested model
    Model m0;
    m0.SetName("m0");
  
    // add nested models m1 and m2
    Entity &nestedModelEntm1 = m0.AddModel();
    nestedModelEntm1.SetName("m1");
    Model *m1 = static_cast<Model *>(&nestedModelEntm1);
    Entity &nestedModelEntm2 = m0.AddModel();
    nestedModelEntm2.SetName("m2");
    Model *m2 = static_cast<Model *>(&nestedModelEntm2);
  
    // add links to nested models
    Entity &linkEnt1 = m1->AddLink();
    linkEnt1.SetName("x");
    EXPECT_EQ(1u, m1->GetChildCount());
    Entity &linkEnt2 = m2->AddLink();
    linkEnt2.SetName("y");
    EXPECT_EQ(1u, m2->GetChildCount());
  
    // set link y to be the canonical link of m0
    m0.SetCanonicalLink(linkEnt2.GetId());
    EXPECT_EQ(linkEnt2.GetId(), m0.GetCanonicalLink().GetId());
  
    // Set link y to be default canonical link of m2
    m2->SetCanonicalLink();
    EXPECT_EQ(linkEnt2.GetId(), m2->GetCanonicalLink().GetId());
  }
  