#include <rviz/ogre_helpers/ogre_vector.h>
#include <OgreSceneNode.h>
#include <OgreSceneManager.h>

#include <rviz/ogre_helpers/shape.h>

#include <ros/ros.h>

#include "point_visual.h"

namespace rviz
{
PointStampedVisual::PointStampedVisual(Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node)
{
  scene_manager_ = scene_manager;

  // Ogre::SceneNode s form a tree, with each node storing the
  // transform (position and orientation) of itself relative to its
  // parent.  Ogre does the math of combining those transforms when it
  // is time to render.
  //
  // Here we create a node to store the pose of the Point's header frame
  // relative to the RViz fixed frame.
  frame_node_ = parent_node->createChildSceneNode();

  // We create the arrow object within the frame node so that we can
  // set its position and direction relative to its header frame.
  point_ = new rviz::Shape(rviz::Shape::Sphere, scene_manager_, frame_node_);
}

PointStampedVisual::~PointStampedVisual()
{
  // Delete the arrow to make it disappear.
  delete point_;

  // Destroy the frame node since we don't need it anymore.
  scene_manager_->destroySceneNode(frame_node_);
}


void PointStampedVisual::setMessage(const geometry_msgs::PointStamped::ConstPtr& msg)
{
  Ogre::Vector3 scale(2 * radius_, 2 * radius_, 2 * radius_);
  point_->setScale(scale);

  // Set the orientation of the arrow to match the direction of the
  // acceleration vector.
  Ogre::Vector3 point(msg->point.x, msg->point.y, msg->point.z);
  point_->setPosition(point);
}

// Position and orientation are passed through to the SceneNode.
void PointStampedVisual::setFramePosition(const Ogre::Vector3& position)
{
  frame_node_->setPosition(position);
}

void PointStampedVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
{
  frame_node_->setOrientation(orientation);
}

// Color is passed through to the rviz object.
void PointStampedVisual::setColor(float r, float g, float b, float a)
{
  point_->setColor(r, g, b, a);
}

void PointStampedVisual::setRadius(float r)
{
  radius_ = r;
}
} // end namespace rviz
