//===========================================================================
// Your implementation of the spring-damper system 
//===========================================================================

//#include <stdafx.h>
#include "MyImplementation.h"

double textureForce[1000];

// Implement the model inside the function (This function will be called at each haptic update loop)
void myUpdate()
{
	// read position of haptic device
    cVector3d newPosition;
    hapticDevice->getPosition(newPosition);

    // update position of cursor
    a3DCursor->setPos(newPosition);

	// save position of cursor
	posiOFS << newPosition.x << "," << newPosition.y << "," << newPosition.z << "," << aTimer.getCurrentTimeSeconds() << std::endl;

    // read linear velocity from device
    cVector3d linearVelocity;
    hapticDevice->getLinearVelocity(linearVelocity);

	// update arrow
    displacementVector->m_pointA = cVector3d(0,0,0);
    displacementVector->m_pointB = newPosition;

    // compute a reaction force
    cVector3d force (0,0,0);

	//---------------Wall texture simulation----------------//
	if (newPosition.y < 0.0)
	{
		double Kp = 8000.0; // [N/m]
		double Kv = 0.5; // Kv < 20.0

		force = cMul(-Kp, newPosition);
		force += cMul(-Kv, linearVelocity);

		force.x = 0.0; force.z = 0.0;	
	}

	// send computed force to haptic device
	hapticDevice->setForce(force);
}

// Prepare your model inside the function (This function will be called only once during initialization)
void myInitialization()
{
	// Save texture data
	std::ofstream textureOFS;
	std::string textureFileName;

	textureFileName = "../../TextureData.csv";
	textureOFS.open(textureFileName);

	//----------- Generate an array of data following normal distribution -----------//
	double gauss_mean = 0.0;
	double gauss_var = 2.0;
	long idumumber = -1;

	for (int i = 0; i < 1000; i++)
	{
		textureForce[i] = gauss(gauss_mean, gauss_var, &idumumber);
		textureOFS << i << "," << textureForce[i] << std::endl;
	}
	textureOFS.close();
}