//-----------------------------------------------------------------------------
// File:     HelloMassCenter.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Performs several operations on two specified vectors
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// 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 WriteVec3ToFile(  const Vec3 &v, int precision, FILE *fptr  );
bool WriteMat33ToFile(  const Mat33 &m, int precision, FILE *fptr );
bool   WriteDoubleToFileWithSpaceAndSpecifiedPrecision( double x, int precision, FILE *fptr);
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute);

//-----------------------------------------------------------------------------
int  main( void )
{
   // Write  " Hello vector!"  to the screen and then put a newline
   printf( " Hello math!\n" );

   // problem constants
   double x = 1e-10;
   double y = 2e-10;
   double z = 0;
   double avogadros = 6.0221415e23;
   double m = 12/avogadros;

   Vec3 rQO( x, y, z );
   Inertia I_Q_O=Inertia(rQO,m);
   Mat33 Mat_Q_O = I_Q_O.toMat33();

   // Write results to file HelloVectorResults.txt
   FILE *fptr1 = FileOpenWithMessageIfCannotOpen( "HelloMassCenterResults.txt", "w+" );
   fprintf( fptr1, "3.2 A simple inertia dyadic for 1 particle\n");
   WriteMat33ToFile( Mat_Q_O, 5, fptr1);

   double mQ1 = 1;
   double mQ2 = 2;
   double mQ3 = 2;
   double mQ4 = 2;
   double mSys = mQ1 + mQ2 + mQ3 + mQ4;

   Vec3 r_Q1_O( -1, 0, 0 );
   Vec3 r_Q2_O( 0, 1, 0 );
   Vec3 r_Q3_O( 1, 0, 0 );
   Vec3 r_Q4_O( 0, -1, 0 );

   Inertia I_Q1_O=Inertia(r_Q1_O,mQ1);
   Inertia I_Q2_O=Inertia(r_Q2_O,mQ2);
   Inertia I_Q3_O=Inertia(r_Q3_O,mQ3);
   Inertia I_Q4_O=Inertia(r_Q4_O,mQ4);

   Vec3 r_Scm_O = ( mQ1*r_Q1_O + mQ2*r_Q2_O + mQ3*r_Q3_O + mQ4*r_Q4_O )/mSys;
   Vec3 r_Q1_Scm = r_Q1_O - r_Scm_O ; 
   Vec3 r_Q2_Scm = r_Q2_O - r_Scm_O ; 
   Vec3 r_Q3_Scm = r_Q3_O - r_Scm_O ; 
   Vec3 r_Q4_Scm = r_Q4_O - r_Scm_O ; 

   Inertia I_Q1_Scm=Inertia(r_Q1_Scm,mQ1);
   Inertia I_Q2_Scm=Inertia(r_Q2_Scm,mQ2);
   Inertia I_Q3_Scm=Inertia(r_Q3_Scm,mQ3);
   Inertia I_Q4_Scm=Inertia(r_Q4_Scm,mQ4);

   Inertia I_sys_O = I_Q1_O + I_Q2_O + I_Q3_O + I_Q4_O;

   Inertia I_sys_Scm = I_sys_O.shiftToMassCenter(r_Scm_O, mSys);

   // Convert inertia results to Mat33 format
   Mat33 Mat_sys_Scm = I_sys_Scm.toMat33();
   Mat33 Mat_sys_O = I_sys_O.toMat33();

   fprintf( fptr1, "3.4 inertia dyadic for 4 particle around the origin\n");
   WriteMat33ToFile( Mat_sys_O, 5, fptr1);
   fprintf( fptr1, "3.4 inertia dyadic for 4 particle around the center of mass\n");
   WriteMat33ToFile( Mat_sys_Scm, 6, fptr1);


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

   return 0;

}


//-----------------------------------------------------------------------------
bool WriteVec3ToFile(  const Vec3 &v, int precision, FILE *fptr  )
{
   if( precision < 0 || precision > 17 ) precision = 5;

	return fprintf( fptr, " %g n_x + %g n_y + %g n_z \n\n",  v[0], v[1], v[2] ) > 0;
}

//-----------------------------------------------------------------------------
bool WriteMat33RowToFile(  const Mat33 &m, int precision, FILE *fptr, int row )
{
   if( precision < 0 || precision > 17 ) precision = 5;
   int fieldWidth = precision + 8;

	// Create the format specifier and print the number
	char format[40];
   sprintf( format, "%%- %d.%dg", fieldWidth, precision);
   
   fprintf( fptr, "[ ");
   fprintf( fptr, format,  m[row][0]);
   fprintf( fptr, format,  m[row][1]);
   fprintf( fptr, format, m[row][2]);
   fprintf( fptr, " ] \n");
   
   return true;

}

//-----------------------------------------------------------------------------
bool WriteMat33ToFile(  const Mat33 &m, int precision, FILE *fptr )
{ 
   WriteMat33RowToFile( m, precision, fptr, 0 );
   WriteMat33RowToFile( m, precision, fptr, 1 );
   WriteMat33RowToFile( m, precision, fptr, 2 );
   fprintf( fptr, "\n");
   
   return true;
}
//-----------------------------------------------------------------------------
bool   WriteDoubleToFileWithSpaceAndSpecifiedPrecision( 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 + 4;

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


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

FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute)
{
	FILE *stream;
	if( (stream = fopen( filename, attribute )) == NULL)
	{
		printf("cannot open file, press enter to exit program.\n");
	}
	else
	{
		printf("results file opened, press enter to continue.\n");
	}
	int key2 = getchar();
	return stream;
}
//-----------------------------------------------------------------------------