/* File: soln_two_animated_arrows.cpp
*/

#include <osg/Notify>
#include <osg/MatrixTransform>
#include <osg/PositionAttitudeTransform>
#include <osg/ShapeDrawable>
#include <osg/Geometry>
#include <osgUtil/Optimizer>
#include <osgGA/TrackballManipulator>
#include <osgViewer/Viewer>
#include <iostream>

using namespace osg;

Geode *make_cone(float xcen, float ycen, float zcen,
                 float radius, float height,
		 float r, float g, float b, float alpha)
{
    Cone* cone = new Cone(Vec3f(xcen, ycen, zcen), radius, height);
    ShapeDrawable *drawable = new ShapeDrawable(cone);
    drawable->setColor(Vec4(r, g, b, alpha));
    Geode* geode = new Geode();
    geode->addDrawable(drawable);
    return geode;
}

Geode *make_cylinder(float xcen, float ycen, float zcen,
                 float radius, float height,
		 float r, float g, float b, float alpha)
{
    Cylinder* cylinder = new Cylinder(Vec3(xcen, ycen, zcen), radius, height);
    ShapeDrawable *drawable = new ShapeDrawable(cylinder);
    drawable->setColor(Vec4f(r, g, b, alpha));
    Geode* geode = new Geode();
    geode->addDrawable(drawable);
    return geode;
}

// make an arrow of given length, based at the origin
// in vertical (z-up) orientation  
// radius of cylinder is given by shaft_radius
// cone will be such that it is a 45 deg angle cone that has
// radius and lenth twice that of shaft radius.
// will assume no transparency, so alpha need not be specified

Group *make_vec_arrow(float shaft_radius, float total_length,
		     float r, float g, float b)
{
    float cone_radius = 2*shaft_radius;
    float cone_height = cone_radius;
    float shaft_length = total_length - cone_height;
    Geode *cylinder = make_cylinder(0.0, 0.0, shaft_length/2.0,
                                    shaft_radius, shaft_length, r, g, b, 1.0);
    Geode *cone = make_cone(0.0, 0.0, shaft_length + cone_height/4.0,
                            cone_radius, cone_height, r, g, b, 1.0);
    Group* vec_arrow = new Group;
    vec_arrow->addChild(cylinder); 
    vec_arrow->addChild(cone); 

    return vec_arrow;
}

// Make three arrows using the routine above to indicate x,y,z axes
Group *make_axes(void)
{

    float shaft_radius = 0.03;
    float total_length = 1.0;
    Group *red_arrow   = make_vec_arrow(shaft_radius, total_length, 1.0, 0.0, 0.0);
    Group *green_arrow = make_vec_arrow(shaft_radius, total_length, 0.0, 1.0, 0.0);
    Group *blue_arrow  = make_vec_arrow(shaft_radius, total_length, 0.0, 0.0, 1.0);

    MatrixTransform* xaxis = new MatrixTransform;
    MatrixTransform* yaxis = new MatrixTransform;
    MatrixTransform* zaxis = new MatrixTransform;

    xaxis->addChild(red_arrow);
    yaxis->addChild(green_arrow);
    zaxis->addChild(blue_arrow);

    xaxis->setMatrix(Matrix::rotate(inDegrees(90.0), 0.0, 1.0, 0.0f));
    yaxis->setMatrix(Matrix::rotate(inDegrees(-90.0), 1.0, 0.0, 0.0f));
    zaxis->setMatrix(Matrix::identity());

    Group *group = new Group();
    group->addChild(xaxis);
    group->addChild(yaxis);
    group->addChild(zaxis);
    return group;
}

void usage()
{
    std::cout << "usage: soln_two_animated_arrows\n";
    exit(0);
}

int main( int argc, char **argv )
{
    float x, y, z;
    double t0, t, speed, theta, anim_radius;
    PositionAttitudeTransform *T1, *T2;

    if (argc != 1)
        usage();

    // initialize the viewer.
    osgViewer::Viewer viewer;

    // make axes
    Group *axes = make_axes();

    // Make a couple of arrows to animate
    Group *arrow1 = make_vec_arrow(0.1, 2.0, 0.0, 1.0, 1.0);
    T1 = new PositionAttitudeTransform;
    T1->addChild(arrow1);

    Group *arrow2 = make_vec_arrow(0.1, 1.2, 1.0, 1.0, 0.0);
    T2 = new PositionAttitudeTransform;
    T2->addChild(arrow2);

    // Add axes and arrows to the scene root node
    MatrixTransform* rootnode = new MatrixTransform;
    rootnode->setMatrix(Matrix::rotate(inDegrees(30.0), 1.0 ,0.0 ,0.0));
    rootnode->addChild(axes);
    rootnode->addChild(T1);
    rootnode->addChild(T2);

    // run optimization over the scene graph
    osgUtil::Optimizer optimzer;
    optimzer.optimize(rootnode);
     
    // set the scene to render
    viewer.setSceneData(rootnode);
    viewer.setCameraManipulator(new osgGA::TrackballManipulator());

    anim_radius = 2.5;
    speed = M_PI/2.0; // (90 deg / sec)
    // call each frame 
    t0 = viewer.getFrameStamp()->getReferenceTime();
    while (!viewer.done()) {       
	t = viewer.getFrameStamp()->getReferenceTime() - t0;
        theta = speed * t;
	x = anim_radius*cos(theta);
	y = 0;
	z = anim_radius*sin(theta);
	T1->setPosition(Vec3f(x, y, z));
	T1->setAttitude(Quat(-theta, Vec3f(0.0, 1.0, 0.0)));
	T2->setPosition(Vec3f(x, y, z));
        T2->setAttitude(Quat(M_PI/2.0, Vec3f(1.0, 0.0, 0.0)));
	viewer.frame();
    } 
}