//-----------------------------------------------------------------------------
// File:     MyosinV.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Simulates myosin V walking
// Creator:  Mary Williard Elting - June 11, 2007
//-----------------------------------------------------------------------------
// Here are some more details about my Myosin V model:
// -model only one step
// -model stroke of front head as a torque spring
// -model attraction to actin binding site as a force that goes as r^-6
// -include Stokes drag
// -each monomer is modeled as a cylinder rigidly attached to a sphere, with a
//  pin joint between the monomers
// -reasonable estimates for the size and mass of these pieces are taken from structures
//  and sequence of myosin V, with calmodulin bound
// -For the force, numbers are mostly chosen so that the visualization of the simulation appears reasonable.
// -Because we dont really quantitatively understand the forces involved, it is difficult to come up with
//  more reasonable estimates of the forces.
// -The most egregious problem with this model is that momentum carries the second head to its binding
//  site.  Realistically, momentum should be pretty much irrelevant at this length/viscosity/velocity scale
//  because of the extremely low Reynolds number.  The Stokes drag should actually probably be much greater than
//  I have estimated it, because the drag should be so great that any motion is essentially instantly
//  damped out.  Because of this, it is probably better to think of modeling these kinds of systems using potentials
//  rather than using a force-oriented simulation.  In reality, it is diffusion that brings the second head to the
//  actin binding site, not momentum.  However, it seems very difficult to try to add diffusion in to this force system.
// -Another, somewhat less significant problem with this model is that everything seems a little underdamped, so that
//  both monomers osciallate a little bit about their final position before settling down.

#include "SimTKsimbody.h"
#include "UserForceAttraction.h"
#include "UserForceATPTorque.h"
#include "UserForceAttraction.h"
#include "UserForceStokesDrag.h"
using namespace SimTK;
using namespace std;
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  doSimulation( void );
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != EOF; }
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteStringToScreenAndFile( const char output[], FILE *fptr) { return WriteStringToScreen(output) && WriteStringToFile(output, fptr);}
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
bool  WriteDoubleToScreenAndFile(double x, int precision, FILE *fptr) { return WriteDoubleToFile( x, precision, stdout) && WriteDoubleToFile(x, precision, fptr);}
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
double  ConvertFromDegreesToRadians( double angleInDegrees ){return angleInDegrees/57.295779513082320876798154814105;/*1 radian is approximately 57.3 degrees*/}



//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
   // Simulate the multibody system
   bool simulationSucceeded = doSimulation();

   // Keep the screen displayed until the user presses the Enter key
   WriteStringToScreen( "\n\n Press  Enter  to terminate the program: " );
   getchar();

   // The value returned by the main function is the exit status of the program.
   // A normal program exit returns 0 (other return values usually signal an error).
   return simulationSucceeded == true ? 0 : 1;
}



//-----------------------------------------------------------------------------
bool  doSimulation( void )
{
   // Declare a multibody system (contains one or more force and matter sub-systems)
   MultibodySystem  mbs;

   // Model of myosin V includes 4 pieces with mass:  Two arms, arm1 and arm2, and 2 heads
   // head1 and head2
   // I will use nm as units of length and kDalton as units of mass.
   // The arms are modeled as cylinders 35 nm long with a 5.9 nm diameter, with 114.2 kD mass
   // The heads are modeled as spheres with a diameter 7.5 nm and a mass 84.3 kD
   // For each piece, mass is assumed to be evenly distributed and the coordinate systems for each
   // piece are such that the center of mass is located at the origin.
   // For the arms, the axes are defined so that the z axis is along the long axis of the cyliner,
   // so that a cross section parallel to the xy plane is a circle.


   // Create a matter sub-system for arm1
   SimbodyMatterSubsystem  mVmass;

   
   //SimbodyMatterSubsystem  head1;
   const Real  massOfHead1 = 84.3;
   const Vec3  head1CenterOfMassLocation( 0, 0, 0 );
   //Define a sphere inertia matrix.
   //For a sphere, Ixx=Iyy=Izz = 2*m*(r^2)/5
   const Real  headIxx = 474.2,  headIyy = 474.2,   headIzz = 474.2;
   const Real  headIxy = 0,  headIxz = 0,   headIyz = 0;
   const Inertia  head1InertiaMatrix( headIxx, headIyy, headIzz, headIxy, headIxz, headIyz );
   MassProperties  head1MassProperties( massOfHead1, head1CenterOfMassLocation, head1InertiaMatrix );
   const Transform head1OutboardFrameTransformFromGround;  // The default constructor is the identity transform
   const Rotation   inboardFrameOrientationInHead1;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromHead1Origin( 0, 0, 0 );
   const Transform  inboardFrameTransformFromHead1( inboardFrameOrientationInHead1, inboardFrameOriginLocationFromHead1Origin );
   Mobilizer head1ToGroundMobilizer = Mobilizer::Pin();
   const BodyId head1BodyId = mVmass.addRigidBody( head1MassProperties, inboardFrameTransformFromHead1, GroundId, head1OutboardFrameTransformFromGround, head1ToGroundMobilizer );
//   mbs.setMatterSubsystem( head1 ); 

   // Create the mass, center of mass, and inertia properties for arm1
   const Real  massOfArm1 = 114.2;

   // The location of arm1's center of mass is a vector from arm1's
   // origin expressed in the "x, y, z"' unit vectors fixed in tarm1's frame.
      const Vec3  arm1CenterOfMassLocation( 0, 0, 0 );

   // Create arm1's inertia matrix about its origin for the "x, y, z" unit vectors fixed in arm1's frame.
   // Note: The 3x3 inertia matrix is symmetric - so only 6 elements need to be defined.
   // Ixx, Iyy, Izz  are moments  of inertia (    diagonal terms in the matrix)
   // Ixy, Ixz, Iyz  are products of inertia (off-diagonal terms in the matrix)
   // For a cylinder, with long axis along y-direction
   // Ixx = Izz = M*(l^2 + 3* r^2)/12 , Iyy = M*(r^2)/2, off diagonal = 0
   const Real  armIxx = 11906.4,  armIzz = 11906.4,   armIyy = 496.9;
   const Real  armIxy = 0,  armIxz = 0,   armIyz = 0;
   const Inertia  arm1InertiaMatrix( armIxx, armIyy, armIzz, armIxy, armIxz, armIyz );

   // The MassProperties class holds the mass, center of mass, and inertia properties of a rigid body.
   // Although the next line creates an instance of the MassProperties class for the baeball,
   // it does not get associated with the baseball until the  addRigidBody  method.
   MassProperties  arm1MassProperties( massOfArm1, arm1CenterOfMassLocation, arm1InertiaMatrix );


   const Rotation  arm1OutboardFrameOrientationInHead1;
   const Vec3      arm1OutboardFrameOriginLocationFromHead1Origin(0, 3.5, 0);
   const Transform arm1OutboardFrameTransformFromHead1(arm1OutboardFrameOrientationInHead1,arm1OutboardFrameOriginLocationFromHead1Origin) ;  //arm1 will be attached tangent to the sphere

   // The orientation and position of the  inboard  frame  from the baseball's  frame is specified below.
   // The inboard frame's  axes are aligned with the baseball's  axes and its origin is coincident with the baseball's  origin
   // In other words, for this simple problem the inboard  frame and the baseball frame are identical.
   // Although the inboard frame can be constructed in a simple manner analogous to the outboardFrameTransformFromGround (above)
   // it is worthwhile to look at the details of the rotation matrix and position vector in the transform:
   // a. The rotation matrix relating the InboardFrame's x,y,z axes to the BaseballFrame's x,y,z axes is specified InboardFrame_BaseballFrame.
   // b. The position of the InboardFrame's origin from the BaseballFrame origin, expressed in terms of the BaseballFrame's "x, y, z" unit vectors.
   const Rotation   inboardFrameOrientationInArm1;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromArm1Origin( 0, -35, 0); // inboard frame is at the other end of the body from the origin frame (end tha attaches to the head)
   const Transform  inboardFrameTransformFromArm1( inboardFrameOrientationInArm1, inboardFrameOriginLocationFromArm1Origin );

   // Arm1 should be able to rotate and translate (will later attach to head1 with a weld joint)
   Mobilizer arm1ToHead1Mobilizer = Mobilizer::Free();
   
   // Add the properties desribed above to the arm1 bodyID
   const BodyId arm1BodyId = mVmass.addRigidBody( arm1MassProperties, inboardFrameTransformFromArm1, head1BodyId, arm1OutboardFrameTransformFromHead1, arm1ToHead1Mobilizer );
   const ConstraintId arm1ToHead1Constraint = mVmass.addWeldConstraint(arm1BodyId, inboardFrameTransformFromArm1, head1BodyId, arm1OutboardFrameTransformFromHead1);

   // Add the matter (arm1) sub-system to the system.
  //mbs.setMatterSubsystem( arm1 );

   
   //Repeat all of above for arm2
   //SimbodyMatterSubsystem  arm2;
   const Real  massOfArm2 = 114.2;
   const Vec3  arm2CenterOfMassLocation( 0, 0, 0 );
   //use the same inertia matrix as before (arms are identical)
   const Inertia  arm2InertiaMatrix( armIxx, armIyy, armIzz, armIxy, armIxz, armIyz );
   MassProperties  arm2MassProperties( massOfArm2, arm2CenterOfMassLocation, arm2InertiaMatrix );
   const Transform arm2OutboardFrameTransformFromArm1;  // The default constructor is the identity transform
   const Rotation   inboardFrameOrientationInArm2;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromArm2Origin( 0, 0, 0 );
   const Transform  inboardFrameTransformFromArm2( inboardFrameOrientationInArm2, inboardFrameOriginLocationFromArm2Origin );
   Mobilizer arm2ToArm1Mobilizer = Mobilizer::Pin();
   const BodyId arm2BodyId = mVmass.addRigidBody( arm2MassProperties, inboardFrameTransformFromArm2, arm1BodyId, arm2OutboardFrameTransformFromArm1, arm2ToArm1Mobilizer );
   //const BodyId arm2BodyId = mVmass.addRigidBody( arm2MassProperties, inboardFrameTransformFromArm2, GroundId, arm2OutboardFrameTransformFromArm1, arm2ToArm1Mobilizer );
   
   //mbs.setMatterSubsystem(  ); 

   //Repeat all of above for head2
   //SimbodyMatterSubsystem  head2;
   const Real  massOfHead2 = 84.3;
   const Vec3  head2CenterOfMassLocation( 0, 0, 0 );
   // Use same definition for head inertia matrix as above
   const Inertia  head2InertiaMatrix( headIxx, headIyy, headIzz, headIxy, headIxz, headIyz );
   MassProperties  head2MassProperties( massOfHead2, head2CenterOfMassLocation, head2InertiaMatrix );
  

   const Rotation   outboardFrameOrientationInArm2;// (1,0,0, 0,1,0, 0,0,1)
   const Vec3       outboardFrameOriginLocationFromArm2Origin(0, -35, 0);//origin is at end of arm2 away from junction with arm1
   const Transform  head2OutboardFrameTransformFromArm2(outboardFrameOrientationInArm2,outboardFrameOriginLocationFromArm2Origin);
   const Rotation   inboardFrameOrientationInHead2;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromHead2Origin( 0, 3.75, 0 );
   const Transform  inboardFrameTransformFromHead2( inboardFrameOrientationInHead2, inboardFrameOriginLocationFromHead2Origin );
   Mobilizer head2ToArm2Mobilizer = Mobilizer::Free();
   const BodyId head2BodyId = mVmass.addRigidBody( head2MassProperties, inboardFrameTransformFromHead2, arm2BodyId, head2OutboardFrameTransformFromArm2, head2ToArm2Mobilizer );
   const ConstraintId head2ToArm1Constraint = mVmass.addWeldConstraint(head2BodyId, inboardFrameTransformFromHead2, arm2BodyId, head2OutboardFrameTransformFromArm2);


   mbs.setMatterSubsystem( mVmass ); 

   // Add a force sub-system to this multi-body system.
   GeneralForceElements userForceElements;
   mbs.addForceSubsystem( userForceElements );
   // Although "new" was used to allocate this UserForce, do not "delete" it.
   // This bug will be fixed in the next version of Simbody so it can take an object from the stack or heap.
   // For now, addUserForce takes ownership of the allocated item and takes care of deleting it at the end.
   // This applies the force cos(t)
   //This "torque spring" is the model for the conformational change that occurs in the front head, which then becomes the rear head,
   // as it strokes forward.  There is damping in this spring that might be analagous to energy absorbed by the molecule during
   // the conformational change.  This acts on myosin1, visualized in red
   Real monomer1Angle = 0.483;//this is the angle from the vertical
   Real torqueSpringK = 1000000000;
   Real torqueSpringDamping = 1000000;
   UserForceATPTorqueSpring *forceFromMotorDomain = new UserForceATPTorqueSpring(head1BodyId, monomer1Angle, torqueSpringK, torqueSpringDamping);
   userForceElements.addUserForce( forceFromMotorDomain );
   
   //This attractive force is the force that pull the head that is becoming the front head (visualized in blue).
   //It is modelled as a force that goes as r^-6, so that it falls off very quickly as the head that is coming in to dock
   //is farther away.  However, the exact r^6 dependence may not be a very realistic choice.
   //This bond is probably made up primarily of VDW interactions, that do individually all off as r^6, but they won't
   // be so easily added to get an overall r^6 dependence.
   //This force also has some damping, that can be thought of as energy absorbed into the many degrees of freedom of the molecule.
   Vec3 actinBindingSite(36, 0, 0);
   Vec3 head2LocationInHead2(0,0,0);
   Real attractionCoeff = 100000;
   //This max is here so that the force doesn't completely blow up as the distace gets very small.
   //Otherwise, the simulator gets very slow as it tries to divide by zero.
   Real maxAttractiveForce = 300000;
   Real dampingCoeff = 100000;
   UserForceAttraction *actinBindingForce = new UserForceAttraction(head2BodyId, actinBindingSite, attractionCoeff, head2LocationInHead2, maxAttractiveForce, dampingCoeff);
   userForceElements.addUserForce( actinBindingForce);

   //Add the drag forces to both bodies.  Divide the body up into 6 places drag is applied: 1 for the head and 5 in the lever arm
   //The drag coeff for the head will be proportional to its radius, 2.95 nm
   //The drag coeff for the arm will be proportional to its radius, 3.75 nm
   //Stokes drag is the appropriate model to use since for very high Reynolds number, as we have here, Stokes drag approximation applies
   
   Real stokesDragConstant = 10000;
   //drag coefficient = stokesDragConstant*radius
   const Real radiusOfHead = 2.95;
   const Real radiusOfArm = 3.75;

   Vec3 vectorToStokesLocationHead(0,0,0);
   UserForceStokesDrag *stokesDragHead1 = new UserForceStokesDrag(head1BodyId, stokesDragConstant*radiusOfHead, vectorToStokesLocationHead);
   userForceElements.addUserForce(stokesDragHead1);
   
   Vec3 vectorToStokesLocationArm1(0,-7,0);
   UserForceStokesDrag *stokesDragArm11 = new UserForceStokesDrag(arm1BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm1);
   userForceElements.addUserForce(stokesDragArm11);
   Vec3 vectorToStokesLocationArm2(0,-14,0);
   UserForceStokesDrag *stokesDragArm12 = new UserForceStokesDrag(arm1BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm2);
   userForceElements.addUserForce(stokesDragArm12);

   Vec3 vectorToStokesLocationArm3(0,-21,0);
   UserForceStokesDrag *stokesDragArm13 = new UserForceStokesDrag(arm1BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm3);
   userForceElements.addUserForce(stokesDragArm13);

   Vec3 vectorToStokesLocationArm4(0,-28,0);
   UserForceStokesDrag *stokesDragArm14 = new UserForceStokesDrag(arm1BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm4);
   userForceElements.addUserForce(stokesDragArm14);

   Vec3 vectorToStokesLocationArm5(0, -35, 0);
   UserForceStokesDrag *stokesDragArm15 = new UserForceStokesDrag(arm1BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm5);
   userForceElements.addUserForce(stokesDragArm15);

   Vec3 vectorToPivotHead2(0, 38.5, 0);
   UserForceStokesDrag *stokesDragHead2 = new UserForceStokesDrag(head1BodyId, stokesDragConstant*radiusOfHead, vectorToStokesLocationHead);
   userForceElements.addUserForce(stokesDragHead2);
   
   Vec3 vectorToPivotArm2(0, 0, 0);
   UserForceStokesDrag *stokesDragArm21 = new UserForceStokesDrag(arm2BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm1);
   userForceElements.addUserForce(stokesDragArm21);

   UserForceStokesDrag *stokesDragArm22 = new UserForceStokesDrag(arm2BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm2);
   userForceElements.addUserForce(stokesDragArm22);

   UserForceStokesDrag *stokesDragArm23 = new UserForceStokesDrag(arm2BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm3);
   userForceElements.addUserForce(stokesDragArm23);

   UserForceStokesDrag *stokesDragArm24 = new UserForceStokesDrag(arm2BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm4);
   userForceElements.addUserForce(stokesDragArm24);

   UserForceStokesDrag *stokesDragArm25 = new UserForceStokesDrag(arm2BodyId, stokesDragConstant*radiusOfArm, vectorToStokesLocationArm5);
   userForceElements.addUserForce(stokesDragArm25);
      
   // Create a state for this system.
   // Define appropriate states for this multi-body system.
   // Set the initial time to 0.0
   State s;
   mbs.realize( s );
   s.setTime( 0.0 );

   // Set the initial values for the configuration variables (x,y,z) for arm1
   mVmass.setMobilizerQ(s, head1BodyId, 0, monomer1Angle); //head1 mobilizer is a pin joint, starting angle is backwards 1.08 radians
   mVmass.setMobilizerQ(s, arm2BodyId, 0, -(3.1415-2*(3.1415/2-monomer1Angle)) );

   // Create a study using the Runge Kutta Merson integrator (alternately use the CPodesIntegrator)
   RungeKuttaMerson myStudy( mbs, s );

   // Set the numerical accuracy for the integrator
   myStudy.setAccuracy( 1.0E-7 );

   // The next statement does lots of accounting
   myStudy.initialize();


  
   // Visualize results with VTKReporter
   VTKReporter animationResults( mbs );


   const Real radiusOfActin = 2.5;
   const Real halfLengthOfActin = 36;
   DecorativeCylinder yellowCylinder = DecorativeCylinder(radiusOfActin, halfLengthOfActin);
   yellowCylinder.setColor(Yellow);
   yellowCylinder.setOpacity(0.5);
   const Rotation  dummyRotation;
   const Rotation  actinToGroundRotation = dummyRotation.aboutZ(1.57);
   const Vec3      actinToGroundLocation(0,0,0);
   const Transform actinToGroundTransform(actinToGroundRotation, actinToGroundLocation);
   animationResults.addDecoration(GroundId, actinToGroundTransform, yellowCylinder);

   const Real myosinOpacity = 0.7;
   // For visualization purposes only, create a red sphere
//   const Real radiusOfHead = 3.75;
   DecorativeSphere redSphere = DecorativeSphere( radiusOfHead );
   redSphere.setColor(Red);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   redSphere.setOpacity(myosinOpacity);   // 0.0 is solid and 1.0 is transparent

  // Decorate head1 with the red sphere at head1's origin
   const Rotation  head1ToRedSphereOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3      head1OriginToRedSphereOriginLocation( 0, 0, 0 );
   const Transform head1ToRedSphereTransform( head1ToRedSphereOrientation, head1OriginToRedSphereOriginLocation );
   
   animationResults.addDecoration( head1BodyId, head1ToRedSphereTransform, redSphere );
   DecorativeSphere blueSphere = DecorativeSphere( radiusOfHead );
   blueSphere.setColor(Blue);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   blueSphere.setOpacity(myosinOpacity);   // 0.0 is solid and 1.0 is transparent
   const Rotation  head2ToBlueSphereOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3      head2OriginToBlueSphereOriginLocation( 0, 0, 0 );
   const Transform head2ToBlueSphereTransform( head2ToBlueSphereOrientation, head2OriginToBlueSphereOriginLocation );
   animationResults.addDecoration( head2BodyId, head2ToBlueSphereTransform, blueSphere );

 //  const Real radiusOfArm = 2.95;
   const Real halfLengthOfArm = 17.5;
   DecorativeCylinder redCylinder = DecorativeCylinder(radiusOfArm, halfLengthOfArm);
   redCylinder.setColor(Red);
   redCylinder.setOpacity(myosinOpacity);   
   const Rotation  armToCylinderOrientation;
   const Vec3      armOriginToCylinderLocation(0, -17.5, 0);
   const Transform armToCylinderTransform(armToCylinderOrientation, armOriginToCylinderLocation);
   animationResults.addDecoration(arm1BodyId, armToCylinderTransform, redCylinder);

   DecorativeCylinder blueCylinder = DecorativeCylinder(radiusOfArm, halfLengthOfArm);
   blueCylinder.setColor(Blue);
   blueCylinder.setOpacity(myosinOpacity);  
   animationResults.addDecoration(arm2BodyId, armToCylinderTransform, blueCylinder);

  /* DecorativeCylinder purpleCylinder = DecorativeCylinder(10, 10);
   purpleCylinder.setColor(Purple);
   animationResults.addDecoration(head1BodyId, head1ToRedSphereTransform, purpleCylinder);*/



   const Real radiusOfActinMarker = 1;
   DecorativeSphere greenSphere1 = DecorativeSphere(radiusOfActinMarker);
   greenSphere1.setColor(Green);
   greenSphere1.setOpacity(0.2);
   const Rotation actinMarkerToGroundRotation;
   Vec3 actinMarkerToGroundLocation(0,0,0);
   Transform actinMarkerToGroundTransform(actinMarkerToGroundRotation, actinMarkerToGroundLocation);
   animationResults.addDecoration(GroundId, actinMarkerToGroundTransform, greenSphere1);
   Vec3 actinMarkerToGroundLocation2(-36,0,0);
   Transform actinMarkerToGroundTransform2(actinMarkerToGroundRotation, actinMarkerToGroundLocation2);
   animationResults.addDecoration(GroundId, actinMarkerToGroundTransform2, greenSphere1);
   Vec3 actinMarkerToGroundLocation3(36,0,0);
   Transform actinMarkerToGroundTransform3(actinMarkerToGroundRotation, actinMarkerToGroundLocation3);
   animationResults.addDecoration(GroundId, actinMarkerToGroundTransform3, greenSphere1);






   // Set the numerical integration step and the time for the simulation to run
   const Real integrationStepDt = 0.01;
   const Real finalTime =12.0;
   const Real finalTimeCompare = finalTime - 0.01*integrationStepDt;

   FILE *outputFile = FileOpenWithMessageIfCannotOpen("MyosinVResults.txt", "w");
   WriteStringToScreenAndFile( "time   head2Velocity  arm1AngleWithVertical  arm2AngleWithVertical \n", outputFile );

   
   // Run the simulation and print the results
   while( 1 )
   {
      Real time = s.getTime();
      // Animate the results for this step
      animationResults.report(s);

      Vec3 head2Velocity = mVmass.calcBodyOriginVelocityInBody(s, head2BodyId, GroundId);
      Real velocityNorm = head2Velocity.norm();
      
      Vec3 yVec(0, 1, 0);
      Vec3 orientation1 = mVmass.calcBodyVectorInBody(s, arm1BodyId, yVec, GroundId);
      Real currentAngle1 = atan(orientation1[0]/orientation1[1]);

      Vec3 orientation2 = mVmass.calcBodyVectorInBody(s, arm2BodyId, yVec, GroundId);
      Real currentAngle2 = atan(orientation2[0]/orientation2[1]);


      WriteDoubleToScreenAndFile(time,7,  outputFile);
      WriteStringToScreenAndFile(" ", outputFile);
      WriteDoubleToScreenAndFile(velocityNorm, 7, outputFile);
      WriteStringToScreenAndFile(" ", outputFile);
      WriteDoubleToScreenAndFile(currentAngle1, 7, outputFile);
      WriteStringToScreenAndFile(" ", outputFile);
      WriteDoubleToScreenAndFile(currentAngle2, 7, outputFile);
      WriteStringToScreenAndFile("\n", outputFile);

      // Check if integration has completed
      if( time >= finalTimeCompare ) break;

      // Increment time step
      myStudy.step( time + integrationStepDt );
   }



   // Simulation completed properly
   return true;
}


//-----------------------------------------------------------------------------
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute )
{
   // Try to open the file
   FILE *Fptr1 = fopen( filename, attribute );

   // If unable to open the file, issue a message
   if( !Fptr1 )
   {
      WriteStringToScreen( "\n\n Unable to open the file: " );
      WriteStringToScreen( filename );
      WriteStringToScreen( "\n\n" );
   }

   return Fptr1;
}


//-----------------------------------------------------------------------------
bool  WriteDoubleToFile( double x, int precision, FILE *fptr )
{
   // Ensure the precision (number of digits in the mantissa after the decimal point) makes sense.
   // Next, calculate the field width so it includes one extra space to the right of the number.
   if( precision < 0 || precision > 17 ) precision = 5;
   int fieldWidth = precision + 8;

   // Create the format specifier and print the number
   char format[20];
   sprintf( format, " %%- %d.%dE", fieldWidth, precision );
   return fprintf( fptr, format, x ) >= 0;
}