//-----------------------------------------------------------------------------
// File:     HelloCenterOfMass.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates the Inertia dyadic and center of mass of molecule Q about 
//           O in basis vectors bx,y,z   
//-----------------------------------------------------------------------------
// 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 4
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void );
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[] )
{
   // 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 failed.  Press  Enter  to finish: " : "\n\nProgram succeeded.\n\nPlease see 'HelloCenterOfMassResults.txt' file for calculated results. \n\nPress  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 )
{
   //Function opens file for writing results to and returns error is unable to open 
   FILE *pFile = FileOpenWithMessageIfCannotOpen("HelloCenterOfMassResults.txt","w");
	if (!pFile) return 1;

   // Lab 3.2 Problems 
   WriteStringToFile("Lab 3.2 Calculating a simple inertia dyadic and inertia matrix:\n\n",pFile);


   //Defining and writing the vector from pt O(orign of frame N) to molecule Q  (r_QO)to the file
   const Vec3 r_QO(1E-10,2E-10,0);
   const char stri[200] = "The following 3 numbers are the nx,ny,nz components of the vector r_QO:";
   WriteStringToFile((stri),pFile);
   WriteVec3ToFile(r_QO,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the vector r_QO dot with vector r_QO to the file
   double r_QOdot = dot(r_QO,r_QO);   
   
   //Calculating the outer product of vectors r_QO and r_QO
   Mat33 r_QOouterBxyz = outer(r_QO,r_QO);
   WriteStringToFile("The following 3 by 3 matrix is a result of outer product of vectors r_QO and r_QO:\n",pFile);
   WriteMat33ToFile(r_QOouterBxyz,signDigits,pFile);
  
   // Defining the unit dyadic expressed in Bxyz to file
   Mat33 unitDyBxyz (1,0,0,0,1,0,0,0,1); 
   WriteStringToFile("\n\nThe following 3 by 3 matrix is the unit dyadic vector in Bxyz:\n",pFile);
   WriteMat33ToFile(unitDyBxyz,signDigits,pFile);
   
   // Defining variable  to store Avogadro's Number
   double AvogadrosNumber=6.0221415E23;
   
   // Caculating mass of molecule
   double massOfQ=12/AvogadrosNumber;
   
   // Calculating Q's inertia matrix about pt O in bxyz frame
   Mat33 I_QOnxyz = massOfQ*((unitDyBxyz*r_QOdot)-r_QOouterBxyz);
   WriteStringToFile("\n\nThe Inertia Dyadic for Q about pt O in Bxyz:\n",pFile);
   WriteMat33ToFile(I_QOnxyz,signDigits,pFile);
   WriteStringToFile("\n\n",pFile);

   //Homework 3.3
   WriteStringToFile("Lab 3.3 Centroid and Center of mass of a set of molecules:\n\n",pFile);
   // 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_Q1_O(-1.0,0.0,0.0); 
   const Vec3 r_Q2_O(0.0,1.0,0.0);
   const Vec3 r_Q3_O(1.0,0.0,0.0);
   const Vec3 r_Q4_O(0.0,-1.0,0.0);

   // Defining the mass of each Molecule in (kg)
   double massOfQ1 = 1.0;
   double massOfQ2 = 2.0;
   double massOfQ3 = 2.0;
   double massOfQ4 = 2.0;

   // 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 = massOfQ1 + massOfQ2 + massOfQ3 + massOfQ4;
   double nS = 4.0;


   // 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 m1r_Q1_O = massOfQ1 * r_Q1_O;
   const Vec3 m2r_Q2_O = massOfQ2 * r_Q2_O;
   const Vec3 m3r_Q3_O = massOfQ3 * r_Q3_O;
   const Vec3 m4r_Q4_O = massOfQ4 * r_Q4_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_Q1_O + r_Q2_O + r_Q3_O + r_Q4_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 = m1r_Q1_O + m2r_Q2_O + m3r_Q3_O + m4r_Q4_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);
   
   // Caculating the Inertia Dyadic of the system S about pt O
   //Definining the dot product of each position vector of each molecule (Qi) from pt 0 with itself
   double r_Q1_Odot = dot(r_Q1_O,r_Q1_O);
   double r_Q2_Odot = dot(r_Q2_O,r_Q2_O);
   double r_Q3_Odot = dot(r_Q3_O,r_Q3_O);
   double r_Q4_Odot = dot(r_Q4_O,r_Q4_O);
   
   
   //Caculating the outer product of each molecules (Qi) position vector with itself
   Mat33 r_Q1outer = outer(r_Q1_O,r_Q1_O);
   Mat33 r_Q2outer = outer(r_Q2_O,r_Q2_O);
   Mat33 r_Q3outer = outer(r_Q3_O,r_Q3_O);
   Mat33 r_Q4outer = outer(r_Q4_O,r_Q4_O);
   
   
   // Defining the unit Dyadic
   Mat33 unitDyNxyz (1,0,0,0,1,0,0,0,1); 

   // Caculating the inertia matrix for each pt Q1-Q4 about pt 0
   // Q1
   Mat33 I_Q1Onxyz = massOfQ1*((unitDyNxyz*r_Q1_Odot)-r_Q1outer);
 
   // Q2
   Mat33 I_Q2Onxyz = massOfQ2*((unitDyNxyz*r_Q2_Odot)-r_Q2outer);
   
   // Q3
   Mat33 I_Q3Onxyz = massOfQ3*((unitDyNxyz*r_Q3_Odot)-r_Q3outer);

   // Q4
   Mat33 I_Q4Onxyz = massOfQ4*((unitDyNxyz*r_Q4_Odot)-r_Q4outer);
   
   // For a system of particles the Inertia of the i particles can be summed to yeild the
   // inertia of the system about pt 0

   Mat33 I_SOnxyz = I_Q1Onxyz + I_Q2Onxyz +I_Q3Onxyz + I_Q4Onxyz;
   WriteStringToFile("The following 3 by 3 matrix is the Inertia dyadic of the system S about pt 0:\n",pFile);
   WriteMat33ToFile(I_SOnxyz,signDigits,pFile);
   WriteStringToFile("\n",pFile);

   // Caculating the inertia matrix of the system about the center of mass of the system
   // Adding the vector from pt O to the center of mass to each molecule's position vector;
   const Vec3 r_Q1_CM = r_Q1_O + r_Scm_O;
   const Vec3 r_Q2_CM = r_Q2_O + r_Scm_O;
   const Vec3 r_Q3_CM = r_Q3_O + r_Scm_O;
   const Vec3 r_Q4_CM = r_Q4_O + r_Scm_O;
   
   
   //Definining the dot product of each position vector of each molecule (Qi) from pt CM with itself
   double r_Q1_CMdot = dot(r_Q1_CM,r_Q1_CM);
   double r_Q2_CMdot = dot(r_Q2_CM,r_Q2_CM);
   double r_Q3_CMdot = dot(r_Q3_CM,r_Q3_CM);
   double r_Q4_CMdot = dot(r_Q4_CM,r_Q4_CM);
   
   
   //Caculating the outer product of each molecules (Qi) position vector with itself
   Mat33 r_Q1CMouter = outer(r_Q1_CM,r_Q1_CM);
   Mat33 r_Q2CMouter = outer(r_Q2_CM,r_Q2_CM);
   Mat33 r_Q3CMouter = outer(r_Q3_CM,r_Q3_CM);
   Mat33 r_Q4CMouter = outer(r_Q4_CM,r_Q4_CM);
   

   // Caculating the inertia matrix for each pt Q1-Q4 about pt 0
   // Q1
   Mat33 I_Q1CMnxyz = massOfQ1*((unitDyNxyz*r_Q1_CMdot)-r_Q1CMouter);
 
   // Q2
   Mat33 I_Q2CMnxyz = massOfQ2*((unitDyNxyz*r_Q2_CMdot)-r_Q2CMouter);
   
   // Q3
   Mat33 I_Q3CMnxyz = massOfQ3*((unitDyNxyz*r_Q3_CMdot)-r_Q3CMouter);

   // Q4
   Mat33 I_Q4CMnxyz = massOfQ4*((unitDyNxyz*r_Q4_CMdot)-r_Q4CMouter);
   
   // For a system of particles the Inertia of the i particles can be summed to yeild the
   // inertia of the system about pt 0

   Mat33 I_SCMnxyz = I_Q1CMnxyz + I_Q2CMnxyz +I_Q3CMnxyz + I_Q4CMnxyz;
   WriteStringToFile("The following 3 by 3 matrix is the Inertia dyadic of the system S about the system's center of mass:\n",pFile);
   WriteMat33ToFile(I_SCMnxyz,signDigits,pFile);
   WriteStringToFile("\n",pFile); 
 
    // Using built in function to check math   
    Inertia I_Q1ONnxyz(r_Q1_O,massOfQ1);
    Inertia I_Q2ONnxyz(r_Q2_O,massOfQ2);
    Inertia I_Q3ONnxyz(r_Q3_O,massOfQ3);
    Inertia I_Q4ONnxyz(r_Q4_O,massOfQ4);

    Inertia Iso = I_Q1ONnxyz + I_Q2ONnxyz + I_Q3ONnxyz + I_Q4ONnxyz;
    Mat33 ISOnxyz = Iso.toMat33();

    WriteStringToFile("Using built in functions to check Math:\n",pFile);
    WriteStringToFile("The following 3 by 3 matrix is the Inertia dyadic of the system S about pt 0 calculated by built in functions:\n",pFile);
    WriteMat33ToFile(ISOnxyz,signDigits,pFile);
    WriteStringToFile("\n",pFile);

    //To calcuate the inertia dyadic of the system about the system's center of mass
    // I will shift the Inertia dyadic of the system taken about pt O to the center of mass
    //  using the built in Simbody function shiftFromMassCenter
    Inertia ISCM = Iso.shiftToMassCenter(r_Scm_O,mS);
    Mat33 ISCMnxyz = ISCM.toMat33();
    WriteStringToFile("The following 3 by 3 matrix is the Inertia dyadic of the system S about the system's CM calculate by built in functions:\n",pFile);
    WriteMat33ToFile(ISCMnxyz,signDigits,pFile);
    WriteStringToFile("\n",pFile);

   // 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 )
{
   // Initializes variables that will establish the size of the matrix
   // a) Number of rows
   // b) Number of columns
   // c) Number of total elements
   const unsigned int numberOfRows = 3;
   const unsigned int numberOfCols = 3;
   const unsigned int numberOfElements = numberOfRows * numberOfCols;
   
   // double array used for storing the 9 scalar values
   double M[ numberOfElements ];
   
   // for loop that will cycle through and extract the nine numbers
   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;
      }
   }
   //Calls function that writes a double value to a specified file
   WriteDoubleMatrixElementsToFile(M,3,3,signDigits,fptr);
      return 0;
}

//-----------------------------------------------------------------------------
bool  WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr )
{
   
   // for loop that will cycle through and write elements of row elements of a matrix to a file
   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 loop that will call the function that will write rows of a matrix to a file x times (x is the number of columns in a matrix)
   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;
}


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