//-----------------------------------------------------------------------------
// File:     InertiaPropertiesOfPDBMoleculesEA.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates the mass, center of mass, and inertia properties of
//           molecules that are downloaded from www.pdb.orb
// Author:   Paul Mitiguy - April 24, 2007
//-----------------------------------------------------------------------------
// The following are standard C/C++ header files.
// If a filename is enclosed inside < >  it means the header file is in the Include directory.
// If a filename is enclosed inside " "  it means the header file is in the current directory.
#include <ctype.h>      // Character Types
#include <math.h>       // Mathematical Constants
#include <stdarg.h>     // Variable Argument Lists
#include <stdio.h>      // Standard Input/Output Functions
#include <stdlib.h>     // Utility Functions
#include <string.h>     // String Operations
#include <signal.h>     // Signals (Contol-C + Unix System Calls)
#include <setjmp.h>     // Nonlocal Goto (For Control-C)
#include <time.h>       // Time and Date information
#include <assert.h>     // Verify Program Assertion
#include <errno.h>      // Error Codes (Used in Unix system())
#include <float.h>      // Floating Point Constants
#include <limits.h>     // Implementation Constants
#include <stddef.h>     // Standard Definitions
#include <exception>    // Exception handling (e.g., try, catch throw)
//-----------------------------------------------------------------------------
#include "SimTKsimbody.h"
#include "ChemicalElement.h"
using namespace SimTK;
using namespace std;
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void );
bool  GetStringFromFile(     char inputString[], unsigned long maxSizeOfString, FILE *fptr )  { return fgets( inputString, maxSizeOfString, fptr ) != NULL; }
bool  GetStringFromKeyboard( char inputString[], unsigned long maxSizeOfString )              { return GetStringFromFile( inputString, maxSizeOfString, stdin ); }
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != EOF; }
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteIntegerToFile( int x, FILE *fptr )                       { return fprintf( fptr, "%d", x) >= 0; }
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
const char*  ConvertStringToDouble( const char *s, double &returnValue, double defaultValue );

// Returns the number of molecules in the multibody system.  Note: Check the status of successReadingPdb before continuing.
int  AddMoleculesFromPdbFileToSimbodyMatterSubsystem( SimbodyMatterSubsystem &allTheMolecules, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );
int  AddBoundingBoxToSimbodyMatterSubsystem( SimbodyMatterSubsystem &allTheMolecules );

int  CalculateMassPropertiesOfPdbMolecule( MassProperties &massProperties, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );
int  SetMobilizerStatesFromPdbFile( SimbodyMatterSubsystem &allTheMolecules, State &state, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );
int  SetMobilizerStatesForBoundingBox( SimbodyMatterSubsystem &allTheMolecules, BodyId &brickId, State &state);

// Sets the decorative geometry for all molecules to spheres
void  DecorateAllTheMoleculesWithSpheres( SimbodyMatterSubsystem &allTheMolecules, Vec6 &boundingBox, VTKReporter &viewer, int numberOfMolecules );

// Sets the decorative geometry for a bounding box
void  DecorateBoundingBoxWithBrick( BodyId &brickId, Real xWidth, Real yWidth, Real zWidth, VTKReporter &viewer, Vec3 boundingBoxCenter );

// Calculates the bounding box for the origins of all the bodies in the matter subsystem.
// Returns 6 real numbers in the following order:  minX, maxX,  minY, maxY,  minZ, maxZ
Vec6  CalculateBoundingBoxOfBodyOriginsUsingGroundXYZDirections( const SimbodyMatterSubsystem &allTheBodies, const State &state, int numberOfMolecules );

// Takes a single line from a PDB file and parses it.  Returns its ChemicalElement (if there is one).
const ChemicalElement*  GetChemicalElementAndPositionFromPDBInputLine( const char *inputLine, char selectedPdbChain, bool includeHETAM, Vec3 &atomPositionFromLaboratoryOrigin, bool &successReadingPdb );

// Takes a single line from a PDB file and parses it looking for the atom's x,y,z positions from the laboratory origin
Vec3  GetAtomPositionFromPdbLine( const char *inputLine, bool &successReadingPdb );


//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
   // Default value is program failed
   bool programSucceeded = false;

   // It is a good programming practice to do little in the main function of a program.
   // The try-catch code in this main routine catches exceptions thrown by functions in the
   // try block, e.g., catching an exception that occurs when a NULL pointer is de-referenced.
   try
   {
      // Do the required programming tasks
      programSucceeded = DoRequiredTasks();
   }
   // This catch statement handles certain types of exceptions
   catch( const exception &e )
   {
      WriteStringToScreen( "\n\n Error: Programming error encountered.\n The exception thrown is: " );
      WriteStringToScreen( e.what() );
   }
   // The exception-declaration statement (...) handles any type of exception,
   // including C exceptions and system/application generated exceptions.
   // This includes exceptions such as memory protection and floating-point violations.
   // An ellipsis catch handler must be the last handler for its try block.
   catch( ... )
   {
      WriteStringToScreen( "\n\n Error: Programming error encountered.\n An unhandled exception was thrown." );
   }

   // Give the user a chance to see a final message before exiting the program.
   WriteStringToScreen( programSucceeded ? "\n\n Program succeeded.  Press  Enter  to finish: " : "\n\n Program failed.  Press  Enter  to finish: " );
   getchar();  // Keeps the screen displayed until the user presses the Enter (or Return) key.

   // 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 programSucceeded == true ? 0 : 1;
}


//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void )
{
   // Name of .pdb file entered by user
   char nameOfPdbFile[400];

   // Prompt the user to enter the name of the .pdb file (from the keyboard)
   WriteStringToScreen(  "\n This program calculates the mass distribution properties of a .pdb file" );
   WriteStringToScreen("\n\n Enter the full path to the .pdb file: " );
   GetStringFromKeyboard( nameOfPdbFile, 400 );
   char *returnCharacterFound = strchr( nameOfPdbFile, '\n');
   if( returnCharacterFound ) *returnCharacterFound = '\0';

   // Calculate the mass, center of mass, and inertia properties about the laboratory origin
   bool includeHETAM = false;
   char selectedPdbChain = '\0';    // To read just Chain A, set this to 'A'.  To read all chains, set it to '\0';
   bool successReadingPdb = false;
   MassProperties pdbMassProperties;
   int numberOfAtomsInCalculation = CalculateMassPropertiesOfPdbMolecule( pdbMassProperties, nameOfPdbFile, selectedPdbChain, includeHETAM, successReadingPdb );

   // Check if successful on reading the file
   if( successReadingPdb == false  ||  numberOfAtomsInCalculation == 0 )
   {
      WriteStringToScreen( "\n\n Unable to properly calculate mass distribution properties" );
      return false;
   }

   // Open a file for writing (return false if cannot open file)
   const char *outputFileName = "InertiaPropertiesOfPDBMolecule1G2S.txt";
   FILE *fptr = FileOpenWithMessageIfCannotOpen( outputFileName, "w" );
   if( !fptr ) return false;

   // Get each of the relevant pieces for writing to output file
   const Real     totalMassInAMU = pdbMassProperties.getMass();
   const Vec3&    centerOfMassLocationFromLaboratoryOrigin = pdbMassProperties.getMassCenter();
   const Inertia& inertiaMatrixAboutLaboratoryOrigin = pdbMassProperties.getInertia();

   // Use shift theorem to calculate the inertia matrix about the molecules mass center
   const Inertia inertiaMatrixAboutSystemCM = inertiaMatrixAboutLaboratoryOrigin.shiftToMassCenter( centerOfMassLocationFromLaboratoryOrigin, totalMassInAMU );

   // Write the results to the file.
   int precision = 5;

   WriteStringToFile( "\n\n Number of relevant atoms in .pdb file: ", fptr );
   WriteIntegerToFile( numberOfAtomsInCalculation, fptr );

   WriteStringToFile( "\n\n System mass (in AMU):\n", fptr );
   WriteDoubleToFile( totalMassInAMU, precision, fptr );

   WriteStringToFile( "\n\n System center of mass position from point O (in Angstroms):\n", fptr );
   WriteVec3ToFile( centerOfMassLocationFromLaboratoryOrigin, precision, fptr );

   WriteStringToFile( "\n\n System inertia matrix about laboratory origin (in AMU * Angstrom^2):\n", fptr );
   const Mat33 inertiaMatrixAboutLaboratoryOriginMat33 = inertiaMatrixAboutLaboratoryOrigin.toMat33();
   WriteMat33ToFile( inertiaMatrixAboutLaboratoryOriginMat33, precision, fptr );

   WriteStringToFile( "\n\n System inertia matrix about system's center of mass (in AMU * Angstrom^2):\n", fptr );
   WriteMat33ToFile( inertiaMatrixAboutSystemCM.toMat33(), precision, fptr );

   // Declare a multibody system (which holds the matter sub-system - containing all the molecules)
   MultibodySystem  mbs;
   SimbodyMatterSubsystem allTheMolecules;
   mbs.setMatterSubsystem( allTheMolecules );

   // Next, we are going to add all these molecules to a Simbody simulation
   AddMoleculesFromPdbFileToSimbodyMatterSubsystem( allTheMolecules, nameOfPdbFile, selectedPdbChain, includeHETAM, successReadingPdb );
   // Add a rigid body to visualize a bounding box.
   AddBoundingBoxToSimbodyMatterSubsystem( allTheMolecules );

   // Create a state for this system, realize its state (set aside room for its Qs and Us) and set its time to 0.0
   State state;
   mbs.realize( state );
   state.setTime( 0.0 );

   // Set the initial values for each molecule's x, y, z position by re-parsing the file
   SetMobilizerStatesFromPdbFile( allTheMolecules, state, nameOfPdbFile, selectedPdbChain, includeHETAM, successReadingPdb );

   // Get ID number for brick body 
   // Assuming the bodies are sequentially numbered, the next line should create the proper Id
   int  numberOfRigidBodies = allTheMolecules.getNBodies();
   BodyId brickId( numberOfRigidBodies - 1 );

   // Set the initial values for bounding box.
   SetMobilizerStatesForBoundingBox( allTheMolecules, brickId, state);

   // realize again for no apparent reason.
   mbs.realize( state );

   // Calculate the bounding box of this set of molecules
   int numberOfMolecules = allTheMolecules.getNBodies()-1;
   Vec6 boundingBox = CalculateBoundingBoxOfBodyOriginsUsingGroundXYZDirections( allTheMolecules, state, numberOfMolecules );
   Real xWidth = boundingBox[1]-boundingBox[0];
   Real yWidth = boundingBox[3]-boundingBox[2];
   Real zWidth = boundingBox[5]-boundingBox[4];
   Real volume = xWidth * yWidth * zWidth;

   Real xCenter = (boundingBox[1]+boundingBox[0])/2;
   Real yCenter = (boundingBox[3]+boundingBox[2])/2;
   Real zCenter = (boundingBox[5]+boundingBox[4])/2;
   Vec3 boundingBoxCenter( xCenter, yCenter, zCenter);
   
   // Create a study using the Runge Kutta Merson integrator (alternately use the CPodesIntegrator)
   RungeKuttaMerson myStudy( mbs, state );

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

   WriteStringToFile( "\n\n Bounding box volume (in Angstoms^3):\n", fptr );
   WriteDoubleToFile( volume, precision, fptr );

   // Calculations performed successfully
   WriteStringToScreen( "\n\n Calculations were successfuly performed.\n Results are in the file: " );
   WriteStringToScreen( outputFileName );

   // Visualize results with VTKReporter (be patient)
   VTKReporter viewer( mbs );
   DecorateAllTheMoleculesWithSpheres( allTheMolecules, boundingBox, viewer, numberOfMolecules );
   DecorateBoundingBoxWithBrick( brickId, xWidth, yWidth, zWidth, viewer,  boundingBoxCenter );

   // View the results
   viewer.report( state );

   return true;
}


//-----------------------------------------------------------------------------
int  AddMoleculesFromPdbFileToSimbodyMatterSubsystem( SimbodyMatterSubsystem &allTheMolecules, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb )
{
   // Open the .pdb file for reading
   FILE *pdbFile = FileOpenWithMessageIfCannotOpen( nameOfPdbFile, "r" );
   if( !pdbFile ) {successReadingPdb = false;  return 0;}

   // Default is that there are no errors in reading PDB file
   successReadingPdb = true;

   // Number of atoms included in this calculation
   int  numberOfAtomsInCalculation = 0;

   // Space for line in .pdb file
   char inputLine[2000];

   // Keep getting strings from the file until there are no more
   while( GetStringFromFile( inputLine, 2000, pdbFile ) )
   {
      // Get the chemical element for this line (if there is one)
      // Also, get this atom's position (from the designated columns from the pdb)
      Vec3  atomPositionFromLaboratoryOrigin;
      const ChemicalElement *chemElement = GetChemicalElementAndPositionFromPDBInputLine( inputLine, selectedPdbChain, includeHETAM, atomPositionFromLaboratoryOrigin, successReadingPdb );

      // It may be that this was supposed to be a chemical element, but the data is not in the correct fields
      if( successReadingPdb == false )  break;

      // It may be that this line of the .pdb does not contain ATOM or HETAM information
      if( !chemElement ) continue;

      // At this point, we have found a valid atom to include in the mass calculations
      ++numberOfAtomsInCalculation;

      // To add a particle (or rigid body), must create a MassProperties class for it
      Real  moleculeMass = chemElement->getMass();
      const Vec3  moleculeCenterOfMassLocationFromMoleculeOrigin( 0, 0, 0 );
      const Inertia  moleculeInertiaMatrixAboutMoleculeOrigin( 0, 0, 0, 0, 0, 0 );
      MassProperties  moleculeMassProperties( moleculeMass, moleculeCenterOfMassLocationFromMoleculeOrigin, moleculeInertiaMatrixAboutMoleculeOrigin );

      // To add a particle (or rigid body), must inform how it is connected
      const Transform outboardFrameTransformFromGround;   // Default constructor is the identity transform
      const Transform  inboardFrameTransformFromMolecule; // Default constructor is the identity transform
      Mobilizer moleculeToGroundMobilizer = Mobilizer::Cartesian;

      // Add this to the matter subsystem (does not get directly added to the multibody system)
      allTheMolecules.addRigidBody( moleculeMassProperties, inboardFrameTransformFromMolecule, GroundId, outboardFrameTransformFromGround, moleculeToGroundMobilizer );
   }

   // Close the pdb file before returning (setting it to NULL is good practice)
   fclose( pdbFile );
   pdbFile = NULL;

   // Was able to add all particles
   return numberOfAtomsInCalculation;
}

int AddBoundingBoxToSimbodyMatterSubsystem( SimbodyMatterSubsystem &allTheMolecules )
{
   //  Add box to all the molecules
   // To add a particle (or rigid body), must create a MassProperties class for it
   Real  brickMass = 1;
   const Vec3  brickCenterOfMassLocationFromBrickOrigin( 0, 0, 0 );
   const Inertia  brickInertiaMatrixAboutBrickOrigin( 1, 1, 1, 0, 0, 0 );
   MassProperties  brickMassProperties( brickMass, brickCenterOfMassLocationFromBrickOrigin, brickInertiaMatrixAboutBrickOrigin );

   // To add a particle (or rigid body), must inform how it is connected
   const Transform outboardFrameTransformFromGround;   // Default constructor is the identity transform
   const Transform  inboardFrameTransformFromBrick; // Default constructor is the identity transform
   Mobilizer brickToGroundMobilizer = Mobilizer::Cartesian;

   // Add this to the matter subsystem (does not get directly added to the multibody system)
   allTheMolecules.addRigidBody( brickMassProperties, inboardFrameTransformFromBrick, GroundId, outboardFrameTransformFromGround, brickToGroundMobilizer );

   int  numberOfRigidBodies = allTheMolecules.getNBodies();
   return numberOfRigidBodies;
}

//-----------------------------------------------------------------------------
int  CalculateMassPropertiesOfPdbMolecule( MassProperties &pdbMassProperties, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb )
{
   // Open the .pdb file for reading
   FILE *pdbFile = FileOpenWithMessageIfCannotOpen( nameOfPdbFile, "r" );
   if( !pdbFile ) {successReadingPdb = false;  return 0;}

   // Default is that there are no errors in reading PDB file
   successReadingPdb = true;

   // Number of atoms included in this calculation
   int  numberOfAtomsInCalculation = 0;

   //  Mass of atoms included in this calculation
   Real totalMassOfAtomsInAMU = 0;

   //  Sum of mQ*r_Q_O center of mass components
   Vec3 centerOfMassTimesTotalMass( 0, 0, 0 );

   Inertia inertiaMatrixAboutLaboratoryOrigin = Inertia( 0, 0, 0, 0, 0, 0 );

   // Space for line in .pdb file
   char inputLine[2000];

   // Keep getting strings from the file until there are no more
   while( GetStringFromFile( inputLine, 2000, pdbFile ) )
   {
      // Get the chemical element for this line (if there is one)
      // Also, get this atom's position (from the designated columns from the pdb)
      Vec3  atomPositionFromLaboratoryOrigin;
      const ChemicalElement *chemElement = GetChemicalElementAndPositionFromPDBInputLine( inputLine, selectedPdbChain, includeHETAM, atomPositionFromLaboratoryOrigin, successReadingPdb );

      // It may be that this was supposed to be a chemical element, but the data is not in the correct fields
      if( successReadingPdb == false )  break;

      // It may be that this line of the .pdb does not contain ATOM or HETAM information
      if( !chemElement ) continue;

      // At this point, we have found a valid atom to include in the mass calculations
      ++numberOfAtomsInCalculation;

      // Get mass of the atom and add it to the molecule mass
      Real  moleculeMass = chemElement->getMass();
      totalMassOfAtomsInAMU += moleculeMass;

      // Calculate contribution to center of mass position, r_Q_O*m_Q
      Vec3 atomCMAdd = moleculeMass * atomPositionFromLaboratoryOrigin;
      centerOfMassTimesTotalMass += atomCMAdd;

      Inertia inertiaMatrixOfAtomAboutLaboratoryOrigin = Inertia( atomPositionFromLaboratoryOrigin, moleculeMass );

      inertiaMatrixAboutLaboratoryOrigin += inertiaMatrixOfAtomAboutLaboratoryOrigin;
   }

   Vec3 centerOfMassLocationFromLaboratoryOrigin = centerOfMassTimesTotalMass / totalMassOfAtomsInAMU;

   // The MassProperties class is a suitcase for mass, center of mass, and inertia
   pdbMassProperties.setMassProperties( totalMassOfAtomsInAMU, centerOfMassLocationFromLaboratoryOrigin, inertiaMatrixAboutLaboratoryOrigin );

   // Return number of atoms in .pdb file
   return numberOfAtomsInCalculation;
}


//-----------------------------------------------------------------------------
int  SetMobilizerStatesFromPdbFile( SimbodyMatterSubsystem &allTheMolecules, State &state, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb )
{
   // MIMIC THE FUNCTIONALITY IN: AddMoleculesFromPdbFileToSimbodyMatterSubsystem

   // Open the .pdb file for reading
   FILE *pdbFile = FileOpenWithMessageIfCannotOpen( nameOfPdbFile, "r" );
   if( !pdbFile ) {successReadingPdb = false;  return 0;}

   // Default is that there are no errors in reading PDB file
   successReadingPdb = true;

   // Number of atoms included in this calculation
   int  numberOfAtomsInCalculation = 0;

   // Space for line in .pdb file
   char inputLine[2000];

   // Keep getting strings from the file until there are no more
   while( GetStringFromFile( inputLine, 2000, pdbFile ) )
   {
      // Get the chemical element for this line (if there is one)
      // Also, get this atom's position (from the designated columns from the pdb)
      Vec3  atomPositionFromLaboratoryOrigin;
      const ChemicalElement *chemElement = GetChemicalElementAndPositionFromPDBInputLine( inputLine, selectedPdbChain, includeHETAM, atomPositionFromLaboratoryOrigin, successReadingPdb );

      // It may be that this line of the .pdb does not contain ATOM or HETAM information
      if( !chemElement ) continue;

      // The number of atoms in the calculation should never be more than all the molecules
      int totalNumberOfMolecules = allTheMolecules.getNBodies();
      if( numberOfAtomsInCalculation > totalNumberOfMolecules ) { successReadingPdb = false;  break; }

      // At this point, we have found a valid atom to include in the mass calculations
      ++numberOfAtomsInCalculation;

      // Assuming the bodies are sequentially numbered, the next line should create the proper Id
      BodyId moleculeId( numberOfAtomsInCalculation );

      // Set the initial values for the configuration variables (x,y,z)
      Real x = atomPositionFromLaboratoryOrigin[0];
      Real y = atomPositionFromLaboratoryOrigin[1];
      Real z = atomPositionFromLaboratoryOrigin[2];
      allTheMolecules.setMobilizerQ( state, moleculeId, 0,  x );
      allTheMolecules.setMobilizerQ( state, moleculeId, 1,  y );
      allTheMolecules.setMobilizerQ( state, moleculeId, 2,  z );

      // Set the initial values for the motion variables (vx, vy, vz)
      Real vx = 0,  vy = 0,  vz = 0;
      allTheMolecules.setMobilizerU( state, moleculeId, 0,  vx  );
      allTheMolecules.setMobilizerU( state, moleculeId, 1,  vy  );
      allTheMolecules.setMobilizerU( state, moleculeId, 2,  vz  );
   }

   // Close the pdb file before returning (setting it to NULL is good practice)
   fclose( pdbFile );
   pdbFile = NULL;

   // Was able to add all particles
   return numberOfAtomsInCalculation;
}


//-----------------------------------------------------------------------------
void  DecorateAllTheMoleculesWithSpheres( SimbodyMatterSubsystem &allTheMolecules, Vec6 &boundingBox, VTKReporter &viewer, int numberOfMolecules )
{
   // Number of atoms included in this calculation
   //int  numberOfMolecules = allTheMolecules.getNBodies()-1;

   // Decorate each molecule with a sphere
   for( int i=1;  i<numberOfMolecules;  i++ )
   {
      // For visualization purposes only, create a red sphere
      // Note: Uranium radius = Hydrogen radius = 1.2 Angstrom;  Carbon Atom = 1.7 Angstrom.
      Real  radiusOfMoleculeForViewer = 1.2;
      DecorativeSphere redSphere = DecorativeSphere( radiusOfMoleculeForViewer );
      redSphere.setColor(Red);     // Can also specify a Vec3 with rgb which scale from 0 to 1
      redSphere.setOpacity(0.0);   // 0.0 is solid and 1.0 is transparent

      // Decorate the particle with the red sphere at the particle's origin
      const Rotation  particleToRedSphereOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
      const Vec3      particleOriginToRedSphereOriginLocation( 0, 0, 0 );
      const Transform particleToRedSphereTransform( particleToRedSphereOrientation, particleOriginToRedSphereOriginLocation );

      // Add this geometry to the viewer
      BodyId moleculeId( i );
      viewer.addDecoration( moleculeId, particleToRedSphereTransform, redSphere );
   }

}
int  SetMobilizerStatesForBoundingBox( SimbodyMatterSubsystem &allTheMolecules, BodyId &brickId, State &state)
{
// Set the initial values for the configuration variables (x,y,z)
   Real x = 0;
   Real y = 0;
   Real z = 0;
   allTheMolecules.setMobilizerQ( state, brickId, 0,  x );
   allTheMolecules.setMobilizerQ( state, brickId, 1,  y );
   allTheMolecules.setMobilizerQ( state, brickId, 2,  z );

   // Set the initial values for the motion variables (vx, vy, vz)
   Real vx = 0,  vy = 0,  vz = 0;
   allTheMolecules.setMobilizerU( state, brickId, 0,  vx  );
   allTheMolecules.setMobilizerU( state, brickId, 1,  vy  );
   allTheMolecules.setMobilizerU( state, brickId, 2,  vz  );

   return allTheMolecules.getNBodies();
}

void  DecorateBoundingBoxWithBrick( BodyId &brickId, Real xWidth, Real yWidth, Real zWidth, VTKReporter &viewer, Vec3 boundingBoxCenter )
{   
   Vec3 brickHalflengths(xWidth/2, yWidth/2, zWidth/2);
   DecorativeBrick blueBrick = DecorativeBrick(  brickHalflengths );

   blueBrick.setColor(Blue);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   blueBrick.setOpacity(0.5);   // 0.0 is solid and 1.0 is transparent

   const Rotation  particleToBlueBrickOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3      particleOriginToBlueBrickOriginLocation( boundingBoxCenter );
   const Transform particleToBlueBrickTransform( particleToBlueBrickOrientation, particleOriginToBlueBrickOriginLocation );

   viewer.addDecoration( brickId, particleToBlueBrickTransform, blueBrick );

}

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// PDB ATOM record format from
// http://www.wwpdb.org/documentation/format23/sect9.html
// COLUMNS      DATA TYPE        FIELD      DEFINITION
// ------------------------------------------------------
//  1 -  6      Record name      "ATOM    "
//  7 - 11      Integer          serial     Atom serial number.
// 13 - 16      Atom             name       Atom name.
// 17           Character        altLoc     Alternate location indicator.
// 18 - 20      Residue name     resName    Residue name.
// 22           Character        chainID    Chain identifier.
// 23 - 26      Integer          resSeq     Residue sequence number.
// 27           AChar            iCode      Code for insertion of residues.
// 31 - 38      Real(8.3)        x          Orthogonal coordinates for X in Angstroms
// 39 - 46      Real(8.3)        y          Orthogonal coordinates for Y in Angstroms
// 47 - 54      Real(8.3)        z          Orthogonal coordinates for Z in Angstroms
// 55 - 60      Real(6.2)        occupancy  Occupancy.
// 61 - 66      Real(6.2)        tempFactor Temperature factor.
// 77 - 78      LString(2)       element    Element symbol, right-justified.
// 79 - 80      LString(2)       charge     Charge on the atom.
//-----------------------------------------------------------------------------
const ChemicalElement*  GetChemicalElementAndPositionFromPDBInputLine( const char *inputLine, char selectedPdbChain, bool includeHETAM, Vec3 &atomPositionFromLaboratoryOrigin, bool &successReadingPdb )
{
   // Default is that there are no errors in reading PDB file
   successReadingPdb = true;

   // Skip lines that are too short to have the element symbol
   if( strlen( inputLine) < 77 ) return NULL;

   // If this atom is associated with a chain, it is designated in column 22 (which is character 21)
   if( selectedPdbChain && selectedPdbChain != inputLine[21] )  return NULL;

   // The first six characters must be "ATOM" or "HETAM" for this line to designate an ATOM.
   bool isAtom   = strncmp( inputLine, "ATOM",   4 ) == 0;
   bool isHETATM = includeHETAM && strncmp( inputLine, "HETATM", 6 ) == 0;
   if( !isAtom && !isHETATM ) return NULL;

   // Get this atom's element symbol (the element symbol is right-justified and starts in column 77)
   const char *elementSymbolStart = inputLine + 76;
   while( isspace(elementSymbolStart[0]) ) elementSymbolStart++;
   const char *elementSymbolStop = inputLine + 78;
   int numberOfCharactersInSymbol = (int)(elementSymbolStop - elementSymbolStart);

   // Copy element symbol information
   char elementSymbol[3];
   strncpy( elementSymbol, elementSymbolStart, numberOfCharactersInSymbol );
   elementSymbol[ numberOfCharactersInSymbol] = '\0';

   // Get this atom's atomic mass in AMU.  If this method returns 0, bad parsing.
   const ChemicalElement *chemElement = ChemicalElement::getChemicalElement( elementSymbol );

   // Get this atom's position (from the designated columns from the pdb)
   atomPositionFromLaboratoryOrigin = GetAtomPositionFromPdbLine( inputLine, successReadingPdb );

   // Return the chemical element if the line could be fully understood
   return successReadingPdb ? chemElement : NULL;
}


//-----------------------------------------------------------------------------
Vec3  GetAtomPositionFromPdbLine( const char *inputLine, bool &successReadingPdb )
{
   // By convention, the position is measured in Angstroms
   double x, y, z;
   if( ConvertStringToDouble( inputLine + 31,  x,  0.0 ) == NULL ||
       ConvertStringToDouble( inputLine + 38,  y,  0.0 ) == NULL ||
       ConvertStringToDouble( inputLine + 46,  z,  0.0 ) == NULL ) successReadingPdb = false;
   return Vec3( x, y, z );
}


//-----------------------------------------------------------------------------
Vec6  CalculateBoundingBoxOfBodyOriginsUsingGroundXYZDirections( const SimbodyMatterSubsystem &allTheBodies, const State &state, int numberOfBodies )
{
   // Keep track of the minimum (most negative) and maximum (most positive) particle locations
   // The min/max values are initially assigned to the x, y, z values of the first particle.
   Real xMin=0, xMax=0;
   Real yMin=0, yMax=0;
   Real zMin=0, zMax=0;

   // Go through each of its bodies (skip 0 which is ground)
   for( int i=1;  i<numberOfBodies;  i++ )
   {
      // BodyId for current body
      BodyId currentBodyId(i);

      // Locate the molecule's origin from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors.
      const Vec3 bodyOriginLocation = allTheBodies.calcBodyOriginLocationInBody( state, currentBodyId, GroundId );
      Real xLocation = bodyOriginLocation[0];
      Real yLocation = bodyOriginLocation[1];
      Real zLocation = bodyOriginLocation[2];

      // Compare the min/max values (and possibly reassign) with the x, y, z, values of each particle.
      if( i==1 || xLocation < xMin ) xMin = xLocation;
      if( i==1 || yLocation < yMin ) yMin = yLocation;
      if( i==1 || zLocation < zMin ) zMin = zLocation;
      if( i==1 || xLocation > xMax ) xMax = xLocation;
      if( i==1 || yLocation > yMax ) yMax = yLocation;
      if( i==1 || zLocation > zMax ) zMax = zLocation;
   }

   // Return the bounding box as 6 numbers
   return Vec6( xMin, xMax, yMin, yMax, zMin, zMax );
}


//-----------------------------------------------------------------------------
const char*  ConvertStringToDouble( const char *s, double &returnValue, double defaultValue )
{
   // Default return value (in case the string is not a valid number)
   returnValue = defaultValue;

   // Check if  s  is a NULL string or  "abc"  or  "123huh"  or  "(&junk#"  or
   if( s != NULL )
   {
      // Use the standard math function strtod to parse the number
      char *pointerToCharacterAfterNumber = NULL;
      double x = strtod( s, &pointerToCharacterAfterNumber );

      // Ensure the number was not too large (overflow), such as 1.0E+999 or -1.0E-999
      if( errno==ERANGE  &&  x!=0.0 ) return NULL;

      // Ensure the character after the number is a space or '\0', not 'a' or 'z' or ...
      char characterAfterNumber = pointerToCharacterAfterNumber ? *pointerToCharacterAfterNumber : 'z';
      if( characterAfterNumber == '\0' || isspace( characterAfterNumber ) )
      {
         returnValue = x;
         return pointerToCharacterAfterNumber;
      }
   }
   return NULL;
}


//-----------------------------------------------------------------------------
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;
}


//-----------------------------------------------------------------------------
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      double vi = v(i); // or v[i];
      if( !WriteDoubleToFile( vi, precision, fptr ) ) return false;
   }
   return true;
}


//-----------------------------------------------------------------------------
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      // Row3 &vi = m[i];   // m[i] returns the row and m(i) returns the column
      // if( !WriteVec3ToFile( vi, precision, fptr ) ) return false;
      for( unsigned int j=0;  j<3;  j++ )
      {
         const Real mij = m[i][j];
         if( !WriteDoubleToFile( mij, precision, fptr ) ) return false;
      }
      if( i<=1 && !WriteStringToFile( "\n", fptr ) ) return false;
   }
   return true;
}