// OpenSG Tutorial Example: Hierarchical Transformation
//
// This example demonstrates how transformations accumulate through the graph.
//
// modified from OpenSG 1.8 cvs tutorial 04hiertransform.cpp
// by Bill Thibault 2009

// Headers
#include <OpenSG/OSGGLUT.h>
#include <OpenSG/OSGConfig.h>
#include <OpenSG/OSGSimpleGeometry.h>
#include <OpenSG/OSGGLUTWindow.h>
#include <OpenSG/OSGSimpleSceneManager.h>
#include <OpenSG/OSGBaseFunctions.h>
#include <OpenSG/OSGTransform.h>
#include <OpenSG/OSGGroup.h>
#include <vector>
#include <cmath>
using namespace std;

// Activate the OpenSG namespace
OSG_USING_NAMESPACE

// use a different transform for each joint
vector<TransformPtr> trans;
const int numJoints = 9;
const Real32 cylLength = 6.0;


// The SimpleSceneManager to manage simple applications
SimpleSceneManager *mgr;

// forward declaration so we can have the interesting stuff upfront
int setupGLUT( int *argc, char *argv[] );


// 
void
setJointTransform ( int joint, Real32 time )
{
  Matrix m,t;
  m.setIdentity();
  t.setIdentity();

  switch ( joint ) {

  case 0: // root transform
    m.setTransform( Vec3f(0,0,0), 
		    Quaternion( Vec3f(0,1,0), (time/1000.f)*3.14159 / 8 ) );
    break;

  case 1: // right elbow
    m.setTransform( Vec3f (  -cylLength, 0, 0 ),
    		    Quaternion( Vec3f(0,0,1), -fabs(osgsin(time/1000.f)*3.14159/6.0)) );
    break;

  case 2: // right shoulder
    m.setTransform (  Vec3f(-4,27,0),
		      Quaternion( Vec3f(0,0,1), 
				  (3.14159/4) *osgsin(time/1000.f)  ) );
    break;
  case 3: // left shoulder
    m.setTransform ( Vec3f ( 4,27,0 ),
    		     Quaternion ( Vec3f (0,0,1), 
				  osgsin(time/1000.f)* M_PI / 2.0 ) );
    break;
  case 4: // left elbow
    m.setTransform( Vec3f (  cylLength, 0, 0 ),
    		    Quaternion( Vec3f(0,0,1), 
				fabs(osgsin(time/1000.f)*3.14159/6.0)));
    break;
  case 5: // right hip
    m.setTransform ( Vec3f ( -3, 12.0, 0 ) );
    t.setTransform ( Quaternion( Vec3f(1,0,0), osgsin(time/1000.f)*3.14159 / 8 ) );
    m.mult(t);
    t.setTransform ( Vec3f ( 0, -cylLength/2.0, 0) );
    m.mult(t);
    break;
  case 6: // left hip
    m.setTransform ( Vec3f ( 3, 12.0, 0 ) );
    t.setTransform( Vec3f(0,0,0), 
		    Quaternion( Vec3f(1,0,0), osgsin(M_PI+time/1000.f)*3.14159 / 8 ) );
    m.mult(t);
    t.setTransform ( Vec3f( 0,-cylLength/2.0, 0) );
    m.mult(t);
    break;
  case 7: // right knee
    m.setTransform ( Vec3f ( 0, -cylLength/2.0, 0 ) );
    t.setTransform( Vec3f(0,0,0), 
		    Quaternion( Vec3f(1,0,0), fabs(osgsin(time/1000.f)*3.14159 / 8 )) );
    m.mult(t);
    t.setTransform ( Vec3f( 0,-cylLength/2.0, 0) );   // move to end of parent. parent's center at origin
    m.mult(t);
    break;
  case 8: // left knee
    m.setTransform ( Vec3f ( 0, -cylLength/2.0, 0 ) );
    t.setTransform( Vec3f(0,0,0), 
		    Quaternion( Vec3f(1,0,0), fabs(osgsin(M_PI+time/1000.f)*3.14159 / 8 )));
    m.mult(t);
    t.setTransform ( Vec3f( 0, -cylLength/2.0, 0) );   
    m.mult(t);
    break;
  default:
    break;
  }

  beginEditCP(trans[joint], Transform::MatrixFieldMask );
    trans[joint]->setMatrix(m);
  endEditCP(trans[joint], Transform::MatrixFieldMask );
}


// redraw the window
void 
display( void )
{
    Matrix m;
    Real32 t = glutGet(GLUT_ELAPSED_TIME );
    
    // compute a transformation for each joint
    for ( int j = 0; j < numJoints; j++ ) {
      setJointTransform ( j, t );
    }

     
    mgr->redraw();
}

void 
update(void)
{
    glutPostRedisplay();
}

NodePtr
makeHorizRNode ( GeometryPtr geo )
{
    // create transform node for a horizontal 
    // (egocentric) Right-side version of the cylinder
    NodePtr horizRNode = Node::create();
    NodePtr geoNode = Node::create();
    TransformPtr horizRTrans = Transform::create();
    Matrix m,t;

    beginEditCP(geoNode);
    geoNode->setCore ( geo );
    endEditCP (geoNode);

    t.setTransform ( Vec3f ( -cylLength / 2.0, 0, 0 ) );
    m.setTransform ( Vec3f ( 0,0,0 ),
    		     Quaternion ( Vec3f (0,0,1), M_PI / 2.0 ) );
    t.mult(m);
    beginEditCP ( horizRTrans );
    horizRTrans->setMatrix ( t );
    endEditCP ( horizRTrans );

    beginEditCP ( horizRNode );
    horizRNode->setCore ( horizRTrans );
    horizRNode->addChild ( geoNode );
    endEditCP ( horizRNode );

    return horizRNode;
}

NodePtr
makeHorizLNode ( GeometryPtr geo )
{
    // create transform node for a horizontal 
    // (egocentric) LEFT-side version of the cylinder
    NodePtr horizLNode = Node::create();
    NodePtr geoNode = Node::create();
    TransformPtr horizLTrans = Transform::create();
    Matrix m,t;

    beginEditCP(geoNode);
    geoNode->setCore ( geo );
    endEditCP (geoNode);

    t.setTransform ( Vec3f ( cylLength / 2.0, 0, 0 ) );
    m.setTransform ( Vec3f ( 0,0,0 ),
    		     Quaternion ( Vec3f (0,0,1), M_PI / 2.0 ) );
    t.mult(m);
    beginEditCP ( horizLTrans );
    horizLTrans->setMatrix ( t );
    endEditCP ( horizLTrans );

    beginEditCP ( horizLNode );
    horizLNode->setCore ( horizLTrans );
    horizLNode->addChild ( geoNode );
    endEditCP ( horizLNode );

    return horizLNode;
}

// Initialize GLUT & OpenSG and set up the scene
int 
main(int argc, char **argv)
{

  Matrix m,t;
  int j;

    // OSG init
    osgInit(argc,argv);

    // GLUT init
    int winid = setupGLUT(&argc, argv);

    // the connection between GLUT and OpenSG
    GLUTWindowPtr gwin= GLUTWindow::create();
    gwin->setId(winid);
    gwin->init();

    // create the scene

    /*
        Transformation accumulate through the graph, i.e. all nodes below
        a Transformation are influenced by it, even other Transformations.
        
        This can be used to create models of objects that move together and
        in relation to each other, the prime examples being a robot arm and
        a planetary system. This example does something not quite unlike a
        robot arm.
    */    

    // create the scene
    
    /*
       This time the graph is not wide, but deep, i.e. every Transformation
       only has two children, a Geometry and another transformation.
       The end resulting motion of the geometry is the accumulation of
       all the Transformations above it.
    */
     
    
    // the transform cores are kept in a container
    m.setIdentity();
    for ( j = 0; j < numJoints; j++ ) {
      TransformPtr t =  Transform::create();
      t->setMatrix ( m );
      trans.push_back ( t );
    }
    
    // setup an intial transformation
    for ( j = 0; j < numJoints; j++ ) {
      setJointTransform ( j, 0.0 );
    }

    
    //    
    // build the robot from cylinders
    // the arm segments are horizontal, the legs vertical
    //

    // create geometry node for the cylinder geometry shared by all extremities
    NodePtr geoNode = Node::create();
    GeometryPtr cylGeo = makeCylinderGeo( cylLength, 1, 8, true, true, true );
    beginEditCP ( geoNode );
    geoNode->setCore ( cylGeo );
    endEditCP ( geoNode );

    // create geometry and transform nodes for the body geometry
    NodePtr bodyNode = Node::create();
    NodePtr bodyGeoNode = Node::create();
    TransformPtr bodyTrans = Transform::create();
    GeometryPtr bodyGeo = makeBoxGeo ( 8, 16, 4, 1,1,1);
    m.setTransform ( Vec3f ( 0, 20, 0 ) );
    beginEditCP ( bodyTrans );
    bodyTrans->setMatrix ( m );
    endEditCP ( bodyTrans );

    beginEditCP ( bodyGeoNode );
    bodyGeoNode->setCore ( bodyGeo );
    endEditCP ( bodyGeoNode );

    beginEditCP ( bodyNode );
    bodyNode->setCore ( bodyTrans );
    bodyNode->addChild ( bodyGeoNode );
    endEditCP ( bodyNode );
    
    // create geometry and transform nodes for the head geometry
    NodePtr headNode = Node::create();
    NodePtr headGeoNode = Node::create();
    TransformPtr headTrans = Transform::create();
    GeometryPtr headGeo = makeSphereGeo ( 3, 3 );
    m.setTransform ( Vec3f ( 0, 30, 0 ) );
    beginEditCP ( headTrans );
    headTrans->setMatrix ( m );
    endEditCP ( headTrans );

    beginEditCP ( headGeoNode );
    headGeoNode->setCore ( headGeo );
    endEditCP ( headGeoNode );

    beginEditCP ( headNode );
    headNode->setCore ( headTrans );
    headNode->addChild ( headGeoNode );
    endEditCP ( headNode );

    // create a transform node for the right lower arm
    // make the horizontal cyl a child of the right lower arm
    NodePtr LRArmNode = Node::create();
    beginEditCP ( LRArmNode );
    LRArmNode->setCore ( trans[1] );
    LRArmNode->addChild ( makeHorizRNode( cylGeo ) );
    endEditCP ( LRArmNode );

    // create a transform node for the right upper arm
    // make the horizontal cyl a child of the right upper arm
    // make the lower arm a child of the upper arm
    NodePtr URArmNode = Node::create();
    beginEditCP ( URArmNode );
    URArmNode->setCore ( trans[2] );
    URArmNode->addChild ( makeHorizRNode(cylGeo) );
    URArmNode->addChild ( LRArmNode );
    endEditCP ( URArmNode );

    // make the left lower arm a child of the right upper arm
    // make the left upper arm a child of the root
    NodePtr LLArmNode = Node::create();
    beginEditCP ( LLArmNode );
    LLArmNode->setCore ( trans[4] );
    LLArmNode->addChild ( makeHorizLNode ( cylGeo ) );
    endEditCP ( LLArmNode );

    NodePtr ULArmNode = Node::create();
    beginEditCP ( ULArmNode );
    ULArmNode->setCore ( trans[3] );
    ULArmNode->addChild ( makeHorizLNode ( cylGeo ) );
    ULArmNode->addChild ( LLArmNode );
    endEditCP ( ULArmNode );

    // right leg
    NodePtr LRLegGeo = Node::create();
    beginEditCP(LRLegGeo);
    LRLegGeo->setCore(cylGeo);
    endEditCP(LRLegGeo);
    NodePtr LRLegNode = Node::create();
    beginEditCP ( LRLegNode );
    LRLegNode->setCore ( trans[7] );
    LRLegNode->addChild ( LRLegGeo );
    endEditCP ( LRLegNode );

    NodePtr URLegGeo = Node::create();
    beginEditCP(URLegGeo);
    URLegGeo->setCore(cylGeo);
    endEditCP(URLegGeo);
    NodePtr URLegNode = Node::create();
    beginEditCP ( URLegNode );
    URLegNode->setCore ( trans[5] );
    URLegNode->addChild ( URLegGeo );
    URLegNode->addChild ( LRLegNode );
    endEditCP ( URLegNode );


    // left leg
    NodePtr LLLegGeo = Node::create();
    beginEditCP(LLLegGeo);
    LLLegGeo->setCore(cylGeo);
    endEditCP(LLLegGeo);
    NodePtr LLLegNode = Node::create();
    beginEditCP ( LLLegNode );
    LLLegNode->setCore ( trans[8] );
    LLLegNode->addChild ( LLLegGeo );
    endEditCP ( LRLegNode );

    NodePtr ULLegGeo = Node::create();
    beginEditCP(ULLegGeo);
    ULLegGeo->setCore(cylGeo);
    endEditCP(ULLegGeo);
    NodePtr ULLegNode = Node::create();
    beginEditCP ( ULLegNode );
    ULLegNode->setCore ( trans[6] );
    ULLegNode->addChild ( ULLegGeo );
    ULLegNode->addChild ( LLLegNode );
    endEditCP ( ULLegNode );
    
    // create a transform node for the root of the graph
    NodePtr rootNode = Node::create();
    beginEditCP ( rootNode );
    rootNode->setCore ( trans[0] );
    rootNode->addChild ( headNode );
    rootNode->addChild ( bodyNode );
    rootNode->addChild ( URArmNode );
    rootNode->addChild ( ULArmNode );
    rootNode->addChild ( URLegNode );
    rootNode->addChild ( ULLegNode );
    endEditCP ( rootNode );

    

    //////////    
    
    // create the SimpleSceneManager helper
    mgr = new SimpleSceneManager;

    // tell the manager what to manage
    mgr->setWindow(gwin );
    mgr->setRoot  (rootNode);

    // show the whole scene
    mgr->showAll();

    // GLUT main loop
    glutMainLoop();

    return 0;
}

//
// GLUT callback functions
//

// react to size changes
void reshape(int w, int h)
{
    mgr->resize(w, h);
    glutPostRedisplay();
}

// react to mouse button presses
void mouse(int button, int state, int x, int y)
{
    if (state)
        mgr->mouseButtonRelease(button, x, y);
    else
        mgr->mouseButtonPress(button, x, y);
        
    glutPostRedisplay();
}

// react to mouse motions with pressed buttons
void motion(int x, int y)
{
    mgr->mouseMove(x, y);
    glutPostRedisplay();
}

// react to keys
void keyboard(unsigned char k, int x, int y)
{
    switch(k)
    {
        case 27:    
        {
            OSG::osgExit();
            exit(0);
        }
        break;
    }
}

// setup the GLUT library which handles the windows for us
int setupGLUT(int *argc, char *argv[])
{
    glutInit(argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
    
    int winid = glutCreateWindow("OpenSG");
    
    glutReshapeFunc(reshape);
    glutDisplayFunc(display);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    glutKeyboardFunc(keyboard);

    // call the redraw function whenever there's nothing else to do
    glutIdleFunc(update);

    return winid;
}