/*****************************************************************************/
//  Basic simulation program.
/*
Modified from the maze sample program and the air hockey program.
*/
/*****************************************************************************/
/* INCLUDES */

#include <stdio.h>
#include <math.h>
#include <X11/Intrinsic.h>
#include <Inventor/SoInput.h>
#include <Inventor/SoDB.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/events/SoLocation2Event.h>
#include <Inventor/events/SoMouseButtonEvent.h>
#include <Inventor/events/SoKeyboardEvent.h>
#include <Inventor/nodes/SoBaseColor.h>
#include <Inventor/nodes/SoCallback.h>
#include <Inventor/nodes/SoCoordinate3.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoEventCallback.h>
#include <Inventor/nodes/SoFont.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoMaterialBinding.h>
#include <Inventor/nodes/SoNormal.h>
#include <Inventor/nodes/SoNormalBinding.h>
#include <Inventor/nodes/SoPerspectiveCamera.h>
#include <Inventor/nodes/SoRotationXYZ.h>
#include <Inventor/nodes/SoText3.h>
#include <Inventor/nodes/SoTexture2.h>
#include <Inventor/nodes/SoTextureCoordinateEnvironment.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoTriangleStripSet.h>
#include <Inventor/sensors/SoAlarmSensor.h>
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/Xt/SoXt.h>
#include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
#include "communicate.h"

/*****************************************************************************/
/* DEFINES */

#define INITIAL_WINDOW_SIZE_X               900
#define INITIAL_WINDOW_SIZE_Y               700

/*****************************************************************************/
/* FUNCTION DECLARATIONS */

void init_graphics( Communication_Buffer *cb );
void draw_it( Communication_Buffer *cb );
extern "C" char * default_iv_filename( Communication_Buffer *cb );
extern "C" float default_time_step( Communication_Buffer *cb );
extern "C" void init_simulation( Communication_Buffer *cb );
extern "C" void simulate_up_to_time( float stop_time,
	    float mouse_x, float mouse_y, Communication_Buffer *cb );
extern "C" void dump_state( Communication_Buffer *cb );
extern "C" void save_data( Communication_Buffer *cb );
extern "C" void handle_goto( Communication_Buffer *cb );
extern "C" void handle_step( Communication_Buffer *cb );
extern "C" void handle_play( Communication_Buffer *cb );
extern "C" void handle_mouse_left( float mouse_x, float mouse_y,
				  Communication_Buffer *cb );
extern "C" void handle_mouse_middle( float mouse_x, float mouse_y,
				  Communication_Buffer *cb );
extern "C" void handle_mouse_right( float mouse_x, float mouse_y,
				  Communication_Buffer *cb );

/*****************************************************************************/

Communication_Buffer the_communication_buffer, *communication_buffer;

SbBool isMouseDown = FALSE;
SbVec2s mouseLocation;

int time_initialized = 0;
SbTime startTime;
SbTime animationTime;

SoTimerSensor *timer;
SoAlarmSensor *resetTimer;

SoSeparator *mainRoot = NULL;

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Callback routine for running simulation
//
// Use: private

static void
RunSimulation( void *, SoSensor * )

//
////////////////////////////////////////////////////////////////////////
{
    float mx, my, seconds;

    SbTime TimeFromStart;

    if ( !time_initialized )
       {
        startTime.setValue( SbTime::getTimeOfDay().getValue() );
	time_initialized = 1;
	}

    animationTime.setValue( SbTime::getTimeOfDay().getValue() );
    
    TimeFromStart.setValue( (animationTime - startTime).getValue() );
    seconds = TimeFromStart.getValue();

    mx = mouseLocation[0];
    my = mouseLocation[1];

    simulate_up_to_time( seconds, mx, my, communication_buffer );
    draw_it( communication_buffer );
}

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Resets the simulation parameters to their initial values to start
//    a new simulation.
//
// Use: private

static void
resetSimulation()

//
////////////////////////////////////////////////////////////////////////
{
} 

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Callback routine for resetting the simulation back to its initial state
//
// Use: private

static void
resetCB( void *, SoSensor * )

//
////////////////////////////////////////////////////////////////////////
{
    resetSimulation();
    resetTimer->unschedule();
}

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Callback routine for receiving keyboard events.
//
// Use: private

static void
ReceiveKeyboardEvent( void *, SoEventCallback *cb )

//
////////////////////////////////////////////////////////////////////////
{
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), H))
    {
      printf( "H for help (this message).\n" );
      printf( "Q to exit.\n" );
      printf( "D to dump state.\n" );
      printf( "E to save data.\n" );
      printf( "G to goto a point.\n" );
      printf( "S to start single stepping.\n" );
      printf( "P to hit the play button.\n" );
      printf( "I to write out Inventor scene description.\n" );
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), Q))
    {
      save_data( communication_buffer );
      exit( 0 );
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), D))
    {
      dump_state( communication_buffer );
      cb->setHandled(); 
      return;    
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), E))
    {
      save_data( communication_buffer );
      cb->setHandled(); 
      return;    
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), G))
    {
      handle_goto( communication_buffer );
      cb->setHandled(); 
      return;    
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), S))
    {
      handle_step( communication_buffer );
      cb->setHandled(); 
      return;    
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), P))
    {
      handle_play( communication_buffer );
      cb->setHandled(); 
      return;    
    } 
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), I))
    {
      /* Way to write out tree to check scene graph */
      SoWriteAction writeAction;
      writeAction.apply(mainRoot);
      cb->setHandled(); 
      return;    
    } 
/*
  if (SO_KEY_PRESS_EVENT(cb->getEvent(), NUMBER_1))
    {
      printf( "1\n" );
      cb->setHandled(); 
      return;    
    } 
*/
}

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Callback routine for receiving mouse button press.
//    Schedule the timer sensor on mouse down.
//
// Use: private

static void
ReceiveMouseButtonEvent( void *, SoEventCallback *cb )

//
////////////////////////////////////////////////////////////////////////
{
  float mx, my;

  if (SO_MOUSE_PRESS_EVENT(cb->getEvent(), BUTTON1))
    {
      const SbVec2s &pos = cb->getEvent()->getPosition();
      mouseLocation.setValue(pos[0], pos[1]);

      isMouseDown = TRUE;
      mx = mouseLocation[0];
      my = mouseLocation[1];
      handle_mouse_left( mx, my, communication_buffer );
      cb->setHandled();
    }
  else if (SO_MOUSE_RELEASE_EVENT(cb->getEvent(), BUTTON1))
    {
      isMouseDown = FALSE;
      cb->setHandled();
    }
  else if (SO_MOUSE_PRESS_EVENT(cb->getEvent(), BUTTON2))
    {
      const SbVec2s &pos = cb->getEvent()->getPosition();
      mouseLocation.setValue(pos[0], pos[1]);

      isMouseDown = TRUE;
      mx = mouseLocation[0];
      my = mouseLocation[1];
      handle_mouse_middle( mx, my, communication_buffer );
      cb->setHandled();
    }
  else if (SO_MOUSE_RELEASE_EVENT(cb->getEvent(), BUTTON2))
    {
      isMouseDown = FALSE;
      cb->setHandled();
    }
  else if (SO_MOUSE_PRESS_EVENT(cb->getEvent(), BUTTON3))
    {
      const SbVec2s &pos = cb->getEvent()->getPosition();
      mouseLocation.setValue(pos[0], pos[1]);

      isMouseDown = TRUE;
      mx = mouseLocation[0];
      my = mouseLocation[1];
      handle_mouse_right( mx, my, communication_buffer );
      cb->setHandled();
    }
  else if (SO_MOUSE_RELEASE_EVENT(cb->getEvent(), BUTTON3))
    {
      isMouseDown = FALSE;
      cb->setHandled();
    }
}

/************************************************************************/

////////////////////////////////////////////////////////////////////////
//
// Description:
//    Callback routine for receiving mouse motion
//
// Use: private

static void
ReceiveMouseMotionEvent( void *, SoEventCallback *cb )

//
////////////////////////////////////////////////////////////////////////
{
	/*
    if (!isMouseDown)
        return;
	*/

    const SbVec2s &pos = cb->getEvent()->getPosition();
    mouseLocation.setValue((short)pos[0], (short)pos[1]);
    cb->setHandled();
}

/************************************************************************/

static SoSeparator *
read_iv_file( const char *filename ) 
{ 
  SoInput in;
  SoSeparator *root;
    
  fprintf( stdout, "Reading file(%s)\n", filename );

  if ( !in.openFile( filename ) ) 
    {
      fprintf(stderr, "Error opening file %s\n", filename );
      return (NULL);
    }

  root = SoDB::readAll(&in);
  if ( root == NULL ) 
    fprintf( stderr, "Error reading file %s\n", filename );

  in.closeFile();
  
  return (root);
}

/************************************************************************/
/************************************************************************/

void
main(int argc, char *argv[])
{
    Widget mainWindow;
    SoSeparator *SceneRoot;
    char *iv_filename;

    communication_buffer = &the_communication_buffer;
    init_simulation( communication_buffer );

    // Initialize Inventor
    mainWindow = SoXt::init("Simple Simulation");
    SoDB::setDelaySensorTimeout(SbTime(0.0));

    if ( argc < 2 )
      {
	iv_filename = default_iv_filename( communication_buffer );
      }
    else
      iv_filename = argv[1];

    SceneRoot = read_iv_file( iv_filename ); 
    if ( SceneRoot == NULL )
      exit( -1 );

    // Setup a timer sensor to run the simulation
    timer = new SoTimerSensor(RunSimulation, NULL);
    timer->setInterval(SbTime(default_time_step( communication_buffer )));
    resetTimer = new SoAlarmSensor(resetCB, NULL);

    // Setup event callbacks
    SoEventCallback *eventCB = new SoEventCallback;
    eventCB->addEventCallback(SoMouseButtonEvent::getClassTypeId(),
            ReceiveMouseButtonEvent, (void *)&mouseLocation);
    eventCB->addEventCallback(SoLocation2Event::getClassTypeId(),
            ReceiveMouseMotionEvent, (void *)&mouseLocation);
    eventCB->addEventCallback(SoKeyboardEvent::getClassTypeId(),
            ReceiveKeyboardEvent, NULL);

    // Setup the camera so the simulation is comfortably viewed
      /* usually defined in .iv file
    SoPerspectiveCamera *cam = new SoPerspectiveCamera;
    SbVec3f pos(0.0, 1.0, 0.0 );
    SbVec3f lookto(0.0, 0.0, 0.0);
    cam->position.setValue(pos);
    cam->orientation.setValue(SbRotation(SbVec3f(0.0, 0.0, -1.0),
                                         lookto-pos));
    cam->focalDistance = (lookto-pos).length();
    */

    /* Order matters here .... scene after cam */
    mainRoot = new SoSeparator;
    mainRoot->ref();
    mainRoot->addChild(eventCB);
    /* mainRoot->addChild(cam); */
    mainRoot->addChild(SceneRoot);

    // Create a viewer and begin the simulation
    SoXtExaminerViewer *vwr = new SoXtExaminerViewer(mainWindow);
    vwr->setSize(SbVec2s( INITIAL_WINDOW_SIZE_X, INITIAL_WINDOW_SIZE_Y ));
    vwr->setSceneGraph(mainRoot);
    vwr->setTitle("Simple Simulation");
    vwr->setViewing(FALSE);/*TRUE shuts off ability to get keystrokes and mouse*/
    vwr->setDecoration(TRUE);
    vwr->show();

    init_graphics( communication_buffer );
    draw_it( communication_buffer );

    SoXt::show(mainWindow);

    if (!timer->isScheduled())
      {
	animationTime.setValue( SbTime::getTimeOfDay().getValue() );
	timer->schedule();
      }

    SoXt::mainLoop();
}

/*****************************************************************************/
/*****************************************************************************/

/*****************************************************************************/

SoTransform *base0_T = NULL;
SoTransform *upperarm0_T = NULL;
SoTransform *forearm0_T = NULL;
SoTransform *hand0_T = NULL;
SoTransform *lfinger0_T = NULL;
SoTransform *rfinger0_T = NULL;
SoTransform *block0_T = NULL;
SoTransform *block1_T = NULL;
SoTransform *block2_T = NULL;

/*****************************************************************************/

SoTransform *
get_transform( char *name )
{
  SoTransform *result; 	

  result = (SoTransform *) SoNode::getByName( name );
  if ( result == NULL )
    {
      fprintf( stderr, "Couldn't find transform %s by name in iv file.\n",
	      name );
      exit( -1 );
    }
  return result;
}

/*****************************************************************************/

void
init_graphics( Communication_Buffer *cb )
{

    base0_T = get_transform( "base0" );
    upperarm0_T = get_transform( "upperarm0" );
    forearm0_T = get_transform( "forearm0" );
    hand0_T = get_transform( "hand0" );
    lfinger0_T = get_transform( "lfinger0" );
    rfinger0_T = get_transform( "rfinger0" );
    block0_T = get_transform( "block0" );
    block1_T = get_transform( "block1" );
    block2_T = get_transform( "block2" );
}

/*****************************************************************************/

int debug_draw_it = FALSE;

void
draw_it( Communication_Buffer *cb )
{
  SbVec3f z_axis( 0., 0., 1. );

  base0_T->translation.setValue( cb->ps->arms[0]->base, 1.0, 0.0 );
  upperarm0_T->translation.setValue( cb->ps->arms[0]->base, 1.0, 0.0 );
  upperarm0_T->rotation.setValue( z_axis, cb->ps->arms[0]->shoulder );
  forearm0_T->translation.setValue( cb->ps->arms[0]->elbow_x,
				    cb->ps->arms[0]->elbow_y, 0.0 );
  forearm0_T->rotation.setValue( z_axis, 
				 cb->ps->arms[0]->shoulder +
				 cb->ps->arms[0]->elbow );
  hand0_T->translation.setValue( cb->ps->arms[0]->wrist_x,
				 cb->ps->arms[0]->wrist_y, 0.0 );
  hand0_T->rotation.setValue( z_axis, 
			      cb->ps->arms[0]->shoulder +
			      cb->ps->arms[0]->elbow +
			      cb->ps->arms[0]->wrist );
  lfinger0_T->translation.setValue( cb->ps->arms[0]->lgripper_x,
				    cb->ps->arms[0]->lgripper_y, 0.0 );
  lfinger0_T->rotation.setValue( z_axis, 
				 cb->ps->arms[0]->shoulder +
				 cb->ps->arms[0]->elbow +
				 cb->ps->arms[0]->wrist );
  rfinger0_T->translation.setValue( cb->ps->arms[0]->rgripper_x,
				    cb->ps->arms[0]->rgripper_y, 0.0 );
  rfinger0_T->rotation.setValue( z_axis, 
				 cb->ps->arms[0]->shoulder +
				 cb->ps->arms[0]->elbow +
				 cb->ps->arms[0]->wrist );
  block0_T->translation.setValue( 
			 (cb->ps->blocks[0]->x + cb->ps->blocks[0]->c3_x)/2,
			 (cb->ps->blocks[0]->y + cb->ps->blocks[0]->c3_y)/2,
			 0.0 );
  block0_T->rotation.setValue( z_axis, cb->ps->blocks[0]->angle );
  block1_T->translation.setValue( 
			 (cb->ps->blocks[1]->x + cb->ps->blocks[1]->c3_x)/2,
			 (cb->ps->blocks[1]->y + cb->ps->blocks[1]->c3_y)/2,
			 0.0 );
  block1_T->rotation.setValue( z_axis, cb->ps->blocks[1]->angle );
  block2_T->translation.setValue( 
			 (cb->ps->blocks[2]->x + cb->ps->blocks[2]->c3_x)/2,
			 (cb->ps->blocks[2]->y + cb->ps->blocks[2]->c3_y)/2,
			 0.0 );
  block2_T->rotation.setValue( z_axis, cb->ps->blocks[2]->angle );
}

/*****************************************************************************/