/*
 * Copyright (C) 2015 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 <string.h>

#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "gazebo/test/ServerFixture.hh"
#include "gazebo/test/helper_physics_generator.hh"

using namespace gazebo;

class JointGetForceTorqueTest : public ServerFixture,
                                public testing::WithParamInterface<const char *>
{
  /// \brief Constructor
  protected: JointGetForceTorqueTest() : ServerFixture()
             {
             }

  /// \brief Class to hold parameters for spawning joints.
  public: class SpawnGetFTBoxOptions
  {
    /// \brief Constructor.
    public: SpawnGetFTBoxOptions() : mass(10.0),
                                     size(1.0, 1.0, 1.0),
                                     parentIsWorld(false)
            {
            }

    /// \brief Destructor.
    public: ~SpawnGetFTBoxOptions()
            {
            }

    /// \brief Mass of box to spawn (inertia computed automatically).
    public: double mass;

    /// \brief Size of box to spawn.
    public: ignition::math::Vector3d size;

    /// \brief Model pose.
    public: ignition::math::Pose3d modelPose;

    /// \brief Link pose.
    public: ignition::math::Pose3d linkPose;

    /// \brief Inertial pose.
    public: ignition::math::Pose3d inertialPose;

    /// \brief Joint type
    public: std::string jointType;

    /// \brief Joint axis
    public: ignition::math::Vector3d jointAxis;

    /// \brief If true the parent of the joint is the world
    ///        otherwise the parent of the tested joint is a dummy link
    ///        that is itself connect to the world
    public: bool parentIsWorld;
  };

  /// \brief Spawn a box rigidly attached to the world
  /// \param[in] _opt Options for box attached to the world.
  public: physics::ModelPtr SpawnBox(const SpawnGetFTBoxOptions &_opt)
          {
            msgs::Model msg;
            std::string modelName = this->GetUniqueString("box_model");
            msg.set_name(modelName);
            msgs::Set(msg.mutable_pose(), _opt.modelPose);

            double dx = _opt.size.X();
            double dy = _opt.size.Y();
            double dz = _opt.size.Z();
            double ixx = _opt.mass/12.0 * (dy*dy + dz*dz);
            double iyy = _opt.mass/12.0 * (dz*dz + dx*dx);
            double izz = _opt.mass/12.0 * (dx*dx + dy*dy);


            if ( !_opt.parentIsWorld )
            {
              msg.add_link();
              int linkCount = msg.link_size();
              auto link = msg.mutable_link(linkCount-1);
              link->set_name("dummy_link");
            }

            msg.add_link();
            int linkCount = msg.link_size();
            auto link = msg.mutable_link(linkCount-1);
            link->set_name("link");
            msgs::Set(link->mutable_pose(), _opt.linkPose);
            msgs::Inertial inertial;
            inertial.set_mass(_opt.mass);
            inertial.set_ixx(ixx);
            inertial.set_iyy(iyy);
            inertial.set_izz(izz);
            inertial.set_ixy(0.0);
            inertial.set_ixz(0.0);
            inertial.set_iyz(0.0);
            msgs::Set(inertial.mutable_pose(), _opt.inertialPose);

            *(link->mutable_inertial()) = inertial;

            // Depending on the parentIsWorld option, we test
            // two different cases: a joint that is connecting
            // a link to the world (parentIsWorld true) and
            // a joint connecting two different links (and the
            // parent one is itself connected to the world for
            // convenience)
            if ( !_opt.parentIsWorld )
            {
              msg.add_joint();
              int jointCount = msg.joint_size();
              auto joint = msg.mutable_joint(jointCount-1);
              joint->set_name("dummy_joint");
              joint->set_parent("world");
              joint->set_child("dummy_link");
              joint->set_type(msgs::ConvertJointType("fixed"));
            }

            msg.add_joint();
            int jointCount = msg.joint_size();
            auto joint = msg.mutable_joint(jointCount-1);
            joint->set_name("joint");
            joint->set_type(msgs::ConvertJointType(_opt.jointType));
            if ( _opt.parentIsWorld )
            {
              joint->set_parent("world");
            }
            else
            {
              joint->set_parent("dummy_link");
            }
            joint->set_child("link");

            if ( _opt.jointType != "fixed" )
            {
              auto axis = joint->mutable_axis1();
              msgs::Set(axis->mutable_xyz(), _opt.jointAxis);
              axis->set_limit_lower(0.0);
              axis->set_limit_upper(0.0);
            }

            physics::WorldPtr world = physics::get_world("default");
            physics::ModelPtr model =this->SpawnModel(msg);

            physics::JointPtr pJoint = model->GetJoint("joint");
            pJoint->SetProvideFeedback(true);

            return model;
          }

  /// \brief Helper function for GetForceTorqueDemo
  /// \param[in] _world         Pointer to the world object
  /// \param[in] _physicsEngine Physics engine to use.
  /// \param[in] _options       Options for the joint to test.
  public: void GetFTDemoHelper(physics::WorldPtr _world,
                               const std::string &_physicsEngine,
                               const SpawnGetFTBoxOptions & opt);

  /// \brief Test GetForceTorque method for different type joints
  /// \param[in] _physicsEngine Physics engine to use.
  public: void GetForceTorqueDemo(const std::string &_physicsEngine);
};

/////////////////////////////////////////////////
void JointGetForceTorqueTest::GetFTDemoHelper(
    physics::WorldPtr _world,
    const std::string& _physicsEngine,
    const SpawnGetFTBoxOptions& opt)
{
  gzdbg << "GetFTDemoHelper for physics " << _physicsEngine
        << "joint type " << opt.jointType
        << " joint axis " << opt.jointAxis << std::endl;
  ignition::math::Vector3d g = _world->Gravity();
  double mass = opt.mass;

  physics::ModelPtr model = SpawnBox(opt);
  physics::LinkPtr  link  = model->GetLink("link");
  physics::JointPtr joint = model->GetJoint("joint");

  ASSERT_TRUE(model != NULL);
  ASSERT_TRUE(link != NULL);
  ASSERT_TRUE(joint != NULL);

  ignition::math::Vector3d com = link->WorldCoGPose().Pos();
  ignition::math::Vector3d jointOrigin = joint->WorldPose().Pos();

  // do a simulation step to get a meaningful measure
  _world->Step(1);

  // ode need some additional steps
  if (_physicsEngine == "ode")
  {
    _world->Step(15);
  }

  // bullet need some additional steps
  // probably related to Gazebo issue 1196
  if (_physicsEngine == "bullet")
  {
    _world->Step(99);
  }

  if (opt.parentIsWorld)
  {
    ASSERT_EQ(model->GetJointCount(), 1u);
  }
  else
  {
    ASSERT_EQ(model->GetJointCount(), 2u);
  }

  physics::JointWrench W = joint->GetForceTorque(0u);

  const int TOL_FORCE = 1.0;
  const int TOL_TORQUE = 2.0;

  // Everthing is expressed in world frame, so
  // the math is easy
  ignition::math::Vector3d fWorld = mass*g;
  ignition::math::Vector3d tauWorld = mass*(com-jointOrigin).Cross(g);

  ignition::math::Pose3d parentPose;
  ignition::math::Pose3d childPose = link->WorldPose();

  if (!opt.parentIsWorld)
  {
    parentPose = link->GetParentJointsLinks()[0]->WorldPose();
  }

  ignition::math::Vector3d body1ForceExpected =
    -(parentPose.Rot().Inverse() * fWorld);
  ignition::math::Vector3d body1TorqueExpected =
    -(parentPose.Rot().Inverse() * tauWorld);
  ignition::math::Vector3d body2ForceExpected =
    childPose.Rot().Inverse() * fWorld;
  ignition::math::Vector3d body2TorqueExpected =
    childPose.Rot().Inverse() * tauWorld;

  EXPECT_NEAR(body1ForceExpected.X(), W.body1Force.X(), TOL_FORCE);
  EXPECT_NEAR(body1ForceExpected.Y(), W.body1Force.Y(), TOL_FORCE);
  EXPECT_NEAR(body1ForceExpected.Z(), W.body1Force.Z(), TOL_FORCE);
  EXPECT_NEAR(body1TorqueExpected.X(), W.body1Torque.X(), TOL_TORQUE);
  EXPECT_NEAR(body1TorqueExpected.Y(), W.body1Torque.Y(), TOL_TORQUE);
  EXPECT_NEAR(body1TorqueExpected.Z(), W.body1Torque.Z(), TOL_TORQUE);

  EXPECT_NEAR(body2ForceExpected.X(), W.body2Force.X(), TOL_FORCE);
  EXPECT_NEAR(body2ForceExpected.Y(), W.body2Force.Y(), TOL_FORCE);
  EXPECT_NEAR(body2ForceExpected.Z(), W.body2Force.Z(), TOL_FORCE);
  EXPECT_NEAR(body2TorqueExpected.X(), W.body2Torque.X(), TOL_TORQUE);
  EXPECT_NEAR(body2TorqueExpected.Y(), W.body2Torque.Y(), TOL_TORQUE);
  EXPECT_NEAR(body2TorqueExpected.Z(), W.body2Torque.Z(), TOL_TORQUE);

  // Remove model
  _world->RemoveModel(model);
}

/////////////////////////////////////////////////
// GetForceTorqueDemo test:
// spawn a simple box connected to the world
// with different types of "fixed" joints
// (actual fixed joint, or revolute/prismatic joint
//  with zero limis) and then check the GetForceTorque
// output against analytical solution.
void JointGetForceTorqueTest::GetForceTorqueDemo(const std::string &_physEng)
{
  Load("worlds/empty.world", true, _physEng);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  physics::PhysicsEnginePtr physics = world->Physics();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physEng);

  // test for a fixed joint
  SpawnGetFTBoxOptions opt;
  opt.jointType = "fixed";
  opt.inertialPose.Pos() = ignition::math::Vector3d(1.0, 2.0, 3.0);
  opt.linkPose.Rot().Euler(0.1, 0.0, 0.0);

  opt.parentIsWorld = true;
  this->GetFTDemoHelper(world, _physEng, opt);

  opt.parentIsWorld = false;
  this->GetFTDemoHelper(world, _physEng, opt);

  // test a revolute joint against all axis

  if ( _physEng == "bullet" )
  {
    for (int i = 0; i < 3; ++i)
    {
      opt.jointType = "revolute";
      switch (i)
      {
        case 0:
          opt.jointAxis = ignition::math::Vector3d::UnitX;
          break;
        case 1:
          opt.jointAxis = ignition::math::Vector3d::UnitY;
          break;
        case 2:
          opt.jointAxis = ignition::math::Vector3d::UnitZ;
          break;
      }

      this->GetFTDemoHelper(world, _physEng, opt);
    }
  }

  // test a prismatic joint against all axis

  // bullet and simbody GetForceTorque() is
  // broken for prismatic joints
  // see gazebo issues 1639 and 1640
  if ( _physEng != "bullet" && _physEng != "simbody" )
  {
    for (int i = 0; i < 3; ++i)
    {
      opt.jointType = "prismatic";
      switch (i)
      {
        case 0:
          opt.jointAxis = ignition::math::Vector3d::UnitX;
          break;
        case 1:
          opt.jointAxis = ignition::math::Vector3d::UnitY;
          break;
        case 2:
          opt.jointAxis = ignition::math::Vector3d::UnitZ;
          break;
      }
      this->GetFTDemoHelper(world, _physEng, opt);
    }
  }
}

/////////////////////////////////////////////////
TEST_P(JointGetForceTorqueTest, GetForceTorqueDemo)
{
  GetForceTorqueDemo(GetParam());
}

/////////////////////////////////////////////////
INSTANTIATE_TEST_CASE_P(PhysicsEngines, JointGetForceTorqueTest,
                        PHYSICS_ENGINE_VALUES,);  // NOLINT

/////////////////////////////////////////////////
int main(int argc, char **argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
