HapticClient.cpp

//==============================================================================
/*
Software License Agreement (BSD License)
Copyright (c) 2003-2016, CHAI3D.
(www.chai3d.org)

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:

* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials provided
with the distribution.

* Neither the name of CHAI3D nor the names of its contributors may
be used to endorse or promote products derived from this software
without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.

\author    <http://www.chai3d.org>
\author    Francois Conti
\version   3.1.0 $Rev: 1869 $
*/
//===========================================================================
/*
//===========================================================================
edited for the TOAST Kinaesthetic Tested, by Daniel Rodriguez
last revision : 08.11.2024
//===========================================================================
*/
//===========================================================================

//---------------------------------------------------------------------------
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <fstream>
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
#include "Sock/Sender.hpp"
#include "Sock/Receiver.hpp"
#include "HapticCommLib.h"
#include <boost/asio.hpp>
#include "config.h" // read configuration settings from /cfg/config.cfg file


unsigned short recvPort = 12355;	//Port this computer uses to listen for packets
unsigned short sendPort = 12345;	//Port that the server listens for packets
std::string sendIp = "127.0.0.1";	//IP address of the server
	
Receiver clientReceiver(recvPort);
Sender clientSender(sendPort, sendIp);



void UDPServerThread();
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
#include "chai3d.h"
//---------------------------------------------------------------------------
//------------------------------------------------------------------------------
using namespace chai3d;
using namespace std;
//------------------------------------------------------------------------------
#ifndef MACOSX
#include "GL/glut.h"
#else
#include "GLUT/glut.h"
#endif
//------------------------------------------------------------------------------
// 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;

//****************** Read Parameters from configuration file **************//
ConfigFile cfg("cfg/config.cfg"); // get the configuration file
double PositionDeadbandParameter = cfg.getValueOfKey<double>("PositionDeadbandParameter")/35; //deadband parameter for force data reduction, 0.1 is the default value
double VelocityDeadbandParameter = cfg.getValueOfKey<double>("VelocityDeadbandParameter"); //deadband parameter for velcity data reduction, 0.1 is the default value

int ForceDelay = cfg.getValueOfKey<int>("ForceDelay"); // ms: constant network delay on Force feedback
int CommandDelay = cfg.getValueOfKey<int>("CommandDelay"); // ms: constant network delay on Commanding channel 
int ControlMode = cfg.getValueOfKey<int>("ControlMode"); // 0: position control, 1:velocity control
int HapticDeviceConnected=cfg.getValueOfKey<int>("HapticDevice");// 0: Not Connected, 1:Connected
int FileName=cfg.getValueOfKey<int>("FileName");//0: square, 1: circle, 2: infinity




DeadbandDataReduction* DBVelocity; // data reduction class for velocity samples
DeadbandDataReduction* DBPosition; // data reduction class for position samples

queue<HapticSample>VelocityQueue; // queue for velocity samples
queue<HapticSample>PositionQueue; // queue for position samples

// Desired velocity (m/s)
double desiredVelocityY = 0;  // Adjust as needed (+ve for up, -ve for down)
double desiredVelocityZ = 0;  // Adjust as needed (+ve for up, -ve for down)
double incrY=0.01;
double incrZ=0.01;
bool changeDirectionY=false;
bool changeDirectionZ=false;

// Control parameters
double kP = 100.0; // Proportional gain (N/m)
double kD = 10.0;  // Derivative gain (N·s/m)


//---------------------------------------------------------------------------
// DECLARED MACROS
//---------------------------------------------------------------------------
// convert to resource path
#define RESOURCE_PATH(p)    (char*)((resourceRoot+string(p)).c_str())

//---------------------------------------------------------------------------
// DECLARED CONSTANTS
//---------------------------------------------------------------------------

// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W = 600;
const int WINDOW_SIZE_H = 600;

// mouse menu options (right button)
const int OPTION_FULLSCREEN = 1;
const int OPTION_WINDOWDISPLAY = 2;

// maximum number of haptic devices supported in this demo
const int MAX_DEVICES = 1;//8;

//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------

// a world that contains all objects of the virtual environment
cWorld* world;

// a camera that renders the world in a window display
cCamera* camera;

// a light source to illuminate the objects in the virtual scene
cDirectionalLight *light;

// a little "chai3d" bitmap logo at the bottom of the screen
cBitmap* logo;

// width and height of the current window display
int displayW = 0;
int displayH = 0;

// a haptic device handler
cHapticDeviceHandler* handler;

// a table containing pointers to all haptic devices detected on this computer
cGenericHapticDevicePtr hapticDevices[MAX_DEVICES];

// a table containing pointers to label which display the position of
// each haptic device
cLabel* labels[MAX_DEVICES];
cLabel* deviceLabels[MAX_DEVICES];

// number of haptic devices detected
int numHapticDevices = 0;

// table containing a list of 3D cursors for each haptic device
cShapeSphere* cursors[MAX_DEVICES];

// table containing a list of lines to display velocity
cShapeLine* velocityVectors[MAX_DEVICES];

// material properties used to render the color of the cursors
cMaterial matCursorButtonON;
cMaterial matCursorButtonOFF;

// status of the main simulation haptics loop
bool simulationRunning = false;

// root resource path
string resourceRoot;

// damping mode ON/OFF
bool useDamping = false;

// force field mode ON/OFF
bool useForceField = true;

// has exited haptics simulation thread
bool simulationFinished = false;

cGenericObject* rootLabels;
cGenericObject* rootLabels2;
cLabel* ParameterLabels[5];

//TDPA needed constants
double MasterVelocity[3] = { 0.0, 0.0, 0.0 };
double MasterPosition[3] = { 0.0, 0.0, 0.0 };
double PreviousPosition[3] = { 0.0, 0.0, 0.0 };
double UpdatedVelocitySample[3] = {0.0,0.0,0.0}; // update 3 DoF velocity sample (holds the signal after deadband)
double UpdatedPositionSample[3] = { 0.0, 0.0, 0.0 }; // update 3 DoF position sample (holds the signal after deadband)
bool VelocityTransmitFlag = false; // true: deadband triger false: keep last recently transmitted sample (ZoH)
bool PositionTransmitFlag = false; // true: deadband triger false: keep last recently transmitted sample (ZoH)
int VelocityPacketNum = 0;
int PositionPacketNum = 0;


double MasterForce[3] = { 0.0, 0.0, 0.0 };

HapticSample* tmp;

//---------------------------------------------------------------------------
// DECLARED MACROS
//---------------------------------------------------------------------------
// convert to resource path
#define RESOURCE_PATH(p)    (char*)((resourceRoot+string(p)).c_str())


//---------------------------------------------------------------------------
// DECLARED FUNCTIONS
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);

// callback when the right mouse button is pressed to select a menu item
void menuSelect(int value);

// function called before exiting the application
void close(void);

// callback of GLUT timer
void graphicsTimer(int data);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);

cVector3d globalForce;

// some time information necessary for the linear predictor
double timestamp;


// linear predictor
cVector3d ch_perceptualLinearPred(const cVector3d& rawForce, bool& received, double& update_timestamp);
unsigned int initial_samples_marker = 0;

// update received flag
bool update_received = false;




//===========================================================================
/*
DEMO:    device.cpp

This application illustrates the use of the haptic device handler
"cHapticDevicehandler" to access all of the haptic devices
"cGenericHapticDevice" connected to the computer.

In this example the application opens an OpenGL window and displays a
3D cursor for each device. Each cursor (sphere + reference frame)
represents the position and orientation of its respective device.
If the operator presses the device user button (if available), the color
of the cursor changes accordingly.

In the main haptics loop function  "updateHaptics()" , the position,
orientation and user switch status of each device are retrieved at
each simulation iteration. The information is then used to update the
position, orientation and color of the cursor. A force is then commanded
to the haptic device to attract the end-effector towards the device origin.
*/
//===========================================================================

int main(int argc, char* argv[])
{
	//-----------------------------------------------------------------------
	// INITIALIZATION
	//-----------------------------------------------------------------------

	printf("\n");
	printf("-----------------------------------\n");
	printf("CHAI 3D\n");
	printf("TOAST Testbed - Haptic UDP Client\n");
	printf("Copyright 2023-2026\n");
	printf("-----------------------------------\n");
	printf("\n\n");
	printf("Keyboard Options:\n\n");
	printf("[x] - Exit application\n");
	printf ("******* Deadband increase/decrease keys *******************\n");
    printf ("[q] - increase force deadband (+0.01) \n");
    printf ("[a] - decrease force deadband (-0.01)\n");
	printf ("[w] - increase velocity/position deadband (+0.01)\n");
    printf ("[s] - decrease velocity/position deadband (-0.01)\n");
	printf("\n\n");
	printf("Current Position deadband parameter = %f \n",PositionDeadbandParameter*35);
	printf("Current velocity deadband parameter = %f \n",VelocityDeadbandParameter);

	// parse first arg to try and locate resources
	resourceRoot = string(argv[0]).substr(0, string(argv[0]).find_last_of("/\\") + 1);

	// initialized deadband classes for force and velocity
	DBPosition = new DeadbandDataReduction(PositionDeadbandParameter);
	DBVelocity = new DeadbandDataReduction(VelocityDeadbandParameter);


	globalForce *= 0;

	for (int i=0;i<CommandDelay;i++){
		tmp = new HapticSample;
		tmp->Sample[0] = 0.0;
		tmp->Sample[1] = 0.0;
		tmp->Sample[2] = 0.0;
		VelocityQueue.push(*tmp);
		delete tmp;


		tmp = new HapticSample;
		tmp->Sample[0] = 0.0;
		tmp->Sample[1] = 0.0;
		tmp->Sample[2] = 0.0;
		PositionQueue.push(*tmp);
		delete tmp;

	}


	//--------------------------------------------------------------------------
	// OPEN GL - WINDOW DISPLAY
	//--------------------------------------------------------------------------

	// initialize GLUT
	glutInit(&argc, argv);

	// retrieve  resolution of computer display and position window accordingly
	int screenW = glutGet(GLUT_SCREEN_WIDTH);
	int screenH = glutGet(GLUT_SCREEN_HEIGHT);
	int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
	int windowPosY = (screenH - WINDOW_SIZE_H) / 2;

	// initialize the OpenGL GLUT window
	glutInitWindowPosition(windowPosX, windowPosY);
	glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);

	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("Haptic UDP Client");

	// set fullscreen mode
	if (fullscreen)
	{
		glutFullScreen();
	}

	//-----------------------------------------------------------------------
	// 3D - SCENEGRAPH
	//-----------------------------------------------------------------------

	// create a new world.
	world = new cWorld();

	// set the background color of the environment
	// the color is defined by its (R,G,B) components.
	world->setBackgroundColor(0.0, 0.0, 0.0);

	// create a camera and insert it into the virtual world
	camera = new cCamera(world);
	world->addChild(camera);

	// position and oriente the camera
	camera->set(cVector3d(0.5, 0.0, 0.0),    // camera position (eye)
		cVector3d(0.0, 0.0, 0.0),    // lookat position (target)
		cVector3d(0.0, 0.0, 1.0));   // direction of the "up" vector

	// set the near and far clipping planes of the camera
	// anything in front/behind these clipping planes will not be rendered
	camera->setClippingPlanes(0.01, 10.0);

	// create a light source and attach it to the camera
	light = new cDirectionalLight(world);
	camera->addChild(light);                   // attach light to camera
	light->setEnabled(true);                   // enable light source
	light->setLocalPos(cVector3d(2.0, 0.5, 1.0));  // position the light source
	light->setDir(cVector3d(-2.0, 0.5, 1.0));  // define the direction of the light beam


	//-----------------------------------------------------------------------
    // 2D - WIDGETS
    //-----------------------------------------------------------------------

    // create a 2D bitmap logo
    logo = new cBitmap();

    // add logo to the front plane
    camera->m_frontLayer->addChild(logo);

    // load a "chai3d" bitmap image file
    bool fileload;
    
	fileload = logo->loadFromFile("resources/TOAST-logo.png");
    if (!fileload)
    {
        #if defined(_MSVC)
        fileload = logo->m_image.loadFromFile("resources/TOAST-logo.png");
        #endif
    }

    // position the logo at the bottom left of the screen (pixel coordinates)
    logo->setLocalPos(0, 430, 0);

    // scale the logo along its horizontal and vertical axis
    logo->setZoom(0.2,0.2);

    //here we replace all black pixels (0,0,0) of the logo bitmap
    //with transparent black pixels (0, 0, 0, 0). This allows us to make
    //the background of the logo look transparent.
    // logo->m_image.replace(
    //                       cColorb(0, 0, 0),      // original RGB color
    //                       cColorb(0, 0, 0, 0)    // new RGBA color
    //                       );

    // enable transparency
    logo->setUseTransparency(true);

	//-----------------------------------------------------------------------
	// HAPTIC DEVICES / TOOLS
	//-----------------------------------------------------------------------

	// create a haptic device handler
	handler = new cHapticDeviceHandler();

	// read the number of haptic devices currently connected to the computer
	numHapticDevices = handler->getNumDevices();

	// limit the number of devices to MAX_DEVICES
	numHapticDevices = cMin(numHapticDevices, MAX_DEVICES);

	// create a node on which we will attach small labels that display the
	// position of each haptic device
	rootLabels2 = new cGenericObject();
	camera->m_frontLayer->addChild(rootLabels2);

	// create a font
	cFontPtr font = NEW_CFONTCALIBRI20();
	// create a small label as title
	cLabel* titleLabel2 = new cLabel(font);
	rootLabels2->addChild(titleLabel2);

	// define its position, color and string message
	titleLabel2->setLocalPos(10, 560, 0);
	titleLabel2->m_fontColor.set(1.0, 1.0, 1.0);
	titleLabel2->setText("Haptic Device Pos [mm]:");

	// Display parameters in virtual environment

	rootLabels = new cGenericObject();
	cLabel* titleLabel = new cLabel(font);
    //camera->m_front_2Dscene.addChild(rootLabels);
    camera->m_frontLayer->addChild(rootLabels);
	// create a small label as title
    rootLabels->addChild(titleLabel);

	titleLabel->setLocalPos(300, 30, 0);
    titleLabel->m_fontColor.setWhite();
	titleLabel->setText("Demo Settings ");

	// Display force delays

	// Display command delay
	string strNum1;
	strNum1=cStr(CommandDelay);
	ParameterLabels[0] = new cLabel(font);
	rootLabels->addChild(ParameterLabels[0]);
	ParameterLabels[0]->setLocalPos(300, 10, 0);
	ParameterLabels[0]->m_fontColor.setWhite();
	ParameterLabels[0]->setText("Command delay: " + strNum1 + " ms");


	// Display velocity deadband
	string strNum3;
	strNum3=cStr(VelocityDeadbandParameter);
	ParameterLabels[1] = new cLabel(font);
	rootLabels->addChild(ParameterLabels[1]);
	ParameterLabels[1]->setLocalPos(300, -10, 0);
	ParameterLabels[1]->m_fontColor.setWhite();
	ParameterLabels[1]->setText("Velocity DB: " + strNum3);


	// Display position deadband
	string strNum4;
	strNum4=cStr(PositionDeadbandParameter);
	ParameterLabels[2] = new cLabel(font);
	rootLabels->addChild(ParameterLabels[2]);
	ParameterLabels[2]->setLocalPos(300, -30, 0);
	ParameterLabels[2]->m_fontColor.setWhite();
	ParameterLabels[2]->setText("Position DB: " + strNum4);
	

	ParameterLabels[3] = new cLabel(font);
	rootLabels->addChild(ParameterLabels[3]);
	ParameterLabels[3]->setLocalPos(300, -50, 0);
	ParameterLabels[3]->m_fontColor.setWhite();
	ParameterLabels[3]->setText("");

	ParameterLabels[4] = new cLabel(font);
	rootLabels->addChild(ParameterLabels[4]);
	ParameterLabels[4]->setLocalPos(300, -70, 0);
	ParameterLabels[4]->m_fontColor.setWhite();
	ParameterLabels[4]->setText("");


	// for each available haptic device, create a 3D cursor
	// and a small line to show velocity
	int i = 0;
	while (i < numHapticDevices)
	{

		// get a handle to the next haptic device
		// a pointer to the current haptic device
		cGenericHapticDevicePtr newHapticDevice;
		handler->getDevice(newHapticDevice, i);

		// open connection to haptic device
		newHapticDevice->open();

		// initialize haptic device
		//newHapticDevice->initialize();
		newHapticDevice->calibrate(true);
		// store the handle in the haptic device table
		hapticDevices[i] = newHapticDevice;

		// retrieve information about the current haptic device
		cHapticDeviceInfo info = newHapticDevice->getSpecifications();

		// create a cursor by setting its radius
		cShapeSphere* newCursor = new cShapeSphere(0.01);

		// add cursor to the world
		world->addChild(newCursor);

		// add cursor to the cursor table
		cursors[i] = newCursor;
		
		// create a small line to illustrate velocity
		cShapeLine* newLine = new cShapeLine(cVector3d(0, 0, 0), cVector3d(0, 0, 0));
		velocityVectors[i] = newLine;

		// add line to the world
		world->addChild(newLine);

		// create a string that concatenates the device number and model name.
		string strID = cStr(i);
		string strDevice = "#" + strID + " - " + info.m_modelName;

		//// attach a small label next to the cursor to indicate device information
		cFontPtr fontlabel = NEW_CFONTCALIBRI20();
		cLabel* newLabel = new cLabel(fontlabel);
		camera->m_frontLayer->addChild(newLabel);
		newLabel->m_fontColor.set(1.0, 1.0, 1.0);
		newLabel->setText(strDevice);
		deviceLabels[i] = newLabel;
		

		// if the device provided orientation sensing (stylus), a reference
		// frame is displayed
		if (info.m_sensedRotation == true)
		{
			// display a reference frame
			newCursor->setShowFrame(true);

			// set the size of the reference frame
			newCursor->setFrameSize(0.05, 0.05);
		}

		// create a small label to indicate the position of the device
		cLabel* newPosLabel = new cLabel(fontlabel);
		rootLabels->addChild(newPosLabel);
		newPosLabel->setLocalPos(0, -20 * i, 0);
		newPosLabel->m_fontColor.set(0.6, 0.6, 0.6);
		labels[i] = newPosLabel;

		// increment counter
		i++;
	}
	
	if (HapticDeviceConnected == 0)
	{
		
		// create a cursor by setting its radius
		cShapeSphere* newCursor = new cShapeSphere(0.01);

		// add cursor to the world
		world->addChild(newCursor);

		// add cursor to the cursor table
		cursors[i] = newCursor;
		cursors[i]->m_material->setRed();
	
		// create a small line to illustrate velocity
		cShapeLine* newLine = new cShapeLine(cVector3d(0, 0, 0), cVector3d(0, 0, 0));
		velocityVectors[i] = newLine;

		// add line to the world
		world->addChild(newLine);

		// create a string that concatenates the device number and model name.
		string strID = cStr(i);
		string strDevice = "#" + strID + " - " + "Recorded Data";

		//// attach a small label next to the cursor to indicate device information
		cFontPtr fontlabel = NEW_CFONTCALIBRI20();
		cLabel* newLabel = new cLabel(fontlabel);
		camera->m_frontLayer->addChild(newLabel);
		newLabel->m_fontColor.set(1.0, 1.0, 1.0);
		newLabel->setText(strDevice);
		deviceLabels[i] = newLabel;


		// create a small label to indicate the position of the device
		cLabel* newPosLabel = new cLabel(fontlabel);
		rootLabels->addChild(newPosLabel);
		newPosLabel->setLocalPos(0, -20 * i, 0);
		newPosLabel->m_fontColor.set(0.6, 0.6, 0.6);
		labels[i] = newPosLabel;

	}


	// here we define the material properties of the cursor when the
	// user button of the device end-effector is engaged (ON) or released (OFF)

	// a light orange material color
	matCursorButtonOFF.m_ambient.set(0.5, 0.2, 0.0);
	matCursorButtonOFF.m_diffuse.set(1.0, 0.5, 0.0);
	matCursorButtonOFF.m_specular.set(1.0, 1.0, 1.0);

	// a blue material color
	matCursorButtonON.m_ambient.set(0.1, 0.1, 0.4);
	matCursorButtonON.m_diffuse.set(0.3, 0.3, 0.8);
	matCursorButtonON.m_specular.set(1.0, 1.0, 1.0);


	//-----------------------------------------------------------------------
	// START SIMULATION
	//-----------------------------------------------------------------------

	// simulation in now running
	simulationRunning = true;

	// create a thread which starts the main haptics rendering loop
	cThread* hapticsThread = new cThread();
	hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
	
	if(!(HapticDeviceConnected==0)){
		cThread* serverThread = new cThread();
		serverThread->start(UDPServerThread, CTHREAD_PRIORITY_GRAPHICS);
	}
	//What happens here??
	//Scale signals
	
	// setup callback when application exits
	atexit(close);
	

	// start the main graphics rendering loop
	glutTimerFunc(50, graphicsTimer, 0);
	glutMainLoop();
	
	
	
	DBPosition->~DeadbandDataReduction();
	DBVelocity->~DeadbandDataReduction();
	
	// exit
	return (0);
}

void UDPServerThread(){
	//Receive a packet and change the variables
	printf("Client is listening to the server\n");
	
	size_t len_globalForce = sizeof(double) * 3;
	size_t len_timestamp = sizeof(double);
	size_t len = len_globalForce + len_timestamp;
	
	char TempBuf[len + 1];
	for(;;){
		//Write the data received from server to TempBuf
		if (HapticDeviceConnected==1)
		{	
			clientReceiver.recvPacket(TempBuf, len);
			
			//DELETEME//printf("Receiving %d, %d, %d, %d\n", TempBuf[0], TempBuf[1], TempBuf[2], TempBuf[3]);
			
			//Copy the data in the TempBuf to globalForce and timestamp
			
			memcpy(&globalForce, &TempBuf, len_globalForce);	
			memcpy(&timestamp, &TempBuf[len_globalForce], len_timestamp);
			//DELETEME//cout << timestamp << endl;}
		}
	}
}




//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
	// update the size of the viewport
	displayW = w;
	displayH = h;
	glViewport(0, 0, displayW, displayH);

	// update position of labels
	rootLabels->setLocalPos(10, displayH - 70, 0);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
	// escape key
	if ((key == 27) || (key == 'x'))
	{
		// close everything
		close();

		// exit application
		exit(0);
	}

	// option 1:
    if ( (key == 'q'))
    {
		if(DBPosition->DeadbandParameter>=0.0 && DBPosition->DeadbandParameter<0.98){
			DBPosition->DeadbandParameter = DBPosition->DeadbandParameter + 0.01;
		
		string strNum2;
		strNum2=cStr(DBPosition->DeadbandParameter);
		ParameterLabels[2]->setText("Position DB: " + strNum2);

		}
        
    }

    // option 2:
    if ((key == 'a'))
    {
		if(DBPosition->DeadbandParameter>0.0 && DBPosition->DeadbandParameter<1.0){
			DBPosition->DeadbandParameter = DBPosition->DeadbandParameter - 0.01;

			if (DBPosition->DeadbandParameter < 0.001)
				DBPosition->DeadbandParameter = 0.0;
			
		string strNum2;
		strNum2=cStr(DBPosition->DeadbandParameter);
		ParameterLabels[2]->setText("Position DB: " + strNum2);

		

		}

    }
	if (key == 'w')
    {

		if (ControlMode == 1) {
			if (DBVelocity->DeadbandParameter >= 0.0 && DBVelocity->DeadbandParameter < 0.98){
				DBVelocity->DeadbandParameter = DBVelocity->DeadbandParameter + 0.01;

				string strNum;
				strNum=cStr(DBVelocity->DeadbandParameter);
				ParameterLabels[1]->setText("Velocity DB: " + strNum);


			}
		}
		else {
		
			if (DBPosition->DeadbandParameter >= 0.0 && DBPosition->DeadbandParameter < 0.98){
				DBPosition->DeadbandParameter = DBPosition->DeadbandParameter + 0.01;

				string strNum;
				strNum=cStr(DBPosition->DeadbandParameter);
				ParameterLabels[2]->setText("Position DB: " + strNum);


			}
		
		
		}
        
    }


	if (key == 's')
    {
		if (ControlMode == 1) {
			if (DBVelocity->DeadbandParameter > 0.00 && DBVelocity->DeadbandParameter < 1.0){
				DBVelocity->DeadbandParameter = DBVelocity->DeadbandParameter - 0.01;

				if (DBVelocity->DeadbandParameter < 0.001)
					DBVelocity->DeadbandParameter = 0.0;

				string strNum;
				strNum=cStr(DBVelocity->DeadbandParameter);
				ParameterLabels[1]->setText("Velocity DB: " + strNum);


			}

		}
		else {
		

			if (DBPosition->DeadbandParameter > 0.00 && DBPosition->DeadbandParameter < 1.0){
				DBPosition->DeadbandParameter = DBPosition->DeadbandParameter - 0.01;

				if (DBPosition->DeadbandParameter < 0.001)
					DBPosition->DeadbandParameter = 0.0;

				string strNum;
				strNum=cStr(DBPosition->DeadbandParameter);
				ParameterLabels[1]->setText("Position DB: " + strNum);


			}
		
		
		}

    }


}

//---------------------------------------------------------------------------

void menuSelect(int value)
{
	switch (value)
	{
		// enable full screen display
	case OPTION_FULLSCREEN:
		glutFullScreen();
		break;

		// reshape window to original size
	case OPTION_WINDOWDISPLAY:
		glutReshapeWindow(WINDOW_SIZE_W, WINDOW_SIZE_H);
		break;
	}
}

//---------------------------------------------------------------------------

void close(void)
{
	// stop the simulation
	simulationRunning = false;

	// wait for graphics and haptics loops to terminate
	while (!simulationFinished) { cSleepMs(100); }

	// close all haptic devices
	int i = 0;
	while (i < numHapticDevices)
	{
		hapticDevices[i]->close();
		i++;
	}
}

//---------------------------------------------------------------------------
void graphicsTimer(int data)
{
	if (simulationRunning)
	{
		glutPostRedisplay();
	}


	glutTimerFunc(50, graphicsTimer, 0);
}
//---------------------------------------------------------------------------

void updateGraphics(void)
{
	// update content of position label
	for (int i = 0; i<numHapticDevices; i++)
	{
		// read position of device an convert into millimeters
		cVector3d pos;
		hapticDevices[i]->getPosition(pos);
		pos.mul(1000);

		// create a string that concatenates the device number and its position.
		string strID = cStr(i);
		string strLabel = "#" + strID + "  x: ";

		string xpos = cStr(pos.x(), 2);
		strLabel = strLabel + xpos + "   y: ";
		string ypos = cStr(pos.y(), 2);
		strLabel = strLabel + ypos + "  z: ";
		string zpos = cStr(pos.z(), 2);
		strLabel = strLabel + zpos;

		labels[i]->setText(strLabel);
		deviceLabels[i]->setLocalPos(pos.x() + (displayW - 100) / 2, pos.y() + (displayH - 100) / 2, pos.z());
	}
	if (HapticDeviceConnected==0)
	{
		int i=0;
		// read position of device an convert into millimeters
		cVector3d pos(MasterPosition[0],MasterPosition[2],MasterPosition[1]);
		//cursors[i]->setLocalPos(pos);
		pos.mul(1000);

		// create a string that concatenates the device number and its position.
		string strID = cStr(i);
		string strLabel = "#" + strID + "  x: ";

		string xpos = cStr(pos.x(), 2);
		strLabel = strLabel + xpos + "   y: ";
		string ypos = cStr(pos.y(), 2);
		strLabel = strLabel + ypos + "  z: ";
		string zpos = cStr(pos.z(), 2);
		strLabel = strLabel + zpos;

		labels[i]->setText(strLabel);
		deviceLabels[i]->setLocalPos(pos.x() + (displayW - 100) / 2, pos.y() + (displayH - 100) / 2, pos.z());
	}


	

	// render world
	camera->renderView(displayW, displayH);
	// Swap buffers
	glutSwapBuffers();

	// check for any OpenGL errors
	GLenum err;
	err = glGetError();
	if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));

	// inform the GLUT window to call updateGraphics again (next frame)
	if (simulationRunning)
	{
		glutPostRedisplay();
	}
	

}


//---------------------------------------------------------------------------

void updateHaptics(void)
{
	// main haptic simulation loop
	static bool firstrunFlag = false;

	// simulation clock
	cPrecisionClock simClock;
	simClock.start(true);	
	int kk=0;
	std::string line;
	std::istringstream ss(line);
	std::string cell;
	std::string NameFile;
	double newRotation[3];
	auto next_time = std::chrono::steady_clock::now();
	if (FileName==0 && HapticDeviceConnected==0)
	{
		NameFile="RecordedData/square_velocity.csv";
	}
	else if (FileName==1 && HapticDeviceConnected==0)
	{
		NameFile="RecordedData/circle_velocity.csv";
	}
	else if (FileName==2 && HapticDeviceConnected==0)
	{
		NameFile="RecordedData/infinity_velocity.csv";
	}
	else if (HapticDeviceConnected==0)
	{
		cout << "Error: Wrong File Option, using square trajectory" << endl;
		NameFile="RecordedData/square_velocity.csv";
	}
	std::ifstream file(NameFile);
	
	getline(file,line);
	
	cVector3d linearForce;
	std::ofstream fidRecord;
	fidRecord.open("TapAndDragObjectMaxStiffnessClientYZ.csv");
	fidRecord << "Timestamp , Position X, Position Y, Position Z, Velocity X, Velocity Y, Velocity Z, Force X, Force Y, Force Z, PacketFlag, PacketRate\n";
	int counterz=0;
	int state=1;


	while (simulationRunning)
	{
		// for each device
		int i =0;
		
		
		while (i < numHapticDevices)
		{
			if (kk==0)
			{
				cVector3d prevpos;
				hapticDevices[i]->getPosition(prevpos);
				PreviousPosition[0] = prevpos.x();
				PreviousPosition[1] = prevpos.y();
				PreviousPosition[2] = prevpos.z();
			}
			


			// read position of haptic device
			cVector3d tempPos;
			hapticDevices[i]->getPosition(tempPos);
			HapticSample* newPosition= new HapticSample;
			cVector3d linearVelocity;
			hapticDevices[i]->getLinearVelocity(linearVelocity);

			// Calculate desired force (1D motion along the Y-axis)
			double positionErrory = desiredVelocityY - tempPos.y();
			double velocityErrory = 0.0 - linearVelocity.y();
			double forceY = kP * positionErrory + kD * velocityErrory;

			// Calculate desired force (1D motion along the Y-axis)
			double positionErrorx = -0.02 - tempPos.x();
			double velocityErrorx = 0.0 - linearVelocity.x();
			double forceX = kP * positionErrorx + kD * velocityErrorx;

			// Calculate desired force (1D motion along the Y-axis)
			double positionErrorz = desiredVelocityZ - tempPos.z();
			double velocityErrorz = 0.0 - linearVelocity.z();
			double forceZ = kP * positionErrorz + kD * velocityErrorz;


			switch(state){
				case 1:
					desiredVelocityZ=desiredVelocityZ-incrZ;
					if(tempPos.z()<-0.02)
					{
						state=2;
					}
					break;
				case 2:
					desiredVelocityY=desiredVelocityY+incrY;
					if(tempPos.y()>0.02)
					{
						state=3;
					}
					break;
				case 3:
					desiredVelocityZ=desiredVelocityZ+incrZ;
					if(tempPos.z()>0.02)
					{
						state=4;
					}
					break;
				case 4:
					desiredVelocityY=desiredVelocityY-incrY;
					if(tempPos.y()<-0.02)
					{
						state=1;
					}
					break;
				default:
					break;

			}

			// if(tempPos.y()>0.02 && changeDirectionY==false)
			// {
			// 	incrY=incrY*-1;
			// 	changeDirectionY=true;
			// }

			// if(tempPos.y()<-0.02 && changeDirectionY==true)
			// {
			// 	incrY=incrY*-1;
			// 	changeDirectionY=false;
			// }

			// if(tempPos.z()>0.02 && changeDirectionZ==false)
			// {
			// 	incrZ=incrZ*-1;
			// 	changeDirectionZ=true;
			// }

			// if(tempPos.z()<-0.02 && changeDirectionZ==true)
			// {
				
			// 	incrZ=incrZ*-1;
				
			// }


			// Apply force only along the Y-axis
			cVector3d force(forceX, forceY, forceZ);
			hapticDevices[i]->setForce(force);
			hapticDevices[i]->getForce(linearForce);

			MasterPosition[0]=tempPos.x();
			MasterPosition[1]=tempPos.y();
			MasterPosition[2]=tempPos.z();
			

			// Apply deadband on position
            DBPosition->GetCurrentSample(MasterPosition);
            DBPosition->ApplyZOHDeadband(UpdatedPositionSample,&PositionTransmitFlag);

            if (PositionTransmitFlag == true) {

                PositionPacketNum++;

            }

			
			newPosition->Sample[0] = UpdatedPositionSample[0];
            newPosition->Sample[1] = UpdatedPositionSample[1];
            newPosition->Sample[2] = UpdatedPositionSample[2];
			PositionQueue.push(*newPosition);
			delete newPosition;
			HapticSample* tempPossamp = new HapticSample;
			HapticSample* tempVelsamp = new HapticSample;
			HapticSample* tempEnersamp = new HapticSample;

			
			// read orientation of haptic device
			cMatrix3d newRotation;
			hapticDevices[i]->getRotation(newRotation);

			

			// update position and orientation of cursor
			cursors[i]->setLocalPos(tempPos);
			cursors[i]->setLocalRot(newRotation);

			// read linear velocity from device
			
			HapticSample* newVelocity= new HapticSample;
			MasterVelocity[0]=linearVelocity.x();
			MasterVelocity[1]=linearVelocity.y();
			MasterVelocity[2]=linearVelocity.z();
			// Apply deadband on velocity
            DBVelocity->GetCurrentSample(MasterVelocity);
            DBVelocity->ApplyZOHDeadband(UpdatedVelocitySample,&VelocityTransmitFlag);
            
            if (VelocityTransmitFlag == true) {
            
                VelocityPacketNum++;

            }
			
			newVelocity->Sample[0] = UpdatedVelocitySample[0];
            newVelocity->Sample[1] = UpdatedVelocitySample[1];
            newVelocity->Sample[2] = UpdatedVelocitySample[2];
			VelocityQueue.push(*newVelocity);
			delete newVelocity;

			
			// MasterForce[0]=linearForce.x();
			// MasterForce[1]=linearForce.y();
			// MasterForce[2]=linearForce.z();
			MasterForce[0]=linearForce.x();
			MasterForce[1]=linearForce.y();
			MasterForce[2]=linearForce.z();//Previously global force (4Nov)
			

			//Send the port you want to receive the answer, and the position to the server
			size_t len_port = sizeof(unsigned short);
			size_t len_newPosition = sizeof(double) * 3;
			size_t len_velocity = sizeof(double) * 3;
			size_t len = len_port + len_newPosition + len_velocity;
			char TempBuf[len + 1];
			//Move the data to the TempBuf
			tempPossamp = new HapticSample;
            *tempPossamp = PositionQueue.front();
			tempVelsamp = new HapticSample;
            *tempVelsamp = VelocityQueue.front();
            PositionQueue.pop();
			VelocityQueue.pop();
			memcpy(&TempBuf, &recvPort, len_port);
			memcpy(&TempBuf[len_port], tempPossamp, len_newPosition);
			memcpy(&TempBuf[len_port+len_newPosition], tempVelsamp, len_velocity);
			clientSender.sendPacket(TempBuf, len);
			delete tempPossamp;
			delete tempVelsamp;

			

			// update arrow
			velocityVectors[i]->m_pointA = tempPos;
			velocityVectors[i]->m_pointB = cAdd(tempPos, linearVelocity);

			// read user button status
			bool buttonStatus;
			hapticDevices[i]->getUserSwitch(0, buttonStatus);

			// adjustthe  color of the cursor according to the status of
			// the user switch (ON = TRUE / OFF = FALSE)
			if (buttonStatus)
			{
				cursors[i]->setMaterial(matCursorButtonON);
			}
			else
			{
				cursors[i]->setMaterial(matCursorButtonOFF);
			}



			// hold-last-sample deadband reconstruction 	
			// send computed force to haptic device
			globalForce.x(MasterForce[0]);
			globalForce.y(MasterForce[1]);
			globalForce.z(MasterForce[2]);

			hapticDevices[i]->setForce(globalForce);
			
			double time = simClock.getCurrentTimeSeconds();

			if (kk%1000 == 0) {
            	if (ControlMode == 1) {
            		string strNumVelocity;
            		strNumVelocity=cStr(VelocityPacketNum);
            		ParameterLabels[3]->setText("Velocity packet rate: " + strNumVelocity + " packets/sec");
            		VelocityPacketNum = 0;
            	}
            	else{
                
            		string strNumPosition;
            		strNumPosition=cStr(PositionPacketNum);
            		ParameterLabels[4]->setText("Position packet rate: " + strNumPosition + " packets/sec");
            		PositionPacketNum = 0;
                
            	}

            }

			fidRecord << time << ",";
			fidRecord << tempPos.x() << "," << tempPos.y() << "," << tempPos.z() << ",";
			fidRecord << linearVelocity.x() << "," << linearVelocity.y() << "," << linearVelocity.z() << ",";
			fidRecord << linearForce.x() << "," << linearForce.y() << "," << linearForce.z() << ",";
			fidRecord << VelocityTransmitFlag << ",";
			fidRecord << VelocityPacketNum << "\n";

			// increment counter
			i++;
			
			//cout << i << endl;
		}
		if (HapticDeviceConnected==0 && numHapticDevices==0)
		{
			if (kk==0)
			{
				getline(file, line);
				std::istringstream ss(line);
				for (int jj = 0; jj < 3; ++jj) {
					if (!std::getline(ss, cell, ',')) {
						std::cerr << "Error: Not enough columns in line!" << std::endl;
					}
					PreviousPosition[jj] = std::stod(cell);  // Convert to double
				}
			}
			// read position of haptic device
			HapticSample* newPosition= new HapticSample;
			cVector3d linearVelocity;
			


			if (kk==0){
				MasterPosition[0]=PreviousPosition[0];
				MasterPosition[1]=PreviousPosition[1];
				MasterPosition[2]=PreviousPosition[2];
				MasterVelocity[0]=0.0;
				MasterVelocity[1]=0.0;
				MasterVelocity[2]=0.0;
			}
			else{
				std::getline(file, line);
				std::istringstream ss(line);
        		std::string cell;
				for (int jj = 0; jj < 3 && std::getline(ss, cell, ','); ++jj) {
					MasterPosition[jj] = std::stod(cell);  // Convert to double
					MasterPosition[jj] = MasterPosition[jj]/5000;
				}
				for (int jj = 0; jj < 3 && std::getline(ss, cell, ','); ++jj) {
					newRotation[jj] = std::stod(cell);  // Convert to double
				}
				for (int jj = 0; jj < 3 && std::getline(ss, cell, ','); ++jj) {
					MasterVelocity[jj] = std::stod(cell);  // Convert to double
					MasterVelocity[jj] = MasterVelocity[jj]/1000;
				}
			}

			// Apply deadband on position
            DBPosition->GetCurrentSample(MasterPosition);
            DBPosition->ApplyZOHDeadband(UpdatedPositionSample,&PositionTransmitFlag);

            if (PositionTransmitFlag == true) {

                PositionPacketNum++;

            }

			
			newPosition->Sample[0] = UpdatedPositionSample[0];
            newPosition->Sample[1] = UpdatedPositionSample[2];
            newPosition->Sample[2] = UpdatedPositionSample[1]+22.15/100-0.19;
			PositionQueue.push(*newPosition);
			delete newPosition;
			HapticSample* tempPossamp = new HapticSample;
			HapticSample* tempVelsamp = new HapticSample;
			cVector3d tempPos(MasterPosition[0],MasterPosition[2], MasterPosition[1]+22.15/100-0.19);
			//cVector3d tempPos(0,0,0);
			// update position and orientation of cursor
			cursors[i]->setLocalPos(tempPos*2);
			//cMatrix3d newRotation2(tempPos,tempPos,tempPos);
			//cursors[i]->setLocalRot(newRotation2);

			// read linear velocity from file
			
			HapticSample* newVelocity= new HapticSample;
			// Apply deadband on velocity
            DBVelocity->GetCurrentSample(MasterVelocity);
            DBVelocity->ApplyZOHDeadband(UpdatedVelocitySample,&VelocityTransmitFlag);
            
            if (VelocityTransmitFlag == true) {
            
                VelocityPacketNum++;

            }
			
			newVelocity->Sample[0] = UpdatedVelocitySample[0];
            newVelocity->Sample[1] = UpdatedVelocitySample[2];
            newVelocity->Sample[2] = UpdatedVelocitySample[1];
			VelocityQueue.push(*newVelocity);
			delete newVelocity;

			

			//Send the port you want to receive the answer, and the position to the server
			size_t len_port = sizeof(unsigned short);
			size_t len_newPosition = sizeof(double) * 3;
			size_t len_velocity = sizeof(double) * 3;
			size_t len = len_port + len_newPosition + len_velocity;
			char TempBuf[len + 1];
			//Move the data to the TempBuf
			tempPossamp = new HapticSample;
            *tempPossamp = PositionQueue.front();
			tempVelsamp = new HapticSample;
            *tempVelsamp = VelocityQueue.front();
            PositionQueue.pop();
			VelocityQueue.pop();
			memcpy(&TempBuf, &recvPort, len_port);
			memcpy(&TempBuf[len_port], tempPossamp, len_newPosition);
			memcpy(&TempBuf[len_port+len_newPosition], tempVelsamp, len_velocity);
			clientSender.sendPacket(TempBuf, len);
			delete tempPossamp;
			delete tempVelsamp;

			
			linearVelocity=cVector3d(MasterVelocity[0],MasterVelocity[2],MasterVelocity[1]);
			//linearVelocity=cVector3d(0,0,0);
			// update arrow
			velocityVectors[i]->m_pointA = tempPos*2;
			velocityVectors[i]->m_pointB = cAdd(tempPos*2, linearVelocity);


			// hold-last-sample deadband reconstruction 	
			// send computed force to haptic device
			
			double time = simClock.getCurrentTimeSeconds();

			if (kk%1000 == 0) {
            	if (ControlMode == 1) {
            		string strNumVelocity;
            		strNumVelocity=cStr(VelocityPacketNum);
            		ParameterLabels[3]->setText("Velocity packet rate: " + strNumVelocity + " packets/sec");
            		VelocityPacketNum = 0;
            	}
            	else{
                
            		string strNumPosition;
            		strNumPosition=cStr(PositionPacketNum);
            		ParameterLabels[4]->setText("Position packet rate: " + strNumPosition + " packets/sec");
            		PositionPacketNum = 0;
                
            	}
			}
		




			next_time += std::chrono::milliseconds(1);
        	std::this_thread::sleep_until(next_time);
                

		}
		kk++;
	}

	// exit haptics thread
	simulationFinished = true;
	fidRecord.close();
	//ClientFile.close();
}