// MyAnalysis_Jamboree.cpp
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//	AUTHOR: Frank C. Anderson, Ajay Seth
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/* Copyright (c)  2008 Stanford University
* Use of the OpenSim software in source form is permitted provided that the following
* conditions are met:
* 	1. The software is used only for non-commercial research and education. It may not
*     be used in relation to any commercial activity.
* 	2. The software is not distributed or redistributed.  Software distribution is allowed 
*     only through https://simtk.org/home/opensim.
* 	3. Use of the OpenSim software or derivatives must be acknowledged in all publications,
*      presentations, or documents describing work in which OpenSim or derivatives are used.
* 	4. Credits to developers may not be removed from executables
*     created from modifications of the source.
* 	5. Modifications of source code must retain the above copyright notice, this list of
*     conditions and the following disclaimer. 
* 
*  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; 
*  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR BUSINESS INTERRUPTION) 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.
 */

//=============================================================================
// INCLUDES
//=============================================================================
#include <iostream>
#include <string>
#include <OpenSim/Common/rdMath.h>
#include <OpenSim/Simulation/Model/DerivCallbackSet.h>
#include <OpenSim/Simulation/Model/Model.h>
#include <OpenSim/Simulation/Model/AbstractDynamicsEngine.h>
#include <OpenSim/Simulation/Model/BodySet.h>
#include "MyAnalysis_Jamboree.h"

using namespace OpenSim;
using namespace std;

//=============================================================================
// CONSTANTS
//=============================================================================

//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/**
 * Destructor.
 * Delete any variables allocated using the "new" operator.  You will not
 * necessarily have any of these.
 */
MyAnalysis_Jamboree::~MyAnalysis_Jamboree()
{

}
//_____________________________________________________________________________
/*
 * Construct an MyAnalysis_Jamboree instance.
 *
 * @param aModel Model for which the analysis is to be run.
 */
MyAnalysis_Jamboree::MyAnalysis_Jamboree(Model *aModel) :
	Analysis(aModel),
	//_bool(_boolProp.getValueBool()),
	//_boolArray(_boolArrayProp.getValueBoolArray()),
	//_int(_intProp.getValueInt()),
	//_intArray(_intArrayProp.getValueIntArray()),
	//_dbl(_dblProp.getValueDbl()),
	//_dblArray(_dblArrayProp.getValueDblArray()),
	//_vec3(_vec3Prop.getValueDblVec3()),
	//_str(_strProp.getValueStr()),
	_bodyNames(_strArrayProp.getValueStrArray())
{
	// make sure members point to NULL if not valid. 
	setNull();
	if(_model==NULL) return;

	// DESCRIPTION AND LABELS
	constructDescription();
	constructColumnLabels();

}
//_____________________________________________________________________________
/*
 * Construct an object from file.
 *
 * The object is constructed from the root element of the XML document.
 * The type of object is the tag name of the XML root element.
 *
 * @param aFileName File name of the document.
 */
MyAnalysis_Jamboree::MyAnalysis_Jamboree(const std::string &aFileName):
	Analysis(aFileName, false),
	//_bool(_boolProp.getValueBool()),
	//_boolArray(_boolArrayProp.getValueBoolArray()),
	//_int(_intProp.getValueInt()),
	//_intArray(_intArrayProp.getValueIntArray()),
	//_dbl(_dblProp.getValueDbl()),
	//_dblArray(_dblArrayProp.getValueDblArray()),
	//_vec3(_vec3Prop.getValueDblVec3()),
	//_str(_strProp.getValueStr()),
	_bodyNames(_strArrayProp.getValueStrArray())
{
	setNull();

	// Read properties from XML
	updateFromXMLNode();
}

// Copy constrctor and virtual copy 
//_____________________________________________________________________________
/*
 * Copy constructor.
 *
 */
MyAnalysis_Jamboree::MyAnalysis_Jamboree(const MyAnalysis_Jamboree &aMyAnalysis_Jamboree):
	Analysis(aMyAnalysis_Jamboree),
	//_bool(_boolProp.getValueBool()),
	//_boolArray(_boolArrayProp.getValueBoolArray()),
	//_int(_intProp.getValueInt()),
	//_intArray(_intArrayProp.getValueIntArray()),
	//_dbl(_dblProp.getValueDbl()),
	//_dblArray(_dblArrayProp.getValueDblArray()),
	//_vec3(_vec3Prop.getValueDblVec3()),
	//_str(_strProp.getValueStr()),
	_bodyNames(_strArrayProp.getValueStrArray())
{
	setNull();
	// COPY TYPE AND NAME
	*this = aMyAnalysis_Jamboree;
}
//_____________________________________________________________________________
/**
 * Clone
 *
 */
Object* MyAnalysis_Jamboree::copy() const
{
	MyAnalysis_Jamboree *object = new MyAnalysis_Jamboree(*this);
	return(object);

}
//=============================================================================
// OPERATORS
//=============================================================================
//-----------------------------------------------------------------------------
// ASSIGNMENT
//-----------------------------------------------------------------------------
//_____________________________________________________________________________
/*
 * Assign this object to the values of another.
 *
 * @return Reference to this object.
 */
MyAnalysis_Jamboree& MyAnalysis_Jamboree::
operator=(const MyAnalysis_Jamboree &aMyAnalysis_Jamboree)
{
	// Base Class
	Analysis::operator=(aMyAnalysis_Jamboree);

	// Member Variables
	_bodyNames = aMyAnalysis_Jamboree._bodyNames;

	return(*this);
}

//_____________________________________________________________________________
/**
 * SetNull().
 */
void MyAnalysis_Jamboree::
setNull()
{
	setType("MyAnalysis_Jamboree");
	setupProperties();

	// Property Default Values
	//_bool = true;
	//_boolArray.setSize(0);
	//_int = 0;
	//_intArray.setSize(0);
	//_dbl = 0.0;
	//_dblArray.setSize(0);
	//_vec3[0] = _vec3[1] = _vec3[2] = 0.0;
	//_str = "";
	_bodyNames.setSize(1);
	_bodyNames[0] = "all";
}
//_____________________________________________________________________________
/*
 * Set up the properties for your analysis.
 *
 * You should give each property a meaningful name and an informative comment.
 * The name you give each property is the tag that will be used in the XML
 * file.  The comment will appear before the property in the XML file.
 * In addition, the comments are used for tool tips in the OpenSim GUI.
 *
 * All properties are added to the property set.  Once added, they can be
 * read in and written to file.
 */
void MyAnalysis_Jamboree::
setupProperties()
{
	// Uncomment and rename the properties required for your analysis

	//_boolProp.setName("bool_parameter");
	//_boolProp.setComment("Flag indicating whether or not to ...");
	//_propertySet.append(&_boolProp);

	//_boolArrayProp.setName("array_of_bool_params");
	//_boolArrayProp.setComment("Array of flags indicating whether or not to ...");
	//_propertySet.append(&_boolArrayProp);

	//_intProp.setName("integer_parameter");
	//_intProp.setComment("Number of ...");
	//_propertySet.append(&_intProp);

	//_intArrayProp.setName("array_of_integer_paramters");
	//_intArrayProp.setComment("Array of numbers per ...");
	//_propertySet.append(&_intArrayProp);

	//_dblProp.setName("double_precision_parameter");
	//_dblProp.setComment("A double precision value for...");
	//_propertySet.append(&_dblProp);

	//_dblArrayProp.setName("array_of_doubles");
	//_dblArrayProp.setComment("Array of double precision parameters ...");
	//_propertySet.append(&_dblArrayProp);

	//_vec3Prop.setName("vec3_parameter");
	//_vec3Prop.setComment("Vector in 3 space.");
	//_propertySet.append(&_vec3Prop);

	//_strProp.setName("string_parameter");
	//_strProp.setComment("String parameter identifying ...");
	//_propertySet.append(&_strProp);

	_strArrayProp.setName("body_names");
	_strArrayProp.setComment("Names of the bodies on which to perform the analysis."
		"The key word 'All' indicates that the analysis should be performed for all bodies.");
	_propertySet.append(&_strArrayProp);
}

//=============================================================================
// CONSTRUCTION METHODS
//=============================================================================
//_____________________________________________________________________________
/**
 * Construct a description for the body kinematics files.
 */
void MyAnalysis_Jamboree::
constructDescription()
{
	string descrip;

	descrip = "\nThis file contains body positions (of origin) and orientations.\n";
	descrip += "\nUnits are S.I. units (seconds, meters, Newtons, ...)";
	if(getInDegrees()) {
		descrip += "\nAngles are in degrees.";
	} else {
		descrip += "\nAngles are in radians.";
	}
	descrip += "\n\n";

	setDescription(descrip);
}

//_____________________________________________________________________________
/**
 * Construct column labels for the output results.
 *
 * For analyses that run during a simulation, the first column is almost
 * always time.  For the purpose of example, the code below adds labels
 * appropriate for recording the translation and orientation of each
 * body in the model.
 *
 * This method needs to be called as necessary to update the column labels.
 */
void MyAnalysis_Jamboree::
constructColumnLabels()
{
	if(_model==NULL) return;

	Array<string> labels;
	labels.append("time");

	BodySet *bodySet = _model->getDynamicsEngine().getBodySet();

	if(_bodyNames[0] == "all"){
		_bodyIndices.setSize(bodySet->getSize());
		// Get indices of all the bodies.
		for(int j=0;j<bodySet->getSize();j++)
			_bodyIndices[j]=j;
	}
	else{
		_bodyIndices.setSize(_bodyNames.getSize());
		// Get indices of just the bodies listed.
		for(int j=0;j<_bodyNames.getSize();j++)
			_bodyIndices[j]=bodySet->getIndex(_bodyNames[j]);
	}

	//Do the analysis on the bodies that are in the indices list
	for(int i=0; i<_bodyIndices.getSize(); i++) {
		AbstractBody *body = bodySet->get(_bodyIndices[i]);
		if(body != NULL){ //If not a non-existant body
			labels.append(body->getName() + "_X");
			labels.append(body->getName() + "_Y");
			labels.append(body->getName() + "_Z");
			labels.append(body->getName() + "_Ox");
			labels.append(body->getName() + "_Oy");
			labels.append(body->getName() + "_Oz");
		}
	}

	setColumnLabels(labels);
}

//_____________________________________________________________________________
/**
 * Set up storage objects.
 *
 * In general, the storage objects in your analysis are used to record
 * the results of your analysis and write them to file.  You will often
 * have a number of storage objects, each for recording a different
 * kind of result.
 */
void MyAnalysis_Jamboree::
setupStorage()
{
	// Positions
	_storePos.reset(0);
	_storePos.setName("Positions");
	_storePos.setDescription(getDescription());
	_storePos.setColumnLabels(getColumnLabels());

	// Velocities
	_storeVel.reset(0);
	_storeVel.setName("Velocities");
	_storeVel.setDescription(getDescription());
	_storeVel.setColumnLabels(getColumnLabels());

	// Accelerations
	_storeAcc.reset(0);
	_storeAcc.setName("Accelerations");
	_storeAcc.setDescription(getDescription());
	_storeAcc.setColumnLabels(getColumnLabels());
}


//=============================================================================
// GET AND SET
//=============================================================================
//_____________________________________________________________________________
/**
 * Set the model for which this analysis is to be run.
 *
 * Sometimes the model on which an analysis should be run is not available
 * at the time an analysis is created.  Or, you might want to change the
 * model.  This method is used to set the model on which the analysis is
 * to be run.
 *
 * @param aModel Model pointer
 */
void MyAnalysis_Jamboree::
setModel(Model *aModel)
{
	// SET THE MODEL IN THE BASE CLASS
	Analysis::setModel(aModel);

	// UPDATE VARIABLES IN THIS CLASS
	constructDescription();
	constructColumnLabels();
	setupStorage();

	//Setup size of work array to hold body positions
	int numBodies = _bodyIndices.getSize();
	_bodypos.setSize(6*numBodies);

	//Setup size of work array to hold body velocities
	_bodyvel.setSize(6*numBodies);

	//Setup size of work array to hold body accelerations
	_bodyacc.setSize(6*numBodies);

	//Setup work arrays to hold joint velocities and accelerations
	int nq = _model->getDynamicsEngine().getNumCoordinates();
	_dqdt.setSize(nq);
	_dudt.setSize(nq);
}


//=============================================================================
// ANALYSIS
//=============================================================================
//_____________________________________________________________________________
/**
 * Compute and record the results.
 *
 * This method, for the purpose of example, records the position and
 * orientation of each body in the model.  You will need to customize it
 * to perform your analysis.
 *
 * @param aT Current time in the simulation.
 * @param aX Current values of the controls.
 * @param aY Current values of the states: includes generalized coords and speeds
 */
int MyAnalysis_Jamboree::
record(double aT,double *aX,double *aY)
{
	// GET THE MODEL READY ----------------------------------
	// Set the configuration (gen. coords and speeds) of the model.
	_model->set(aT,aX,aY);
	_model->getDerivCallbackSet()->set(aT,aX,aY);

	// Compute and apply all actuator forces.
	_model->getActuatorSet()->computeActuation();
	_model->getDerivCallbackSet()->computeActuation(aT,aX,aY);
	_model->getActuatorSet()->apply();
	_model->getDerivCallbackSet()->applyActuation(aT,aX,aY);

	// compute and apply all contact forces.
	_model->getContactSet()->computeContact();
	_model->getDerivCallbackSet()->computeContact(aT,aX,aY);
	_model->getContactSet()->apply();
	_model->getDerivCallbackSet()->applyContact(aT,aX,aY);

	// After setting the sate of the model and applying forces
	// Compute the derivative of the multibody system (speeds and accelerations)
    // NOTE: computeDerivatives() on the dynamicsEngine must be called before
	// getting accelerations and reaction forces.
	// API (Doxygen html files) for AbstractDynamicsEngine can be found in 
	// <OpenSimInstallDir>/sdk/doc/html
	_model->getDynamicsEngine().computeDerivatives(&_dqdt[0], &_dqdt[0]);
	// & sets to reference to _dqdt at the first element [0] since computeDerivatives wants
	// a pointer to a block of doubles and not an Array
	// From here on asking the dynamicsEngine for accleration info is valid

	// VARIABLES
	double dirCos[3][3];
	SimTK::Vec3 vec, angVec;
	double Mass = 0.0;

	// GROUND BODY
	AbstractBody &ground = _model->getDynamicsEngine().getGroundBody();

	// Get the set of all bodies in the model
	BodySet *bodySet = _model->getDynamicsEngine().getBodySet();

	// POSITION
	// cycle through the list of specified bodies
	for(int i=0;i<_bodyIndices.getSize();i++) {

		AbstractBody *body = bodySet->get(_bodyIndices[i]);
		SimTK::Vec3 com;
		body->getMassCenter(com);

		// GET POSITIONS AND EULER ANGLES
		_model->getDynamicsEngine().getPosition(*body,com,vec);
		_model->getDynamicsEngine().getDirectionCosines(*body,dirCos);
		_model->getDynamicsEngine().convertDirectionCosinesToAngles(dirCos,
			&angVec[0],&angVec[1],&angVec[2]);

		// CONVERT TO DEGREES?
		if(getInDegrees())
			angVec *= SimTK_RADIAN_TO_DEGREE;
		

		// FILL BODY KINEMATICS ARRAY
		int I=6*i;
		memcpy(&_bodypos[I],&vec[0],3*sizeof(double));
		memcpy(&_bodypos[I+3],&angVec[0],3*sizeof(double));
	}
	// Append this instant in time as a row in the storage 
	_storePos.append(aT,_bodypos.getSize(),&_bodypos[0]);

	// VELOCITY 
	// cycle through the list of specified bodies
	for(int i=0;i<_bodyIndices.getSize();i++) {

		AbstractBody *body = bodySet->get(_bodyIndices[i]);
		SimTK::Vec3 com;
		body->getMassCenter(com);

		// GET VELOCITIES AND ANGULAR VELOCITIES
		_model->getDynamicsEngine().getVelocity(*body, com, vec);
		_model->getDynamicsEngine().getAngularVelocity(*body, angVec);

		// CONVERT TO DEGREES?
		if(getInDegrees())
			angVec *= SimTK_RADIAN_TO_DEGREE;
		
		// FILL BODY KINEMATICS ARRAY
		int I=6*i;
		memcpy(&_bodyvel[I], &vec[0], 3*sizeof(double));
		memcpy(&_bodyvel[I+3], &angVec[0], 3*sizeof(double));
	}
	// Append this instant in time as a row in the storage 
	_storeVel.append(aT,_bodyvel.getSize(),&_bodyvel[0]);

	// ACCELERATIONS
	for(int i=0;i<_bodyIndices.getSize();i++) {

		AbstractBody *body = bodySet->get(_bodyIndices[i]);
		SimTK::Vec3 com;
		body->getMassCenter(com);

		// GET VELOCITIES AND ANGULAR VELOCITIES
		_model->getDynamicsEngine().getAcceleration(*body, com, vec);
		_model->getDynamicsEngine().getAngularAcceleration(*body, angVec);

		// CONVERT TO DEGREES?
		if(getInDegrees())
			angVec *= SimTK_RADIAN_TO_DEGREE;
		
		// FILL BODY KINEMATICS ARRAY
		int I=6*i;
		memcpy(&_bodyacc[I], &vec[0], 3*sizeof(double));
		memcpy(&_bodyacc[I+3], &angVec[0], 3*sizeof(double));
	}
	// Append this instant in time as a row in the storage 
	_storeAcc.append(aT,_bodyacc.getSize(),&_bodyacc[0]);

	return(0);
}
//_____________________________________________________________________________
/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method is meant to be called at the begining of an integration in
 * Model::integBeginCallback() and has the same argument list.
 *
 * @param aStep Step number of the integration.
 * @param aDT Size of the time step that will be attempted.
 * @param aT Current time in the integration.
 * @param aX Current control values.
 * @param aY Current states.
 * @param aYP Current pseudo states.
 * @param aDYDT Current state derivatives.
 * @param aClientData General use pointer for sending in client data.
 *
 * @return -1 on error, 0 otherwise.
 */
int MyAnalysis_Jamboree::
begin(int aStep,double aDT,double aT,double *aX,double *aY,double *aYP,double *aDYDT,
		void *aClientData)
{
	if(!proceed()) return(0);

	// RESET STORAGE
	_storePos.reset(aT);
	_storeVel.reset(aT);
	_storeAcc.reset(aT);

	// RECORD
	int status = 0;
	if(_storePos.getSize()<=0) {
		status = record(aT,aX,aY);
	}

	return(status);
}
//_____________________________________________________________________________
/**
 * This method is called to perform the analysis.  It can be called during
 * the execution of a forward integrations or after the integration by
 * feeding it the necessary data.
 *
 * When called during an integration, this method is meant to be called in
 * Model::integStepCallback(), which has the same argument list.
 *
 * @param aXPrev Controls at the beginining of the current time step.
 * @param aYPrev States at the beginning of the current time step.
 * @param aYPPrev Pseudo states at the beginning of the current time step.
 * @param aStep Step number of the integration.
 * @param aDT Size of the time step that was just taken.
 * @param aT Current time in the integration.
 * @param aX Current control values.
 * @param aY Current states.
 * @param aYP Current pseudo states.
 * @param aDYDT Current state derivatives.
 * @param aClientData General use pointer for sending in client data.
 *
 * @return -1 on error, 0 otherwise.
 */
int MyAnalysis_Jamboree::
step(double *aXPrev,double *aYPrev,double *aYPPrev,
	int aStep,double aDT,double aT,double *aX,double *aY,double *aYP,double *aDYDT,
	void *aClientData)
{
	if(!proceed(aStep)) return(0);

	record(aT,aX,aY);

	return(0);
}
//_____________________________________________________________________________
/**
 * This method is called at the end of an analysis so that any
 * necessary finalizations may be performed.
 *
 * This method is meant to be called at the end of an integration in
 * Model::integEndCallback() and has the same argument list.
 *
 * @param aStep Step number of the integration.
 * @param aDT Size of the time step that was just completed.
 * @param aT Current time in the integration.
 * @param aX Current control values.
 * @param aY Current states.
 * @param aYP Current pseudo states.
 * @param aDYDT Current state derivatives.
 * @param aClientData General use pointer for sending in client data.
 *
 * @return -1 on error, 0 otherwise.
 */
int MyAnalysis_Jamboree::
end(int aStep,double aDT,double aT,double *aX,double *aY,double *aYP,double *aDYDT,
		void *aClientData)
{
	if(!proceed()) return(0);

	record(aT,aX,aY);

	return(0);
}




//=============================================================================
// IO
//=============================================================================
//_____________________________________________________________________________
/**
 * Print results.
 * 
 * The file names are constructed as
 * aDir + "/" + aBaseName + "_" + ComponentName + aExtension
 *
 * @param aDir Directory in which the results reside.
 * @param aBaseName Base file name.
 * @param aDT Desired time interval between adjacent storage vectors.  Linear
 * interpolation is used to print the data out at the desired interval.
 * @param aExtension File extension.
 *
 * @return 0 on success, -1 on error.
 */
int MyAnalysis_Jamboree::
printResults(const string &aBaseName,const string &aDir,double aDT,
				 const string &aExtension)
{
	// POSITIONS
	_storePos.scaleTime(_model->getTimeNormConstant());
	Storage::printResult(&_storePos,aBaseName+"_"+getName()+"_pos",aDir,aDT,aExtension);

	// VELOCITIES
	_storeVel.scaleTime(_model->getTimeNormConstant());
	Storage::printResult(&_storeVel,aBaseName+"_"+getName()+"_vel",aDir,aDT,aExtension);

	// ACCELERATIONS
	_storeAcc.scaleTime(_model->getTimeNormConstant());
	Storage::printResult(&_storeAcc,aBaseName+"_"+getName()+"_acc",aDir,aDT,aExtension);

	return(0);
}