//-----------------------------------------------------------------------------
// 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 = "HumanMassPropertiesOfResultsScaledMandy.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 = 120*(1/2.20462262);
   const Real subjectTotalHeightInMM = 5.5*(1/0.00328399);
   const bool subjectIsFemale = true;

   // 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[] = {  230.0,  500.0,    355.0,      228.0,       184.0,    430.0,    410.0,    245.0 };

   // 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);
   // Look through the list for this symbol
   if( !standardSegment ) return false;

   //  This is a suitcase for the subject specific Scaled Mass Properties

   const ScaledBodySegmentMassProperties scaledSegment( *standardSegment,subjectTotalMassInKG, subjectTotalHeightInMM, subjectSegmentLengthInMM );

   // Scale the properties with the given information:
   // Now with the get commands, retrieve the scaled properties 


   // 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 = kxx*kxx*segmentMass;

   // 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 = kyy*kyy*segmentMass;

   // 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 = kzz*kzz*segmentMass;

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