// -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
/**
 c) Kensuke Harada (AIST)
 */

#ifndef DRAW_UTILITY_H
#define DRAW_UTILITY_H

#include <osg/Geode>
#include <osg/Geometry>
#include <osg/ShapeDrawable>
#include <osg/Material>
#include <osg/Light>
#include <osg/Depth>
#include <osg/LightSource>
#include <osg/PolygonMode>
#include <osg/PositionAttitudeTransform>
#include <osg/CullFace>
#include <cnoid/BodyItem>
#include <cnoid/ToolBar>
#include <cnoid/SignalProxy>
//#include <cnoid/SceneView>
//#include <cnoid/SceneBodyManager>
//#include <cnoid/SceneObject>
#ifdef CNOID_ENABLE_OSG
#include <cnoid/OSGSceneView>
#include <cnoid/OSGSceneBodyManager> 
#include <cnoid/OSGSceneObject>	
#endif
#include <cnoid/RootItem>
#include "exportdef.h"
//#include <BodyPlugin/BodyItem.h>

using namespace cnoid;
/*
namespace cnoid {
	class MessageView;
}
*/
class Cylinders
{
public:
	Cylinders();

	Cylinders(const cnoid::Vector3& pos_, const cnoid::Vector3& dir_, double radius_, double length_, cnoid::Vector3 color_=Vector3(0.5,0.5,0.5),double alpha_ = 1.0){
		this->pos = pos_;
		this->dir = dir_;
		this->radius = radius_;
		this->length = length_;
		this->rgb = color_;
		this->alpha = alpha_;
	}
	cnoid::Vector3 pos, dir, rgb;
	double alpha;
	double radius, length;
};

class Boxes
{
public:
	Boxes();

	Boxes(const cnoid::Vector3& pos_, const cnoid::Matrix3& R_, const cnoid::Vector3& length_, cnoid::Vector3 color_=Vector3(0.5,0.5,0.5),double alpha_ = 1.0){
		this->pos = pos_;
		this->R = R_;
		this->length = length_;
		this->rgb = color_;
		this->alpha = alpha_;
	}

	cnoid::Vector3 pos,length,rgb;
	double alpha;
	cnoid::Matrix3 R;
};

class Spheres
{
public:
	Spheres();

	Spheres(const cnoid::Vector3& pos_, double radius_, cnoid::Vector3 color_=Vector3(0.5,0.5,0.5),double alpha_ = 1.0){
		this->pos = pos_;
		this->radius = radius_;
		this->rgb = color_;
		this->alpha = alpha_;
	}

	cnoid::Vector3 pos, rgb;
	double alpha;
	double radius;
};

class Ellipsoid
{
public:
	Ellipsoid();

	Ellipsoid(const cnoid::Vector3& pos_, const cnoid::Matrix3& R_, const cnoid::Vector3& length_,cnoid::Vector3 color_=Vector3(0.5,0.5,0.5),double alpha_ = 1.0){
		this->pos = pos_;
		this->R = R_;
		this->length = length_;
		this->rgb = color_;
		this->alpha = alpha_;
	}

	cnoid::Vector3 pos, length,rgb;
	double alpha;
	cnoid::Matrix3 R;

};

class CoordinateAxis
{
public:
	CoordinateAxis();
	CoordinateAxis(const cnoid::Vector3& pos_, const cnoid::Matrix3& R_, double size_){
		this->pos = pos_;
		this->R = R_;
		this->size = size_;
		this->rgb = Vector3(1,0,0);
	}
	CoordinateAxis(const cnoid::Vector3& pos_, const cnoid::Matrix3& R_, double size_, const cnoid::Vector3& color_){
		this->pos = pos_;
		this->R = R_;
		this->size = size_;
		this->rgb = color_;
	}


	cnoid::Matrix3 R;
	cnoid::Vector3 pos, rgb;
	double size;
};

class EXCADE_API DrawUtility
{
public:

	static DrawUtility* instance();
	DrawUtility();
	virtual ~DrawUtility();

	bool displayPoints();
	bool displayLines();
	bool displayCoordinateAxes();
	bool displayCylinders();
	bool displayBoxes();
	bool displaySpheres();
	bool displayEllipsoids();
	bool displayShapes();
	//bool displayPolygon();
	bool displayTriangles(double alpha = 1.0);
	void clear();

	cnoid::Vector3 rgb;
	std::vector<Cylinders> cylinders;
	std::vector<Boxes> boxes;
	std::vector<Spheres> spheres;
	std::vector<Ellipsoid> ellipsoids;
	std::vector<cnoid::Vector3> points, colors;
	std::vector<std::vector<int> > triangles;
	std::vector<CoordinateAxis> axes;

private:

	BodyItem kinect;
	BodyItem cube;
	osg::ref_ptr<osg::Geode> pclNode;
	osg::Vec3Array * curPoint;
	osg::Vec4Array * curColor;
	osg::Geometry* geom;
	std::ostream& os;

    cnoid::OSGSceneView * viewer;
	std::vector<osg::ShapeDrawable*> shape;
	std::vector<osg::Cylinder*> cylinder;
	std::vector<osg::Shape*> shapes;
    cnoid::OSGSceneObjectPtr ellipsoid_obj;
};

#endif
