text: //------------------------------------------------------------------------------
  #include "chai3d.h"
  #include <ctime>
  #include <ratio>
  #include <chrono>
  #include <iostream>
  #include <fstream>
  #include <queue>
  //------------------------------------------------------------------------------
  using namespace chai3d;
  using namespace std;
  //------------------------------------------------------------------------------
  #ifndef MACOSX
  #include "GL/glut.h"
  #else
  #include "GLUT/glut.h"
  #endif
  //------------------------------------------------------------------------------
  
  //------------------------------------------------------------------------------
  // GENERAL SETTINGS
  //------------------------------------------------------------------------------
  
  // stereo Mode
  /*
      C_STEREO_DISABLED:            Stereo is disabled 
      C_STEREO_ACTIVE:              Active stereo for OpenGL NVDIA QUADRO cards
      C_STEREO_PASSIVE_LEFT_RIGHT:  Passive stereo where L/R images are rendered next to each other
      C_STEREO_PASSIVE_TOP_BOTTOM:  Passive stereo where L/R images are rendered above each other
  */
  cStereoMode stereoMode = C_STEREO_DISABLED;
  
  // fullscreen mode
  bool fullscreen = false;
  
  // mirrored display
  bool mirroredDisplay = false;
  
  //------------------------------------------------------------------------------
  // DECLARED VARIABLES
  //------------------------------------------------------------------------------
  
  // a world that contains all objects of the virtual environment
  cWorld* world;
  
  // a camera to render the world in the window display
  cCamera* camera;
  
  // a light source to illuminate the objects in the world
  cDirectionalLight *light;
  
  // a haptic device handler
  cHapticDeviceHandler* handler;
  
  // a pointer to the current haptic device
  cGenericHapticDevicePtr hapticDevice;
  
  // a label to display the haptic device model
  cLabel* labelHapticDeviceModel;
  
  // a label to display the position [m] of the haptic device
  cLabel* labelHapticDevicePosition;
  
  // a global variable to store the position [m] of the haptic device
  cVector3d hapticDevicePosition;
  
  // a label to display the rate [Hz] at which the simulation is running
  cLabel* labelHapticRate;
  
  // a label to display the haptics level (constant of force)
  cLabel* labelDelay;
  
  // a small sphere (cursor) representing the haptic device 
  cShapeSphere* cursor;
  
  // a small sphere representing the predicted position
  cShapeSphere* predictIndicator;
  
  // a line representing the velocity vector of the haptic device
  cShapeLine* velocity;
  
  // a line representing the average velocity of the haptic device
  cShapeLine* avg_velocity;
  
  // flag for using force field (ON/OFF)
  bool useForceField = true;
  
  // flag to indicate if the haptic simulation currently running
  bool simulationRunning = false;
  
  // flag to indicate if the haptic simulation has terminated
  bool simulationFinished = true;
  
  // frequency counter to measure the simulation haptic rate
  cFrequencyCounter frequencyCounter;
  
  double delay = 1.0; //delay in seconds
  
  //queues for x, y, and z
  queue <double> xPos;
  queue <double> yPos;
  queue <double> zPos;
      
  
  // information about computer screen and GLUT display window
  int screenW;
  int screenH;
  int windowW;
  int windowH;
  int windowPosX;
  int windowPosY;
  
  
  //------------------------------------------------------------------------------
  // DECLARED FUNCTIONS
  //------------------------------------------------------------------------------
  
  // callback when the window display is resized
  void resizeWindow(int w, int h);
  
  // callback when a key is pressed
  void keySelect(unsigned char key, int x, int y);
  
  // callback to render graphic scene
  void updateGraphics(void);
  
  // callback of GLUT timer
  void graphicsTimer(int data);
  
  // function that closes the application
  void close(void);
  
  // main haptics simulation loop
  void updateHaptics(void);
  
  double axisUpperLim(double a, double limit);
  
  double runningAverage(double prev_a, double a, double operand);
  
  //==============================================================================
  /*
      Program:   Prediction_Algo
  
      This application illustrates a threshold based prediction algorithm using
      the Chai3D example program 01-mydevice.cpp as a basis
  */
  //==============================================================================
  
  int main(int argc, char* argv[])
  {
      //--------------------------------------------------------------------------
      // INITIALIZATION
      //--------------------------------------------------------------------------
  
      cout << endl;
      cout << "---------------------------------------------" << endl;
      cout << "CHAI3D" << endl;
      cout << "Prediction Algorithm v1.0.4" << endl;
      cout << "---------------------------------------------" << endl << endl;
      cout << "Keyboard Options:" << endl << endl;
      cout << "[1] - Enable/Disable haptic feedback" << endl;
      cout << "[f] - Enable/Disable full screen mode" << endl;
      cout << "[m] - Enable/Disable vertical mirroring" << endl;
      cout << "[w] - increase haptics level by 1.0" << endl;
      cout << "[s] - decrease haptics level by 1.0" << endl;
      cout << "[e] - increase scale by .1" << endl;
      cout << "[d] - decrease scale by .1" << endl;
      cout << "[esc] - Exit application" << endl;
      cout << endl << endl;
  
  
      //--------------------------------------------------------------------------
      // OPENGL - WINDOW DISPLAY
      //--------------------------------------------------------------------------
  
      // initialize GLUT
      glutInit(&argc, argv);
  
      // retrieve  resolution of computer display and position window accordingly
      screenW = glutGet(GLUT_SCREEN_WIDTH);
      screenH = glutGet(GLUT_SCREEN_HEIGHT);
      windowW = (int)(0.8 * screenH);
      windowH = (int)(0.5 * screenH);
      windowPosY = (screenH - windowH) / 2;
      windowPosX = windowPosY; 
  
      // initialize the OpenGL GLUT window
      glutInitWindowPosition(windowPosX, windowPosY);
      glutInitWindowSize(windowW, windowH);
  
      if (stereoMode == C_STEREO_ACTIVE)
          glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);
      else
          glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
  
      // create display context and initialize GLEW library
      glutCreateWindow(argv[0]);
  
      #ifdef GLEW_VERSION
      // initialize GLEW
          glewInit();
      #endif
  
      // setup GLUT options
      glutDisplayFunc(updateGraphics);
      glutKeyboardFunc(keySelect);
      glutReshapeFunc(resizeWindow);
      glutSetWindowTitle("CHAI3D");
  
      // set fullscreen mode
      if (fullscreen)
      {
          glutFullScreen();
      }
  
  
      //--------------------------------------------------------------------------
      // WORLD - CAMERA - LIGHTING
      //--------------------------------------------------------------------------
  
      // create a new world.
      world = new cWorld();
  
      // set the background color of the environment
      world->m_backgroundColor.setBlack();
  
      // create a camera and insert it into the virtual world
      camera = new cCamera(world);
      world->addChild(camera);
  
      // position and orient the camera
      camera->set( cVector3d (0.5, 0.0, 0.0),    // camera position (eye)
                   cVector3d (0.0, 0.0, 0.0),    // look at position (target)
                   cVector3d (0.0, 0.0, 1.0));   // direction of the (up) vector
  
      // set the near and far clipping planes of the camera
      camera->setClippingPlanes(0.01, 10.0);
  
      // set stereo mode
      camera->setStereoMode(stereoMode);
  
      // set stereo eye separation and focal length (applies only if stereo is enabled)
      camera->setStereoEyeSeparation(0.01);
      camera->setStereoFocalLength(0.5);
  
      // set vertical mirrored display mode
      camera->setMirrorVertical(mirroredDisplay);
  
      // create a directional light source
      light = new cDirectionalLight(world);
  
      // insert light source inside world
      world->addChild(light);
  
      // enable light source
      light->setEnabled(true);
  
      // define direction of light beam
      light->setDir(-1.0, 0.0, 0.0);
  
      // create a sphere (cursor) to represent the haptic device
      cursor = new cShapeSphere(0.01);
  
      // insert cursor inside world
      world->addChild(cursor);
  
      // insert lines inside world
      world->addChild(velocity);
  
  
      //--------------------------------------------------------------------------
      // HAPTIC DEVICE
      //--------------------------------------------------------------------------
  
      // create a haptic device handler
      handler = new cHapticDeviceHandler();
  
      // get a handle to the first haptic device
      handler->getDevice(hapticDevice, 0);
  
      // open a connection to haptic device
      hapticDevice->open();
  
      // calibrate device (if necessary)
      hapticDevice->calibrate();
  
      // retrieve information about the current haptic device
      cHapticDeviceInfo info = hapticDevice->getSpecifications();
  
      // display a reference frame if haptic device supports orientations
      if (info.m_sensedRotation == true)
      {
          // display reference frame
          cursor->setShowFrame(true);
  
          // set the size of the reference frame
          cursor->setFrameSize(0.05);
      }
  
      // if the device has a gripper, enable the gripper to simulate a user switch
      hapticDevice->setEnableGripperUserSwitch(true);
  
  
      //--------------------------------------------------------------------------
      // WIDGETS
      //--------------------------------------------------------------------------
  
      // create a font
      cFont *font = NEW_CFONTCALIBRI20();
  
      // create a label to display the haptic device model
      labelHapticDeviceModel = new cLabel(font);
      camera->m_frontLayer->addChild(labelHapticDeviceModel);
      labelHapticDeviceModel->setText(info.m_modelName);
  
      // create a label to display the position of haptic device
      labelHapticDevicePosition = new cLabel(font);
      camera->m_frontLayer->addChild(labelHapticDevicePosition);
      
      // create a label to display the haptic rate of the simulation
      labelHapticRate = new cLabel(font);
      camera->m_frontLayer->addChild(labelHapticRate);
  
      // create a label to display the delay
      labelDelay = new cLabel(font);
      camera->m_frontLayer->addChild(labelDelay);
  
      //--------------------------------------------------------------------------
      // START SIMULATION
      //--------------------------------------------------------------------------
  
      // create a thread which starts the main haptics rendering loop
      cThread* hapticsThread = new cThread();
      hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
  
      // setup callback when application exits
      atexit(close);
  
      // start the main graphics rendering loop
      glutTimerFunc(50, graphicsTimer, 0);
      glutMainLoop();
  
      // exit
      return (0);
  }
  
  //------------------------------------------------------------------------------
  
  void resizeWindow(int w, int h)
  {
      windowW = w;
      windowH = h;
  }
  
  //------------------------------------------------------------------------------
  
  void keySelect(unsigned char key, int x, int y)
  {
      // option ESC: exit
      if ((key == 27) || (key == 'x'))
      {
          exit(0);
      }
  
      // option 1: enable/disable force field
      if (key == '1')
      {
          useForceField = !useForceField;
          if (useForceField)
              cout << "> Enable force field     \r";
          else
              cout << "> Disable force field    \r";
      }
  
      // option f: toggle fullscreen
      if (key == 'f')
      {
          if (fullscreen)
          {
              windowPosX = glutGet(GLUT_INIT_WINDOW_X);
              windowPosY = glutGet(GLUT_INIT_WINDOW_Y);
              windowW = glutGet(GLUT_INIT_WINDOW_WIDTH);
              windowH = glutGet(GLUT_INIT_WINDOW_HEIGHT);
              glutPositionWindow(windowPosX, windowPosY);
              glutReshapeWindow(windowW, windowH);
              fullscreen = false;
          }
          else
          {
              glutFullScreen();
              fullscreen = true;
          }
      }
  
      // option m: toggle vertical mirroring
      if (key == 'm')
      {
          mirroredDisplay = !mirroredDisplay;
          camera->setMirrorVertical(mirroredDisplay);
      }
  
      // options t and g
      if (key == 'e'){
          delay = delay + 0.1;
      }
      if (key == 'd'){
          if (delay -.1 >= 0){
              delay = delay -.1;
          }
          else{
              cout << "Delay cannot be smaller than 0!\n";
          }
      }
  }
  
  //------------------------------------------------------------------------------
  
  void close(void)
  {
      // stop the simulation
      simulationRunning = false;
  
      // wait for graphics and haptics loops to terminate
      while (!simulationFinished) { cSleepMs(100); }
  
      // close haptic device
      hapticDevice->close();
  }
  
  //------------------------------------------------------------------------------
  
  void graphicsTimer(int data)
  {
      if (simulationRunning)
      {
          glutPostRedisplay();
      }
  
      glutTimerFunc(50, graphicsTimer, 0);
  }
  
  //------------------------------------------------------------------------------
  
  void updateGraphics(void)
  {
      /////////////////////////////////////////////////////////////////////
      // UPDATE WIDGETS
      /////////////////////////////////////////////////////////////////////
  
      // update position of label
      labelHapticDeviceModel->setLocalPos(20, windowH - 40, 0);
  
      // display new position data
      labelHapticDevicePosition->setText(hapticDevicePosition.str(3));
  
      // update position of label
      labelHapticDevicePosition->setLocalPos(20, windowH - 60, 0);
  
      // display haptic rate data
      labelHapticRate->setText(cStr(frequencyCounter.getFrequency(), 0) + " Hz");
  
      // update position of label
      labelHapticRate->setLocalPos((int)(0.5 * (windowW - labelHapticRate->getWidth())), 15);
  
      // display haptic level
      labelDelay->setText( cStr(delay) );
  
      // update position of label
      labelDelay->setLocalPos(20, windowH - 80, 0);
  
      /////////////////////////////////////////////////////////////////////
      // RENDER SCENE
      /////////////////////////////////////////////////////////////////////
  
      // update shadow maps (if any)
      world->updateShadowMaps(false, mirroredDisplay);
  
      // render world
      camera->renderView(windowW, windowH);
  
      // swap buffers
      glutSwapBuffers();
  
      // wait until all GL commands are completed
      glFinish();
  
      // check for any OpenGL errors
      GLenum err;
      err = glGetError();
      if (err != GL_NO_ERROR) cout << "Error:  %s\n" << gluErrorString(err);
  }
  
  //------------------------------------------------------------------------------
  
  void updateHaptics(void)
  {
      // initialize frequency counter
      frequencyCounter.reset();
  
      // simulation in now running
      simulationRunning  = true;
      simulationFinished = false;
  
      // main haptic simulation loop
      while(simulationRunning)
      {
  
          /////////////////////////////////////////////////////////////////////
          // READ HAPTIC DEVICE
          /////////////////////////////////////////////////////////////////////
          // read position
          cVector3d position;
          hapticDevice->getPosition(position);
          position = position;
  
          // read orientation
          cMatrix3d rotation;
          hapticDevice->getRotation(rotation);
  
          xPos.push (position.get(0));
          yPos.push (position.get(1));
          zPos.push (position.get(2));
  
          /////////////////////////////////////////////////////////////////////
          // UPDATE 3D CURSOR MODEL
          /////////////////////////////////////////////////////////////////////
  
          // update position and orientation of cursor
          if ( xPos.size() >= delay * 1000){
              position.set(xPos.front(),yPos.front(),zPos.front());
              cursor->setLocalPos(position);
              xPos.pop();
              yPos.pop();
              zPos.pop();
          }
  
          // update global variable for graphic display update
          hapticDevicePosition = position;
  
          /////////////////////////////////////////////////////////////////////
          // COMPUTE AND APPLY FORCES
          /////////////////////////////////////////////////////////////////////
          
          // variables for forces    
          cVector3d force (0,0,0);
          cVector3d torque (0,0,0);
          double gripperForce = 0.0;
  
          // send computed force, torque, and gripper force to haptic device
          hapticDevice->setForceAndTorqueAndGripperForce(force, torque, gripperForce);
  
          // update frequency counter
          frequencyCounter.signal(1);
  
      }
      // exit haptics thread
      simulationFinished = true;
  }