/*
 * Copyright (C) 2014 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 "gazebo/gui/GuiIface.hh"
#include "gazebo/gui/ModelAlign.hh"

#include "gazebo/rendering/RenderEvents.hh"
#include "gazebo/rendering/RenderTypes.hh"
#include "gazebo/gui/ModelAlign_TEST.hh"

#include "test_config.h"

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignXMin()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "min", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMinX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X() - targetBbox.XLength()/2.0;
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X() -
        bbox.XLength()/2.0;
    QVERIFY(ignition::math::equal(minX, targetMinX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignXMinReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "min", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMinX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X() - targetBbox.XLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X() +
        bbox.XLength()/2.0;
    QVERIFY(ignition::math::equal(minX, targetMinX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignXCenter()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "center", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetCenterX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X();

  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double centerX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X();
    QVERIFY(ignition::math::equal(centerX, targetCenterX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignXMax()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "max", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMaxX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X() + targetBbox.XLength()/2.0;

  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X()
        + bbox.XLength()/2.0;
    QVERIFY(ignition::math::equal(maxX, targetMaxX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignXMaxReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "max", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMaxX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X() + targetBbox.XLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X() -
        bbox.XLength()/2.0;
    QVERIFY(ignition::math::equal(maxX, targetMaxX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignYMin()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "y", "min", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMinY = modelVisuals[0]->GetWorldPose().Ign().Pos().Y() +
      centerOffsets[0].Y() - targetBbox.YLength()/2.0;
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minY = vis->GetWorldPose().Ign().Pos().Y() + centerOffsets[i].Y() -
        bbox.YLength()/2.0;
    QVERIFY(ignition::math::equal(minY, targetMinY, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignYMinReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "y", "min", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMinY = modelVisuals[0]->GetWorldPose().Ign().Pos().Y() +
      centerOffsets[0].Y() - targetBbox.YLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minY = vis->GetWorldPose().Ign().Pos().Y() + centerOffsets[i].Y() +
        bbox.YLength()/2.0;
    QVERIFY(ignition::math::equal(minY, targetMinY, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignYCenter()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "y", "center", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetCenterY = modelVisuals[0]->GetWorldPose().Ign().Pos().Y() +
      centerOffsets[0].Y();


  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double centerY = vis->GetWorldPose().Ign().Pos().Y() + centerOffsets[i].Y();
    QVERIFY(ignition::math::equal(centerY, targetCenterY, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignYMax()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "y", "max", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMaxY = modelVisuals[0]->GetWorldPose().Ign().Pos().Y() +
      centerOffsets[0].Y() + targetBbox.YLength()/2.0;

  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxY = vis->GetWorldPose().Ign().Pos().Y() + centerOffsets[i].Y()
        + bbox.YLength()/2.0;
    QVERIFY(ignition::math::equal(maxY, targetMaxY, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignYMaxReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "y", "max", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMaxY = modelVisuals[0]->GetWorldPose().Ign().Pos().Y() +
      centerOffsets[0].Y() + targetBbox.YLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxY = vis->GetWorldPose().Ign().Pos().Y() + centerOffsets[i].Y() -
        bbox.YLength()/2.0;
    QVERIFY(ignition::math::equal(maxY, targetMaxY, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignZMin()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "min", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMinZ = modelVisuals[0]->GetWorldPose().Ign().Pos().Z() +
      centerOffsets[0].Z() - targetBbox.ZLength()/2.0;
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minZ = vis->GetWorldPose().Ign().Pos().Z() + centerOffsets[i].Z() -
        bbox.ZLength()/2.0;
    QVERIFY(ignition::math::equal(minZ, targetMinZ, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignZMinReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "min", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMinZ = modelVisuals[0]->GetWorldPose().Ign().Pos().Z() +
      centerOffsets[0].Z() - targetBbox.ZLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double minZ = vis->GetWorldPose().Ign().Pos().Z() + centerOffsets[i].Z() +
        bbox.ZLength()/2.0;
    QVERIFY(ignition::math::equal(minZ, targetMinZ, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignZCenter()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "center", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetCenterZ = modelVisuals[0]->GetWorldPose().Ign().Pos().Z() +
      centerOffsets[0].Z();

  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double centerZ = vis->GetWorldPose().Ign().Pos().Z() + centerOffsets[i].Z();
    QVERIFY(ignition::math::equal(centerZ, targetCenterZ, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignZMax()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/align.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "max", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();

  double targetMaxZ = modelVisuals[0]->GetWorldPose().Ign().Pos().Z() +
      centerOffsets[0].Z() + targetBbox.ZLength()/2.0;

  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxZ = vis->GetWorldPose().Ign().Pos().Z() + centerOffsets[i].Z()
        + bbox.ZLength()/2.0;
    QVERIFY(ignition::math::equal(maxZ, targetMaxZ, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignZMaxReverse()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  // Load the align world
  this->Load("worlds/align.world", false, false, true);

  // Get the scene
  auto scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Check all visuals are there
  std::vector<std::string> modelNames;
  modelNames.push_back("Dumpster");
  modelNames.push_back("bookshelf");
  modelNames.push_back("table");
  modelNames.push_back("jersey_barrier");

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  // Get the offsets for all models
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // Align visuals
  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "max", "first", true, true);

  // Get the target pose
  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  double targetMaxZ = modelVisuals[0]->GetWorldPose().Ign().Pos().Z() +
      centerOffsets[0].Z() + targetBbox.ZLength()/2.0;

  // Check models were properly aligned
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    auto vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();

    double maxZ = vis->GetWorldPose().Ign().Pos().Z() + centerOffsets[i].Z() -
        bbox.ZLength()/2.0;
    QVERIFY(ignition::math::equal(maxZ, targetMaxZ, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::AlignScale()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/shapes.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  std::vector<std::string> modelNames;
  modelNames.push_back("box");
  modelNames.push_back("cylinder");
  modelNames.push_back("sphere");

  gazebo::event::Events::preRender();

  int sleep  = 0;
  int maxSleep = 200;
  while (!scene->Initialized() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  sleep = 0;
  unsigned int modelVisualCount = 0;
  while (modelVisualCount != modelNames.size() && sleep < maxSleep)
  {
    gazebo::event::Events::preRender();
    modelVisualCount = 0;
    for (unsigned int i = 0; i < modelNames.size(); ++i)
    {
      if (scene->GetVisual(modelNames[i]))
        modelVisualCount++;
    }
    gazebo::common::Time::MSleep(30);
    sleep++;
  }

  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  std::vector<ignition::math::Vector3d> centerOffsets;
  for (unsigned int i = 0; i < modelNames.size(); ++i)
  {
    gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNames[i]);
    QVERIFY(modelVis != NULL);
    auto modelCenterOffset = modelVis->GetBoundingBox().Ign().Center();
    modelVisuals.push_back(modelVis);
    centerOffsets.push_back(modelCenterOffset);
  }

  // manually change scale of model visual and verify
  gazebo::rendering::VisualPtr targetVis = modelVisuals[0];
  targetVis->SetScale(ignition::math::Vector3d(1.5, 1, 1));
  QVERIFY(targetVis->GetScale().Ign() == ignition::math::Vector3d(1.5, 1, 1));

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "min", "first");

  auto targetBbox = modelVisuals[0]->GetBoundingBox().Ign();
  auto targetScale = modelVisuals[0]->GetScale().Ign();

  // verify other models align at minx of the scaled target model
  double targetMinX = modelVisuals[0]->GetWorldPose().Ign().Pos().X() +
      centerOffsets[0].X() - targetScale.X() * targetBbox.XLength()/2.0;
  for (unsigned int i = 1; i < modelVisuals.size(); ++i)
  {
    gazebo::rendering::VisualPtr vis = modelVisuals[i];
    auto bbox = vis->GetBoundingBox().Ign();
    auto visScale = vis->GetScale().Ign();

    double minX = vis->GetWorldPose().Ign().Pos().X() + centerOffsets[i].X() -
        visScale.X() * bbox.XLength()/2.0;
    QVERIFY(ignition::math::equal(minX, targetMinX, 1e-5));
  }
}

/////////////////////////////////////////////////
void ModelAlign_TEST::SetHighlighted()
{
  this->resMaxPercentChange = 5.0;
  this->shareMaxPercentChange = 2.0;

  this->Load("worlds/blank.world", false, false, true);

  gazebo::rendering::ScenePtr scene;
  scene = gazebo::rendering::get_scene("default");
  QVERIFY(scene != NULL);

  // Create a deeply nested visual where each level has a different transparency
  gazebo::rendering::VisualPtr vis1;
  vis1.reset(new gazebo::rendering::Visual("vis1", scene->WorldVisual()));
  vis1->Load();
  double vis1Transp = 0.0;
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis1->GetTransparency()), vis1Transp, 1e-5));

  gazebo::rendering::VisualPtr vis2;
  vis2.reset(new gazebo::rendering::Visual("vis2", vis1));
  vis2->Load();
  double vis2Transp = 0.6;
  vis2->SetTransparency(vis2Transp);
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis2->GetTransparency()), vis2Transp, 1e-5));

  gazebo::rendering::VisualPtr vis3_1;
  vis3_1.reset(new gazebo::rendering::Visual("vis3_1", vis2));
  vis3_1->Load();
  double vis3_1Transp = 0.25;
  vis3_1->SetTransparency(vis3_1Transp);
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis3_1->GetTransparency()), vis3_1Transp, 1e-5));

  gazebo::rendering::VisualPtr vis3_2;
  vis3_2.reset(new gazebo::rendering::Visual("vis3_2_LEAF", vis2));
  vis3_2->Load();
  double vis3_2Transp = 1.0;
  vis3_2->SetTransparency(vis3_2Transp);
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));

  gazebo::rendering::VisualPtr vis4;
  vis4.reset(new gazebo::rendering::Visual("vis4_LEAF", vis3_1));
  vis4->Load();
  double vis4Transp = 0.9;
  vis4->SetTransparency(vis4Transp);
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));

  // Create another model just to align them
  gazebo::rendering::VisualPtr otherVis;
  otherVis.reset(
      new gazebo::rendering::Visual("otherVis", scene->WorldVisual()));
  otherVis->Load();

  // Align preview
  std::vector<gazebo::rendering::VisualPtr> modelVisuals;
  modelVisuals.push_back(vis1);
  modelVisuals.push_back(otherVis);

  gazebo::gui::ModelAlign::Instance()->Init();
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "min", "last", false);

  this->ProcessEventsAndDraw();

  // Check that the transparency of the leaves have changed
  QVERIFY(!ignition::math::equal(
      static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
  QVERIFY(!ignition::math::equal(
      static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));

  // Reset
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "x", "reset", "last", false);

  this->ProcessEventsAndDraw();

  // Check that the transparency of the leaves have the original value
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));

  // Align preview again
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "max", "last", false);

  this->ProcessEventsAndDraw();

  // Check that the transparency of the leaves have changed
  QVERIFY(!ignition::math::equal(
      static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
  QVERIFY(!ignition::math::equal(
      static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));

  // Publish the position
  gazebo::gui::ModelAlign::Instance()->AlignVisuals(
      modelVisuals, "z", "max", "last", true);

  this->ProcessEventsAndDraw();

  // Check that the transparency of the leaves have the original value
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis3_2->GetTransparency()), vis3_2Transp, 1e-5));
  QVERIFY(ignition::math::equal(
      static_cast<double>(vis4->GetTransparency()), vis4Transp, 1e-5));
}

// Generate a main function for the test
QTEST_MAIN(ModelAlign_TEST)