/*
 * 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.
 *
*/
#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
  #include <Winsock2.h>
#endif
#include <boost/algorithm/string.hpp>
#include "gazebo/transport/transport.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/World.hh"
#include "gazebo/physics/Model.hh"

#include "gazebo/sensors/SensorFactory.hh"
#include "gazebo/sensors/LogicalCameraSensorPrivate.hh"
#include "gazebo/sensors/LogicalCameraSensor.hh"

using namespace gazebo;
using namespace sensors;

GZ_REGISTER_STATIC_SENSOR("logical_camera", LogicalCameraSensor)

//////////////////////////////////////////////////
LogicalCameraSensor::LogicalCameraSensor()
: Sensor(sensors::OTHER),
  dataPtr(new LogicalCameraSensorPrivate)
{
}

//////////////////////////////////////////////////
LogicalCameraSensor::~LogicalCameraSensor()
{
}

//////////////////////////////////////////////////
void LogicalCameraSensor::Load(const std::string &_worldName,
                               sdf::ElementPtr _sdf)
{
  Sensor::Load(_worldName, _sdf);

  // Get a pointer to the parent link. This will be used to adjust the
  // orientation of the logical camera.
  physics::EntityPtr parentEntity =
    this->world->GetEntity(this->ParentName());
  this->dataPtr->parentLink =
    boost::dynamic_pointer_cast<physics::Link>(parentEntity);

  // Store parent model's name for use in the UpdateImpl function.
  this->dataPtr->modelName =
    this->dataPtr->parentLink->GetModel()->GetScopedName();
}

//////////////////////////////////////////////////
std::string LogicalCameraSensor::GetTopic() const
{
  return this->Topic();
}

//////////////////////////////////////////////////
std::string LogicalCameraSensor::Topic() const
{
  std::string topicName = Sensor::Topic();
  if (topicName.empty())
  {
    topicName = "~/" + this->ParentName() + "/" + this->Name() + "/models";
    boost::replace_all(topicName, "::", "/");
  }

  return topicName;
}

//////////////////////////////////////////////////
void LogicalCameraSensor::Load(const std::string &_worldName)
{
  Sensor::Load(_worldName);

  // Create publisher of the logical camera images
  this->dataPtr->pub =
    this->node->Advertise<msgs::LogicalCameraImage>(this->Topic(), 50);
}

//////////////////////////////////////////////////
void LogicalCameraSensor::Init()
{
  // Read configuration values
  if (this->sdf->HasElement("logical_camera"))
  {
    sdf::ElementPtr cameraSdf =
      this->sdf->GetElement("logical_camera");

    // These values are required in SDF, so no need to check for their
    // existence.
    this->dataPtr->frustum.SetNear(cameraSdf->Get<double>("near"));
    this->dataPtr->frustum.SetFar(cameraSdf->Get<double>("far"));
    this->dataPtr->frustum.SetFOV(cameraSdf->Get<double>("horizontal_fov"));
    this->dataPtr->frustum.SetAspectRatio(
        cameraSdf->Get<double>("aspect_ratio"));
  }

  Sensor::Init();
}

//////////////////////////////////////////////////
void LogicalCameraSensor::Fini()
{
  Sensor::Fini();
}

//////////////////////////////////////////////////
void LogicalCameraSensorPrivate::AddVisibleModels(
    ignition::math::Pose3d &_myPose, const physics::Model_V &_models)
{
  for (auto const &model : _models)
  {
    auto const &scopedName = model->GetScopedName();
    auto const aabb = model->GetBoundingBox().Ign();

    if (this->modelName != scopedName && this->frustum.Contains(aabb))
    {
      // Add new model msg
      msgs::LogicalCameraImage::Model *modelMsg = this->msg.add_model();

      // Set the name and pose reported by the sensor.
      modelMsg->set_name(scopedName);
      msgs::Set(modelMsg->mutable_pose(),
          model->GetWorldPose().Ign() - _myPose);
    }
    // Check nested models
    // Note, the model AABB does not necessarily contain the nested model
    // so nested models must be searched even if the frustum does not contain
    // the parent model.
    AddVisibleModels(_myPose, model->NestedModels());
  }
}

//////////////////////////////////////////////////
bool LogicalCameraSensor::UpdateImpl(const bool _force)
{
  // Only compute if active, or the update is forced
  if (_force || this->IsActive())
  {
    std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
    this->dataPtr->msg.clear_model();

    // Get the pose of the camera's parent.
    ignition::math::Pose3d myPose = this->pose +
      this->dataPtr->parentLink->GetWorldPose().Ign();

    // Update the pose of the frustum.
    this->dataPtr->frustum.SetPose(myPose);

    // Set the camera's pose in the message.
    msgs::Set(this->dataPtr->msg.mutable_pose(), myPose);

    // Recursively check if models and nested models are in the frustum.
    this->dataPtr->AddVisibleModels(myPose, this->world->GetModels());

    // Send the message.
    this->dataPtr->pub->Publish(this->dataPtr->msg);
  }

  return true;
}

//////////////////////////////////////////////////
bool LogicalCameraSensor::IsActive() const
{
  return Sensor::IsActive() ||
    (this->dataPtr->pub && this->dataPtr->pub->HasConnections());
}

//////////////////////////////////////////////////
double LogicalCameraSensor::Near() const
{
  return this->dataPtr->frustum.Near();
}

//////////////////////////////////////////////////
double LogicalCameraSensor::Far() const
{
  return this->dataPtr->frustum.Far();
}

//////////////////////////////////////////////////
ignition::math::Angle LogicalCameraSensor::HorizontalFOV() const
{
  return this->dataPtr->frustum.FOV();
}

//////////////////////////////////////////////////
double LogicalCameraSensor::AspectRatio() const
{
  return this->dataPtr->frustum.AspectRatio();
}

//////////////////////////////////////////////////
msgs::LogicalCameraImage LogicalCameraSensor::Image() const
{
  std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
  return this->dataPtr->msg;
}