#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/default_plugin/covariance_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/frame_manager.h>

#include "state_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( 10, 10, 10 ),
	                                           "Color to draw the arrows.",
	                                           this, SLOT( updateColorAndAlpha() ));

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

	shape_alpha_property_ = new rviz::FloatProperty( "Shape Alpha", 0.4,
	                                           "Shape transparency, 0 is fully transparent, 1.0 is fully opaque.",
	                                           this, SLOT( updateColorAndAlpha() ));

	covariance_property_ = new rviz::CovarianceProperty( "Covariance", true, "Whether or not the covariances of the messages should be shown.",
	                                                     this, SLOT( queueRender() ));
}

// 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();
	state_visuals_.clear();
	shape_visuals_.clear();
}

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

	for(size_t i=0; i<state_visuals_.size(); i++)
	{
		state_visuals_[i]->setColor(color.r, color.g, color.b, alpha);
		shape_visuals_[i]->setColor(color.r, color.g, color.b, shape_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 state visual pointers
	state_visuals_.clear(); shape_visuals_.clear();
	state_visuals_.reserve(N); shape_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_));
			continue;
		}

		boost::shared_ptr<StateVisual> state(new StateVisual(context_->getSceneManager(), scene_node_, covariance_property_));
		boost::shared_ptr<rviz::Shape> shape(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(), scene_node_));

		// Set the state visual's pose and content
		state->setContent(perception.state);
		state->setFramePosition(position);
		state->setFrameOrientation(orientation);

		// Set the shape visual's pose and shape
		Ogre::Vector3 shapePose(perception.state.x, perception.state.y, 0.1);
		Ogre::Quaternion shapeOrientation(Ogre::Radian(perception.state.yaw), Ogre::Vector3::UNIT_Z);
		Ogre::Vector3 shapeScale (perception.length, perception.width, std::max(0.1f, perception.height));
		shape->setPosition(shapePose);
		shape->setOrientation(shapeOrientation);
		shape->setScale(shapeScale);

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

		// And add it to the vector
		state_visuals_.push_back(state);
		shape_visuals_.push_back(shape);
	}

	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)