//-----------------------------------------------------------------------------
// File:     HumanMassProperties.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates human body segments' mass, center of mass, and inertia properties
// Author:   Paul Mitiguy - May 13, 2007
//-----------------------------------------------------------------------------
// 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"
#include "ScaledBodySegmentMassProperties.h"
using namespace SimTK;
using namespace std;
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void );
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != EOF; }
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  WriteScaledBodySegmentMassPropertiesToFile( bool subjectIsFemale, const char *segmentName, Real subjectTotalMassInKG, Real subjectTotalHeightInMM, Real subjectSegmentLengthInMM, FILE *resultsFile );


//-----------------------------------------------------------------------------
// 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 succeeded.  Press  Enter  to finish: " : "\n\n Program failed.  Press  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 )
{
   // Open a file for writing (return false if cannot open file)
   const char *outputFileName = "HumanMassPropertiesResults.txt";
   FILE *resultsFile = FileOpenWithMessageIfCannotOpen( outputFileName, "w" );
   if( !resultsFile ) return false;

   // Write headings to file
   WriteStringToFile( " Segment   Length (mm)  Mass (kg)    CM (mm)      kxx (mm)     kyy (mm)     kzz (mm)    Ixx (kg*mm^2) Iyy (kg*mm^2) Izz (kg*mm^2)\n ", resultsFile );
   // WriteStringToFile( " Segment   Length (mm)  Mass (kg)    CM (mm)     Ixx (kg*mm^2) Iyy (kg*mm^2) Izz (kg*mm^2)\n ", resultsFile );

   // Enter the subject-specific total mass and total height
   const Real subjectTotalMassInKG = 90.0;
   const Real subjectTotalHeightInMM = 0.0;
   const bool subjectIsFemale = false;

   // Array of segment Names and segment lengths
   // Note: A value of zero for a segment length causes the program to estimate the segment length using the
   //       standardBodySegmentLengthInMM * subjectTotalHeightInMM / standardBodyTotalHeightInMM.
   const char *segmentName[] =      { "Head", "Trunk",  "UpperArm",  "ForeArm",   "Hand",  "Thigh",   "Shank",  "Foot" };
   const Real segmentLengthInMM[] = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    279.4 };

   // Write out each of the resuls
   for( int i=0;  i<8;  i++ )
   {
      const char *segmentNamei = segmentName[i];
      const Real segmentLengthi = segmentLengthInMM[i];
      if( !WriteScaledBodySegmentMassPropertiesToFile( subjectIsFemale, segmentNamei, subjectTotalMassInKG, subjectTotalHeightInMM, segmentLengthi, resultsFile ) ) return false;
   }


   // Calculations performed successfully - close output file
   WriteStringToScreen( "\n\n Calculations were successfuly performed.\n Results are in the file: " );
   WriteStringToScreen( outputFileName );
   fclose( resultsFile );
   resultsFile = NULL;

   return true;
}


//-----------------------------------------------------------------------------
bool  WriteScaledBodySegmentMassPropertiesToFile( bool subjectIsFemale, const char *segmentName, Real subjectTotalMassInKG, Real subjectTotalHeightInMM, Real subjectSegmentLengthInMM, FILE *resultsFile )
{
   // If there is no resultsFile, return false.
   if( !resultsFile ) return false;

   // Get the standard body segment mass properties
   const StandardBodySegmentMassProperties*  standardSegment = StandardBodySegmentMassProperties::getStandardHumanBodySegmentMassProperties( subjectIsFemale, segmentName );

   if( !standardSegment ) return false;

   // Scale the properties with the given information
   const ScaledBodySegmentMassProperties scaledSegment( *standardSegment, subjectTotalMassInKG, subjectTotalHeightInMM, subjectSegmentLengthInMM );
   /* NOTE: Putting an asterisk before the first argument in the function call above dereferences the pointer 'standardSegment'.
   In other words, it returns the structure that the pointer is pointing to.  The constructor wants a reference as the type of this
   first argument, so you must either pass a reference or the structure itself.  Dereferencing the pointer allows you to pass
   the structure itself by reference to the function. */

   // Write the segment name to the file with padding
   WriteStringToFile( segmentName, resultsFile );
   int extraSpaces = (int)(8 - strlen(segmentName));
   for( int i=0;  i<extraSpaces;  i++ )
      WriteStringToFile( " ", resultsFile );

   // Write the segment length to the file
   int precision = 4;
   Real segmentLength = scaledSegment.getSegmentLengthInMM();
   WriteDoubleToFile( segmentLength, precision, resultsFile );

   // Write the segment mass to the file
   Real segmentMass = scaledSegment.getMassInKG();
   WriteDoubleToFile( segmentMass, precision, resultsFile );

   // Get the segment's distance from its origin anatomical marker to its mass center distance
   Real segmentMassCenterLength = scaledSegment.getMassCenterLengthInMM();
   WriteDoubleToFile( segmentMassCenterLength, precision, resultsFile );

   // Get       the segment x-radius of gyration about its mass center to the file
   // Calculate the segment x-moment of inertia  about its mass center to the file
   Real kxx = scaledSegment.getXRadiusOfGyrationInMM();
   Real Ixx = segmentMass*pow(kxx, 2);

   // Get       the segment y-radius of gyration about its mass center to the file
   // Calculate the segment y-moment of inertia  about its mass center to the file
   Real kyy = scaledSegment.getYRadiusOfGyrationInMM();
   Real Iyy = segmentMass*pow(kyy, 2);

   // Get       the segment z-radius of gyration about its mass center to the file
   // Calculate the segment z-moment of inertia  about its mass center to the file
   Real kzz = scaledSegment.getZRadiusOfGyrationInMM();
   Real Izz = segmentMass*pow(kzz, 2);

   // Write results to file
   WriteDoubleToFile( kxx, precision, resultsFile );
   WriteDoubleToFile( kyy, precision, resultsFile );
   WriteDoubleToFile( kzz, precision, resultsFile );
   WriteDoubleToFile( Ixx, precision, resultsFile );
   WriteDoubleToFile( Iyy, precision, resultsFile );
   WriteDoubleToFile( Izz, precision, resultsFile );

   // Write a newline to the file
   WriteStringToFile( "\n ", resultsFile );

   return true;
}


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