/*
 * Copyright (C) 2012 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 <ignition/math/Rand.hh>

#include "gazebo/test/ServerFixture.hh"
#include "gazebo/gazebo_config.h"
#include "gazebo/math/SignalStats.hh"
#include "gazebo/physics/physics.hh"
#include "SimplePendulumIntegrator.hh"
#include "gazebo/test/helper_physics_generator.hh"
#include "test/integration/joint_test.hh"

using namespace gazebo;
const double g_tolerance = 1e-2;

class JointTestRevolute : public JointTest
{
  /// \brief Spawn single pendulum and test GetWorldEnergy functions.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void PendulumEnergy(const std::string &_physicsEngine);

  /// \brief Spin joints several rotations and verify that the angles
  /// are properly unwrapped.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void UnwrappedAngle(const std::string &_physicsEngine);

  /// \brief Load 8 double pendulums arranged in a circle.
  /// Measure angular velocity of links, and verify proper axis orientation.
  /// Then set joint limits and verify that links remain within limits.
  /// \param[in] _physicsEngine Type of physics engine to use.
  /// \param[in] _solverType Type of solver to use.
  public: void RevoluteJoint(const std::string &_physicsEngine,
                             const std::string &_solverType="quick");

  /// \brief Load a simple pendulum, simulate and compare to numerical
  /// results from SimplePendulumIntegrator.
  /// \param[in] _physicsEngine Type of physics engine to use.
  public: void SimplePendulum(const std::string &_physicsEngine);
};

////////////////////////////////////////////////////////////
void JointTestRevolute::PendulumEnergy(const std::string &_physicsEngine)
{
  // Load an empty world
  Load("worlds/empty.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // Verify physics engine type
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);

  {
    SpawnJointOptions opt;
    opt.type = "revolute";
    opt.worldParent = true;
    double Am = M_PI / 4;
    opt.modelPose.rot.SetFromEuler(0, 0, Am);
    opt.childLinkPose.pos.z = 3.0;
    opt.jointPose.pos.y = 1.5;
    opt.axis.Set(1, 0, 0);

    gzdbg << "SpawnJoint " << opt.type << " child world" << std::endl;
    physics::JointPtr joint = SpawnJoint(opt);
    ASSERT_TRUE(joint != NULL);

    // Get initial energy
    physics::LinkPtr link = joint->GetChild();
    ASSERT_TRUE(link != NULL);
    physics::ModelPtr model = link->GetModel();
    ASSERT_TRUE(model != NULL);

    double energy0 = model->GetWorldEnergy();
    EXPECT_NEAR(model->GetWorldEnergyKinetic(), 0.0, g_tolerance);

    unsigned int stepSize = 5;
    unsigned int stepCount = 500;
    for (unsigned int i = 0; i < stepCount; ++i)
    {
      world->Step(stepSize);
      double energy = model->GetWorldEnergy();
      EXPECT_NEAR(energy / energy0, 1.0, g_tolerance);
    }
  }
}

////////////////////////////////////////////////////////////
void JointTestRevolute::UnwrappedAngle(const std::string &_physicsEngine)
{
  // Load an empty world
  Load("worlds/empty.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // Verify physics engine type
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);

  // disable gravity
  physics->SetGravity(math::Vector3::Zero);

  {
    std::string jointType = "revolute";
    gzdbg << "SpawnJoint " << jointType << " child world" << std::endl;
    physics::JointPtr joint = SpawnJoint(jointType, false, true);
    ASSERT_TRUE(joint != NULL);

    // expect large joint limits
    EXPECT_DOUBLE_EQ(-1e16, joint->GetLowerLimit(0).Radian());
    EXPECT_DOUBLE_EQ(+1e16, joint->GetUpperLimit(0).Radian());
    EXPECT_DOUBLE_EQ(-1e16, joint->GetLowStop(0).Radian());
    EXPECT_DOUBLE_EQ(+1e16, joint->GetHighStop(0).Radian());

    // set velocity to 2 pi rad/s and step forward 1.5 seconds.
    // angle should reach 3 pi rad.
    double vel = 2*M_PI;
    unsigned int stepSize = 50;
    unsigned int stepCount = 30;
    double dt = physics->GetMaxStepSize();

    // Verify that the joint should make more than 1 revolution
    EXPECT_GT(vel * stepSize * stepCount * dt, 1.25 * 2 * M_PI);

    joint->SetVelocity(0, vel);

    math::SignalMaxAbsoluteValue angleErrorMax;
    // expect that joint velocity is constant
    // and that joint angle is unwrapped
    for (unsigned int i = 0; i < stepCount; ++i)
    {
      world->Step(stepSize);
      EXPECT_NEAR(joint->GetVelocity(0), vel, g_tolerance);
      double time = world->GetSimTime().Double();
      angleErrorMax.InsertData(joint->GetAngle(0).Radian() - time*vel);
    }
#ifndef LIBBULLET_VERSION_GT_282
    if (_physicsEngine == "bullet")
    {
      gzerr << "Skipping portion of test, angle wrapping requires bullet 2.83"
            << std::endl;
    }
    else
#endif
    {
      EXPECT_NEAR(angleErrorMax.Value(), 0.0, g_tolerance);
    }
  }
}


////////////////////////////////////////////////////////////////////////
void JointTestRevolute::RevoluteJoint(const std::string &_physicsEngine,
                                      const std::string &_solverType)
{
  ignition::math::Rand::Seed(0);
  // Load world
  Load("worlds/revolute_joint_test.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

  // Verify physics engine type
  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
  ASSERT_TRUE(physics != NULL);
  EXPECT_EQ(physics->GetType(), _physicsEngine);

  // Set solver type
  physics->SetParam("solver_type", _solverType);
  if (_solverType == "world")
  {
    physics->SetParam("ode_quiet", true);
  }

  // Model names
  std::vector<std::string> modelNames;
  modelNames.push_back("pendulum_0deg");
  modelNames.push_back("pendulum_45deg");
  modelNames.push_back("pendulum_90deg");
  modelNames.push_back("pendulum_135deg");
  modelNames.push_back("pendulum_180deg");
  modelNames.push_back("pendulum_225deg");
  modelNames.push_back("pendulum_270deg");
  modelNames.push_back("pendulum_315deg");

  // Global axis
  double sqrt1_2 = sqrt(2.0) / 2.0;
  std::vector<math::Vector3> globalAxes;
  globalAxes.push_back(math::Vector3(1, 0, 0));
  globalAxes.push_back(math::Vector3(sqrt1_2, sqrt1_2, 0));
  globalAxes.push_back(math::Vector3(0, 1, 0));
  globalAxes.push_back(math::Vector3(-sqrt1_2, sqrt1_2, 0));
  globalAxes.push_back(math::Vector3(-1, 0, 0));
  globalAxes.push_back(math::Vector3(-sqrt1_2, -sqrt1_2, 0));
  globalAxes.push_back(math::Vector3(0, -1, 0));
  globalAxes.push_back(math::Vector3(sqrt1_2, -sqrt1_2, 0));

  // Link names
  std::vector<std::string> linkNames;
  linkNames.push_back("lower_link");
  linkNames.push_back("upper_link");

  // Joint names
  std::vector<std::string> jointNames;
  jointNames.push_back("lower_joint");
  jointNames.push_back("upper_joint");

  physics::ModelPtr model;
  physics::LinkPtr link;
  std::vector<std::string>::iterator modelIter;
  physics::JointPtr joint;

  double energy0 = 1.0;
  // Check global axes before simulation starts
  std::vector<math::Vector3>::iterator axisIter;
  axisIter = globalAxes.begin();
  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      energy0 = model->GetWorldEnergy();
      gzdbg << "Check global axes of model " << *modelIter << '\n';
      std::vector<std::string>::iterator jointIter;
      for (jointIter  = jointNames.begin();
           jointIter != jointNames.end(); ++jointIter)
      {
        joint = model->GetJoint(*jointIter);
        if (joint)
        {
          math::Vector3 axis = joint->GetGlobalAxis(0);
          EXPECT_NEAR(axis.x, (*axisIter).x, g_tolerance);
          EXPECT_NEAR(axis.y, (*axisIter).y, g_tolerance);
          EXPECT_NEAR(axis.z, (*axisIter).z, g_tolerance);
        }
        else
        {
          gzerr << "Error loading joint " << *jointIter
                << " of model " << *modelIter << '\n';
          EXPECT_TRUE(joint != NULL);
        }
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
    ++axisIter;
  }

  // Step forward 0.75 seconds
  double dt = physics->GetMaxStepSize();
  ASSERT_GT(dt, 0);
  int steps = ceil(0.75 / dt);
  world->Step(steps);

  // Get global angular velocity of each link
  math::Vector3 angVel;
  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      EXPECT_NEAR(model->GetWorldEnergy() / energy0, 1.0, g_tolerance);
      gzdbg << "Check angular velocity of model " << *modelIter << '\n';
      link = model->GetLink("base");
      if (link)
      {
        // Expect stationary base
        angVel = link->GetWorldAngularVel();
        EXPECT_NEAR(angVel.x, 0, g_tolerance*10);
        EXPECT_NEAR(angVel.y, 0, g_tolerance*10);
        EXPECT_NEAR(angVel.z, 0, g_tolerance*10);
      }
      else
      {
        gzerr << "Error loading base link of model " << *modelIter << '\n';
        EXPECT_TRUE(link != NULL);
      }

      std::vector<std::string>::iterator linkIter;
      for (linkIter  = linkNames.begin();
           linkIter != linkNames.end(); ++linkIter)
      {
        link = model->GetLink(*linkIter);
        if (link)
        {
          // Expect relative angular velocity of pendulum links to be negative
          // and along x axis.
          angVel = link->GetRelativeAngularVel().Normalize();
          EXPECT_NEAR(angVel.x, -1, g_tolerance);
          EXPECT_NEAR(angVel.y,  0, 2*g_tolerance);
          EXPECT_NEAR(angVel.z,  0, 2*g_tolerance);
        }
        else
        {
          gzerr << "Error loading link " << *linkIter
                << " of model " << *modelIter << '\n';
          EXPECT_TRUE(link != NULL);
        }
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
  }

  // Keep stepping forward, verifying that joint angles move in the direction
  // implied by the joint velocity
  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      EXPECT_NEAR(model->GetWorldEnergy() / energy0, 1.0, g_tolerance);
      double jointVel1, jointVel2;
      double angle1, angle2, angle3;

      gzdbg << "Check angle measurement for " << *modelIter << '\n';
      std::vector<std::string>::iterator jointIter;
      for (jointIter  = jointNames.begin();
           jointIter != jointNames.end(); ++jointIter)
      {
        joint = model->GetJoint(*jointIter);
        if (joint)
        {
          // Get first joint angle
          angle1 = joint->GetAngle(0).Radian();

          // Get joint velocity and assume it is not too small
          jointVel1 = joint->GetVelocity(0);
          EXPECT_GT(fabs(jointVel1), 1e-1);

          // Take 1 step and measure again
          world->Step(1);

          // Expect angle change in direction of joint velocity
          angle2 = joint->GetAngle(0).Radian();
          EXPECT_GT((angle2 - angle1) * math::clamp(jointVel1*1e4, -1.0, 1.0)
                    , 0);

          jointVel2 = joint->GetVelocity(0);
          EXPECT_GT(fabs(jointVel2), 1e-1);

          // Take 1 step and measure the last angle, expect decrease
          world->Step(1);
          angle3 = joint->GetAngle(0).Radian();
          EXPECT_GT((angle3 - angle2) * math::clamp(jointVel2*1e4, -1.0, 1.0)
                    , 0);
        }
        else
        {
          gzerr << "Error loading joint " << *jointIter
                << " of model " << *modelIter << '\n';
          EXPECT_TRUE(joint != NULL);
        }
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
  }


  // Reset the world, and impose joint limits
  world->Reset();

  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      std::vector<std::string>::iterator jointIter;
      for (jointIter  = jointNames.begin();
           jointIter != jointNames.end(); ++jointIter)
      {
        joint = model->GetJoint(*jointIter);
        if (joint)
        {
          joint->SetLowStop(0, math::Angle(-0.1));
          joint->SetHighStop(0, math::Angle(0.1));
        }
        else
        {
          gzerr << "Error loading joint " << *jointIter
                << " of model " << *modelIter << '\n';
          EXPECT_TRUE(joint != NULL);
        }
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
  }

  // Step forward again for 0.75 seconds and check that joint angles
  // are within limits
  world->Step(steps);
  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      gzdbg << "Check angle limits and velocity of joints of model "
            << *modelIter << '\n';
      std::vector<std::string>::iterator jointIter;
      for (jointIter  = jointNames.begin();
           jointIter != jointNames.end(); ++jointIter)
      {
        joint = model->GetJoint(*jointIter);
        if (joint)
        {
          EXPECT_NEAR(joint->GetAngle(0).Radian(), 0, 0.11);
        }
        else
        {
          gzerr << "Error loading joint " << *jointIter
                << " of model " << *modelIter << '\n';
          EXPECT_TRUE(joint != NULL);
        }
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
  }

  // Reset world again, disable gravity, detach upper_joint
  // Then apply torque at lower_joint and verify motion
  world->Reset();
  physics->SetGravity(math::Vector3::Zero);
  for (modelIter  = modelNames.begin();
       modelIter != modelNames.end(); ++modelIter)
  {
    model = world->GetModel(*modelIter);
    if (model)
    {
      gzdbg << "Check SetForce for model " << *modelIter << '\n';

      joint = model->GetJoint("upper_joint");
      if (joint)
      {
        if (_physicsEngine == "simbody" ||
            _physicsEngine == "dart")
        {
          gzerr << "Skipping joint detachment per #862 / #903" << std::endl;
          continue;
        }
        // Detach upper_joint.
        joint->Detach();
        // Simbody and DART will not easily detach joints,
        // consider freezing joint limit instead
        // math::Angle curAngle = joint->GetAngle(0u);
        // joint->SetLowStop(0, curAngle);
        // joint->SetHighStop(0, curAngle);
      }
      else
      {
        gzerr << "Error loading upper_joint "
              << " of model " << *modelIter << '\n';
        EXPECT_TRUE(joint != NULL);
      }

      // Step forward and let things settle a bit.
      world->Step(100);

      joint = model->GetJoint("lower_joint");
      if (joint)
      {
        double oldVel, newVel, force;
        oldVel = joint->GetVelocity(0);
        // Apply positive torque to the lower_joint and step forward.
        force = 1;

        for (int i = 0; i < 10; ++i)
        {
          joint->SetForce(0, force);
          world->Step(1);
          joint->SetForce(0, force);
          world->Step(1);
          joint->SetForce(0, force);
          world->Step(1);
          newVel = joint->GetVelocity(0);

          // gzdbg << "model " << *modelIter
          //       << "  i " << i
          //       << "  oldVel " << oldVel
          //       << "  newVel " << newVel
          //       << "  upper joint v "
          //       << model->GetJoint("upper_joint")->GetVelocity(0)
          //       << " joint " << joint->GetName() <<  "\n";

          // Expect increasing velocities
          EXPECT_GT(newVel, oldVel);
          oldVel = newVel;

          // Check that GetForce returns what we set
          EXPECT_NEAR(joint->GetForce(0u), force, g_tolerance);

          // Expect joint velocity to be near angular velocity difference
          // of child and parent, along global axis
          // jointVel == DOT(angVelChild - angVelParent, axis)
          double jointVel = joint->GetVelocity(0);
          math::Vector3 axis = joint->GetGlobalAxis(0);
          angVel  = joint->GetChild()->GetWorldAngularVel();
          angVel -= joint->GetParent()->GetWorldAngularVel();
          EXPECT_NEAR(jointVel, axis.Dot(angVel), g_tolerance);
        }
        // Apply negative torque to lower_joint
        force = -3;
        for (int i = 0; i < 10; ++i)
        {
          joint->SetForce(0, force);
          world->Step(1);
          joint->SetForce(0, force);
          world->Step(1);
          joint->SetForce(0, force);
          world->Step(1);
          newVel = joint->GetVelocity(0);

          // gzdbg << "model " << *modelIter
          //       << "  i " << i
          //       << "  oldVel " << oldVel
          //       << "  newVel " << newVel
          //       << "  upper joint v "
          //       << model->GetJoint("upper_joint")->GetVelocity(0)
          //       << " joint " << joint->GetName() <<  "\n";

          // Expect decreasing velocities
          EXPECT_LT(newVel, oldVel);

          // Check that GetForce returns what we set
          EXPECT_NEAR(joint->GetForce(0u), force, g_tolerance);

          // Expect joint velocity to be near angular velocity difference
          // of child and parent, along global axis
          // jointVel == DOT(angVelChild - angVelParent, axis)
          double jointVel = joint->GetVelocity(0);
          math::Vector3 axis = joint->GetGlobalAxis(0);
          angVel  = joint->GetChild()->GetWorldAngularVel();
          angVel -= joint->GetParent()->GetWorldAngularVel();
          EXPECT_NEAR(jointVel, axis.Dot(angVel), g_tolerance);
        }
      }
      else
      {
        gzerr << "Error loading lower_joint "
              << " of model " << *modelIter << '\n';
        EXPECT_TRUE(joint != NULL);
      }
    }
    else
    {
      gzerr << "Error loading model " << *modelIter << '\n';
      EXPECT_TRUE(model != NULL);
    }
  }
}

////////////////////////////////////////////////////////////////////////
void JointTestRevolute::SimplePendulum(const std::string &_physicsEngine)
{
  Load("worlds/simple_pendulums.world", true, _physicsEngine);
  physics::WorldPtr world = physics::get_world("default");
  ASSERT_TRUE(world != NULL);

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

  int i = 0;
  while (!this->HasEntity("model_1") && i < 20)
  {
    common::Time::MSleep(100);
    ++i;
  }

  if (i > 20)
    gzthrow("Unable to get model_1");

  physics::PhysicsEnginePtr physicsEngine = world->GetPhysicsEngine();
  EXPECT_TRUE(physicsEngine != NULL);
  physics::ModelPtr model = world->GetModel("model_1");
  EXPECT_TRUE(model != NULL);
  physics::LinkPtr link = model->GetLink("link_2");  // sphere link at end
  EXPECT_TRUE(link != NULL);

  double g = 9.81;
  double l = 10.0;
  double m = 10.0;

  double e_start;

  {
    // check velocity / energy
    math::Vector3 vel = link->GetWorldLinearVel();
    math::Pose pos = link->GetWorldPose();
    double pe = g * m * pos.pos.z;
    double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
    e_start = pe + ke;
    // gzdbg << "total energy [" << e_start
    //       << "] pe[" << pe
    //       << "] ke[" << ke
    //       << "] p[" << pos.pos.z
    //       << "] v[" << vel
    //       << "]\n";
  }
  physicsEngine->SetMaxStepSize(0.0001);
  physicsEngine->SetParam("iters", 1000);

  {
    // test with global contact_max_correcting_vel at 0 as set by world file
    //   here we expect significant energy loss as the velocity correction
    //   is set to 0
    int steps = 10;  // @todo: make this more general
    for (int i = 0; i < steps; i ++)
    {
      world->Step(2000);
      {
        // check velocity / energy
        math::Vector3 vel = link->GetWorldLinearVel();
        math::Pose pos = link->GetWorldPose();
        double pe = g * m * pos.pos.z;
        double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
        double e = pe + ke;
        double e_tol = 3.0*static_cast<double>(i+1)
          / static_cast<double>(steps);
        // gzdbg << "total energy [" << e
        //       << "] pe[" << pe
        //       << "] ke[" << ke
        //       << "] p[" << pos.pos.z
        //       << "] v[" << vel
        //       << "] error[" << e - e_start
        //       << "] tol[" << e_tol
        //       << "]\n";

        EXPECT_LT(fabs(e - e_start), e_tol);
      }

      physics::JointPtr joint = model->GetJoint("joint_0");
      if (joint)
      {
        double integ_theta = (
          PendulumAngle(g, l, 1.57079633, 0.0, world->GetSimTime().Double(),
          0.000001) - 1.5707963);
        double actual_theta = joint->GetAngle(0).Radian();
        // gzdbg << "time [" << world->GetSimTime().Double()
        //       << "] exact [" << integ_theta
        //       << "] actual [" << actual_theta
        //       << "] pose [" << model->GetWorldPose()
        //       << "]\n";
         EXPECT_LT(fabs(integ_theta - actual_theta) , 0.01);
      }
    }
  }



  {
    // test with global contact_max_correcting_vel at 100
    // here we expect much lower energy loss
    world->Reset();
    if (_physicsEngine == "ode")
      EXPECT_TRUE(physicsEngine->SetParam("contact_max_correcting_vel", 100.0));

    int steps = 10;  // @todo: make this more general
    for (int i = 0; i < steps; i ++)
    {
      world->Step(2000);
      {
        // check velocity / energy
        math::Vector3 vel = link->GetWorldLinearVel();
        math::Pose pos = link->GetWorldPose();
        double pe = g * m * pos.pos.z;
        double ke = 0.5 * m * (vel.x*vel.x + vel.y*vel.y + vel.z*vel.z);
        double e = pe + ke;
        double e_tol = 3.0*static_cast<double>(i+1)
          / static_cast<double>(steps);
        // gzdbg << "total energy [" << e
        //       << "] pe[" << pe
        //       << "] ke[" << ke
        //       << "] p[" << pos.pos.z
        //       << "] v[" << vel
        //       << "] error[" << e - e_start
        //       << "] tol[" << e_tol
        //       << "]\n";

        EXPECT_LT(fabs(e - e_start), e_tol);
      }

      physics::JointPtr joint = model->GetJoint("joint_0");
      if (joint)
      {
        double integ_theta = (
          PendulumAngle(g, l, 1.57079633, 0.0, world->GetSimTime().Double(),
          0.000001) - 1.5707963);
        double actual_theta = joint->GetAngle(0).Radian();
        // gzdbg << "time [" << world->GetSimTime().Double()
        //       << "] exact [" << integ_theta
        //       << "] actual [" << actual_theta
        //       << "] pose [" << model->GetWorldPose()
        //       << "]\n";
         EXPECT_LT(fabs(integ_theta - actual_theta) , 0.01);
      }
    }
  }
}

TEST_P(JointTestRevolute, PendulumEnergy)
{
  PendulumEnergy(this->physicsEngine);
}

TEST_P(JointTestRevolute, RevoluteJoint)
{
  RevoluteJoint(this->physicsEngine);
}

TEST_F(JointTestRevolute, RevoluteJointWorldStep)
{
  RevoluteJoint("ode", "world");
}

TEST_P(JointTestRevolute, SimplePendulum)
{
  SimplePendulum(this->physicsEngine);
}

TEST_P(JointTestRevolute, UnwrappedAngle)
{
  UnwrappedAngle(this->physicsEngine);
}

INSTANTIATE_TEST_CASE_P(PhysicsEngines, JointTestRevolute,
  ::testing::Combine(PHYSICS_ENGINE_VALUES,
  ::testing::Values("revolute")));

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