//-----------------------------------------------------------------------------
// File:     MassPropOfOxygenMoelcule.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calcualtes the oxygen molecule's mass, distance between atoms and center of \
//           molecule, and Inertia dyadic and center of mass of molecule.
//-----------------------------------------------------------------------------
// 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"
using namespace SimTK;
using namespace std;
# define signDigits 9
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
double  ConvertFromRadiansToDegrees( double angleInRadians )  { return angleInRadians * 57.295779513082320876798154814105170332405472466564321549160243861; }
double  ConvertFromDegreesToRadians( double angleInDegrees )  { return angleInDegrees * 0.017453292519943295769236907684886127134428718885417254560971914402; }
bool    WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != 0; }
bool    WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
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 );
bool  WriteDoubleMatrixElementsToFile( const double *array, unsigned int numberOfRows, unsigned int numberOfCols, int precision, FILE *fptr );
bool  WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr );

//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
   //Function opens file for writing results to and returns error is unable to open 
   FILE *pFile = FileOpenWithMessageIfCannotOpen("MassPropertiesOfO2Results.txt","w");
	if (!pFile) return 1;


   //Defining and writing the vector
   double LengthOfBond = 121.0/100.0;

   // Caculating the centroid and mass center for a set of molecules in System S
   // Defining and writing the vectors that relate the 4 molecules (Q1,Q2,Q3,Q4)
   // to point 0, orign of frame N
   const Vec3 r_QR_O(LengthOfBond/2,0.0,0.0); 
   //WriteStringToFile("The following vector is the postion vector of the centroid of system S from pt O\n",pFile);
   WriteVec3ToFile(r_QR_O,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);


   const Vec3 r_QL_O(-LengthOfBond/2,0.0,0.0);
   WriteVec3ToFile(r_QL_O,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);


   // Defining the mass of each Molecule in (kg)
   double massOfQR=15.9994;
   double massOfQL=15.9994;


   // Creating scalar values that will: 
   // a) represent the sum of all the mass in system S
   // b) represent the number of molecules in the system s
   double mS = massOfQR + massOfQL;
   double nS = 2.0;


   //Next semt of commands will verify that the centroid and center of mass are indeed locate at the
   //same pt.

   // Defining the position vector that relates pt Qi to pt 0 that is scaled by each 
   // molecules mass.  These vectors will be used later to calcualte the center of mass for 
   // system S.

   const Vec3 mRr_QR_O = massOfQR * r_QR_O;
   const Vec3 mLr_QL_O = massOfQL * r_QL_O;
  
   // Performing necessary alegebraic calculations for calculating mass center and 
   // centroid of system S
   
   // Defining the position vector that is the summation of all the position
   // vectors of the 4 molecules (Q1,Q2,Q3,Q4)about pt 0, orign of frame N.
   const Vec3 r_Qi_O = r_QR_O + r_QL_O;
   
   //Defining position vector that is the summation of the all the scaled position
   // vectors (by their respective mass) that relate the molecules Qi to pt O.
   const Vec3 mIr_Qi_O = mRr_QR_O + mLr_QL_O;

   // Calculating the position vector of the 
   // a)centroid of S about O
   // b)center of mass of of S about 0. 
   const Vec3 r_Scentroid_O = (1/nS)*(r_Qi_O);
   const Vec3 r_Scm_O = -(1/mS)*mIr_Qi_O;
   
   // Writing vectors to file
   WriteStringToFile("The following vector is the postion vector of the centroid of system S from pt O\n",pFile);
   WriteVec3ToFile(r_Scentroid_O,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);
   WriteStringToFile("The following vector is the postion vector of the center of mass of system S from pt O\n",pFile);
   WriteVec3ToFile(r_Scm_O,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);




   //Calculating the Inertia dyadic for each particle
    Inertia I_QRONbxyz(r_QR_O,massOfQR);
    Inertia I_QLONbxyz(r_QL_O,massOfQL);


    Inertia IQi_O = I_QRONbxyz + I_QLONbxyz;
    Mat33 ISObxyz = IQi_O.toMat33();

    WriteStringToFile("The following 3 by 3 matrix is the Inertia dyadic of the system S about pt 0:\n",pFile);
    WriteMat33ToFile(ISObxyz,signDigits,pFile);
    WriteStringToFile("\n",pFile);
 

   // Keep the screen displayed until the user presses the Enter key
   WriteStringToScreen( "\n\n Press  Enter  to terminate the program: " );
   int key = getchar();
   
   // A normal program exit returns 0 (other return values usually signal an error)
   return 0;
}



//-----------------------------------------------------------------------------
bool    WriteVec3ToFile( const Vec3 &v, 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 = 15;
   int fieldWidth = precision + 8;


   //Extracing the nx, ny, nz component from the Vec3 for printing
   Real AnyVectorNx=v[0];
   Real AnyVectorNy=v[1];
   Real AnyVectorNz=v[2];

   for (unsigned int i=0; i<3; i++)
   {
      // Create the format specifier and print the number
      char format[20];
      sprintf( format, " %%- %d.%dE", fieldWidth, precision );
   
      //Returns a error message if unable to write double
      fprintf( fptr, format, v[i]);
   }
return 0;
}


//__________________________________________________________________________________________
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    WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   const unsigned int numberOfRows = 3;
   const unsigned int numberOfCols = 3;
   const unsigned int numberOfElements = numberOfRows * numberOfCols;
   double M[ numberOfElements ];
   unsigned int i=0;
   for(unsigned int x=0; x<3; x++)
   {
      for (unsigned int y=0; y<3; y++)
      {
         M[i] = m[x][y];
         i=i+1;
      }
   }
   WriteDoubleMatrixElementsToFile(M,3,3,signDigits,fptr);
      return 0;
}


//-----------------------------------------------------------------------------
bool  WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<numberOfCols;  i++ )
   {
      if( !WriteDoubleToFile( array[i], precision, fptr ) ) return false;
   }
   return true;
}


//-----------------------------------------------------------------------------
bool  WriteDoubleMatrixElementsToFile( const double *array, unsigned int numberOfRows, unsigned int numberOfCols, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<numberOfRows;  i++ )
   {
      const double *rowArray = &array[ i * numberOfCols ];
      if( !WriteDoubleRowElementsToFile( rowArray, numberOfCols, precision, fptr ) ) return false;
      WriteStringToFile( "\n", fptr );
   }
   return true;
}


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