#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>

#include <tf/transform_listener.h>

#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/frame_manager.h>

#include "perception_visual.h"
#include "perceptions_display.h"

namespace rviz_multiception
{

// The constructor must have no arguments, so we can't give the
// constructor the parameters it needs to fully initialize.
PerceptionsDisplay::PerceptionsDisplay()
{
	color_property_ = new rviz::ColorProperty( "Color", QColor( 204, 51, 204 ),
	                                           "Color to draw the arrows.",
	                                           this, SLOT( updateColorAndAlpha() ));

	alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
	                                           "Object transparency, 0 is fully transparent, 1.0 is fully opaque.",
	                                           this, SLOT( updateColorAndAlpha() ));
}

// After the top-level rviz::Display::initialize() does its own setup,
// it calls the subclass's onInitialize() function. This is where we
// instantiate all the workings of the class. We make sure to also
// call our immediate super-class's onInitialize() function, since it
// does important stuff setting up the message filter.
//
//  Note that "MFDClass" is a typedef of
// ``MessageFilterDisplay<message type>``, to save typing that long
// templated class name every time you need to refer to the
// superclass.
void PerceptionsDisplay::onInitialize()
{
	MFDClass::onInitialize();
}

PerceptionsDisplay::~PerceptionsDisplay()
{
}

// Clear the visuals by deleting their objects.
void PerceptionsDisplay::reset()
{
	MFDClass::reset();
	visuals_.clear();
}

// Set the current color and alpha values for each visual.
void PerceptionsDisplay::updateColorAndAlpha()
{
	float alpha = alpha_property_->getFloat();
	Ogre::ColourValue color = color_property_->getOgreColor();

	for(size_t i=0; i<visuals_.size(); i++)
	{
		visuals_[i]->setColor(color.r, color.g, color.b, alpha);
	}
}

// This is our callback to handle an incoming message.
void PerceptionsDisplay::processMessage(const multiception::Perceptions::ConstPtr& msg)
{
	// Here we call the rviz::FrameManager to get the transform from the
	// fixed frame to the frame in the header of this message. If
	// it fails, we can't do anything else so we return.
	const size_t N = msg->perceptions.size();
	if(N==0)
	{
		return;
	}

	// We are keeping a vector of visual pointers as well as the originating message to avoid it
	// being cleared while visual still references its content.
	msg_ = boost::static_pointer_cast<const multiception::Perceptions>(msg);
	visuals_.clear();
	visuals_.reserve(N);

	for(const multiception::Perception& perception : msg->perceptions)
	{
		Ogre::Quaternion orientation;
		Ogre::Vector3 position;

		if(!context_->getFrameManager()->getTransform(perception.header.frame_id, perception.header.stamp, position, orientation))
		{
			ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", perception.header.frame_id.c_str(), qPrintable(fixed_frame_));
			return;
		}

		boost::shared_ptr<PerceptionVisual> visual(new PerceptionVisual(context_->getSceneManager(), scene_node_));

		// Now set or update the contents of the chosen visual.
		visual->setMessage(&perception);
		visual->setFramePosition(position);
		visual->setFrameOrientation(orientation);

		float alpha = alpha_property_->getFloat();
		Ogre::ColourValue color = color_property_->getOgreColor();
		visual->setColor(color.r, color.g, color.b, alpha);

		// And add it to the vector
		visuals_.push_back(visual);
	}
	
	//~ context_->queueRender();
}

} // end namespace rviz_multiception

// Tell pluginlib about this class. It is important to do this in
// global scope, outside our package's namespace.
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_multiception::PerceptionsDisplay,rviz::Display)