//-----------------------------------------------------------------------------
// File:     HelloCenterOfMass.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates the Center of Mass and Inertia Matrice about the CoM and the Origin
//-----------------------------------------------------------------------------
// 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;
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------

bool  WriteStringToFile(   const char outputString[], FILE *fptr );  
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );


//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] ){

   // Build the vectors to the particles
   const Vec3 r_Q1( -1,  0, 0);
   const Vec3 r_Q2(  0,  1, 0);
   const Vec3 r_Q3(  1,  0, 0);
   const Vec3 r_Q4(  0, -1, 0);

   // Masses
   const Real m_Q1 = 1;
   const Real m_Q2 = 2;
   const Real m_Q3 = 2;
   const Real m_Q4 = 2;

   //Total mass
   const Real TotalMass = m_Q1 + m_Q2 + m_Q3 + m_Q4;

   //The center of mass
   const Vec3 CenterOfMass = (m_Q1*r_Q1 + m_Q2*r_Q2 + m_Q3*r_Q3 + m_Q4*r_Q4) / TotalMass;

   //build the inertia matrix   
   Mat33 InertiaMatrixFromOrigin;
   Mat33 InertiaMatrixFromCenterOfMass;
   const Mat33 I(1, 0, 0, 0, 1, 0, 0, 0, 1);

   //inertia matrix from the origin
   InertiaMatrixFromOrigin =	m_Q1*(I*( dot( r_Q1, r_Q1 ))-r_Q1*~r_Q1 ) + 
								m_Q2*(I*( dot( r_Q2, r_Q2 ))-r_Q2*~r_Q2 ) +
								m_Q3*(I*( dot( r_Q3, r_Q3 ))-r_Q3*~r_Q3 ) +
								m_Q4*(I*( dot( r_Q4, r_Q4 ))-r_Q4*~r_Q4 );

  

   //inertia matrix from the center of mass
   InertiaMatrixFromCenterOfMass =	m_Q1*( I*( dot(( r_Q1-CenterOfMass ), ( r_Q1-CenterOfMass )))-( r_Q1-CenterOfMass )*~( r_Q1-CenterOfMass )) + 
									m_Q2*( I*( dot(( r_Q2-CenterOfMass ), ( r_Q2-CenterOfMass )))-( r_Q2-CenterOfMass )*~( r_Q2-CenterOfMass )) +
									m_Q3*( I*( dot(( r_Q3-CenterOfMass ), ( r_Q3-CenterOfMass )))-( r_Q3-CenterOfMass )*~( r_Q3-CenterOfMass )) +
									m_Q4*( I*( dot(( r_Q4-CenterOfMass ), ( r_Q4-CenterOfMass )))-( r_Q4-CenterOfMass )*~( r_Q4-CenterOfMass ));

   
   FILE *MassPropInfo = FileOpenWithMessageIfCannotOpen("HelloCenterOfMass.txt", "w");

  
   //Write the center of mass to the file
   WriteStringToFile("Center Of Mass =\n ", MassPropInfo);
   WriteVec3ToFile(CenterOfMass, 7, MassPropInfo);

   //Write the Inertia from the origin to the file
   WriteStringToFile("\nInertia from the origin =\n ", MassPropInfo);
   WriteMat33ToFile(InertiaMatrixFromOrigin, 7, MassPropInfo);

   //write the inertia from the origin to the file
   WriteStringToFile("\n\nInertia from the center of mass =\n", MassPropInfo);
   WriteMat33ToFile(InertiaMatrixFromCenterOfMass, 7, MassPropInfo);



}

//-------------------------------------------------------------------------------
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { 
   return fputs( outputString, fptr ) >=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  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr ){

   
   WriteStringToFile( "| ", fptr);
   
   for(int i= 0; i < 3; i++)
   {
		WriteDoubleToFile( v[i], precision, fptr);
		WriteStringToFile( "\t", fptr);
   }
   
   WriteStringToFile( "| \n ", fptr);
  
return 0;
}



//-----------------------------------------------------------------------------
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr ) {

  for(int i=0; i<3; i++)
  {
   Row3 R=m[i];
   WriteVec3ToFile(~R, precision, fptr);
  }
   
 return 0;    
}