//-----------------------------------------------------------------------------
// File:     Hello Vector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates Vector Operations for Two Sets of Vectors
// Author:   Daniel Jacobs 
//-----------------------------------------------------------------------------
// 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  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 );
Mat33 FormParticleInertiaMatrix(const double mass, const Vec3 positionFromCenter);
static double ConvertFromRadiansToDegrees ( double angleInRadians );
static double ConvertFromDegreesToRadians ( double angleInDegrees );
//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
   // Simulate the multibody system
   bool simulationSucceeded = DoRequiredTasks();

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

   // 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 simulationSucceeded == true ? 0 : 1;
}
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void )
{
   // Open a file to record the results (they are also displayed on screen)
   FILE *outputFile = FileOpenWithMessageIfCannotOpen( "HelloCenterOfMassResults.txt", "w" );

   // Give everything the same precision
   int precision = 3;
   
   // Define Vectors from Origin to Each Particle
   Vec3 r_Q1_o(-1.0,0.0,0.0);
   Vec3 r_Q2_o(0.0,1.0,0.0);
   Vec3 r_Q3_o(1.0,0.0,0.0);
   Vec3 r_Q4_o(0.0,-1.0,0.0);

   // Define the Particle Mass (in kg);
   Real mass1 = 1, mass2 = 2, mass3 = 2, mass4 = 2;
   Real totalMass = mass1 + mass2 + mass3 + mass4;
   // Find the Centroid and Center of Mass
   Vec3 positionCentroidFromO = 1.0/4.0*(r_Q1_o+r_Q2_o+r_Q3_o+r_Q4_o);
   Vec3 positionCMFromO = (1.0/totalMass)*(mass1*r_Q1_o+mass2*r_Q2_o+mass3*r_Q3_o+mass4*r_Q4_o);
   
   WriteStringToFile(   "Position of the Centroid of S about O       : \n",  outputFile );
   WriteVec3ToFile( positionCentroidFromO, precision, outputFile );
   WriteStringToFile(   "\n",  outputFile );

   WriteStringToFile(   "Position of the Centroid of S about O       : \n",  stdout );
   WriteVec3ToFile( positionCentroidFromO, precision, stdout );
   WriteStringToFile(   "\n",  stdout );

   WriteStringToFile(   "Position of the Center of Mass of S about O : \n",  outputFile );
   WriteVec3ToFile( positionCMFromO, precision, outputFile );
   WriteStringToFile(   "\n",  outputFile );

   WriteStringToFile(   "Position of the Center of Mass of S about O : \n",  stdout );
   WriteVec3ToFile( positionCMFromO, precision, stdout );
   WriteStringToFile(   "\n",  stdout );

   // System Inertia is the sum of the particles
   Mat33 InertiaSystemAboutO(0,0,0,0,0,0,0,0,0);
   InertiaSystemAboutO += FormParticleInertiaMatrix(mass1, r_Q1_o);
   InertiaSystemAboutO += FormParticleInertiaMatrix(mass2, r_Q2_o);
   InertiaSystemAboutO += FormParticleInertiaMatrix(mass3, r_Q3_o);
   InertiaSystemAboutO += FormParticleInertiaMatrix(mass4, r_Q4_o);
   
   WriteStringToFile(   "Inertia of System S about O : \n",  outputFile );
   WriteMat33ToFile( InertiaSystemAboutO, precision, outputFile );
   WriteStringToFile(   "\n",  outputFile );

   WriteStringToFile(   "Inertia of System S about O : \n",  stdout );
   WriteMat33ToFile( InertiaSystemAboutO, precision, stdout );
   WriteStringToFile(   "\n",  stdout );

   //I_S_O = I_S_Cm + I_S_O/Cm
   //I_S_Cm = I_S_O -I_S_O/Cm
   //Form Additional Movement Torm
   Mat33 InertiaSystemFromCMtoO = FormParticleInertiaMatrix(totalMass,positionCMFromO);
   Mat33 InertialSystemAboutCM = InertiaSystemAboutO - InertiaSystemFromCMtoO;
   WriteStringToFile(   "Inertia of System S about CM : \n",  outputFile );
   WriteMat33ToFile( InertialSystemAboutCM, precision, outputFile );
   WriteStringToFile(   "\n",  outputFile );

   WriteStringToFile(   "Inertia of System S about CM : \n",  stdout );
   WriteMat33ToFile( InertialSystemAboutCM, precision, stdout );
   WriteStringToFile(   "\n",  stdout );


  


   return outputFile != NULL ;
}
//-----------------------------------------------------------------------------
Mat33 FormParticleInertiaMatrix(const double mass, const Vec3 positionFromCenter)
{
   Mat33 outputInertiaMatrix;

   //Form Unit Dyadic
   const Mat33 unitDyadic(1,0,0,0,1,0,0,0,1);
   // Inner Product
   Real innerProduct = dot(positionFromCenter,positionFromCenter);
   // OuterProduct
   Mat33 outerProduct = positionFromCenter*positionFromCenter.transpose();
   // Inertia Matrix
   return ( mass*( (unitDyadic*innerProduct)-outerProduct) );


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

//-----------------------------------------------------------------------------
static double ConvertFromRadiansToDegrees ( double angleInRadians )
{
   return angleInRadians * 180/3.1415926535897932384626433832795028841971;
}
//-----------------------------------------------------------------------------
static double ConvertFromDegreesToRadians ( double angleInDegrees )
{
 return angleInDegrees * 3.1415926535897932384626433832795028841971/180;
}
//-----------------------------------------------------------------------------