//-----------------------------------------------------------------------------
// File:     StandingLegForVisualization.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Simulates a 2D human composed of 4 rigid bodies (foot, shank, thigh, HAT)
//-----------------------------------------------------------------------------
// 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  SimulateNewtons2DHuman( void );
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != 0; }
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 );


//-----------------------------------------------------------------------------
// 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 = SimulateNewtons2DHuman();

   }
   // 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  SimulateNewtons2DHuman( void )
{
   // Declare a multibody system
   // Each multibody system contains at most one SimbodyMatterSubsytem (which inherits from MatterSubsystem - the ma in F=ma)
   // Each multibody system can contain 0, 1, 2, ... force subsystems (the F in F=ma)
   MultibodySystem  mbs;
   SimbodyMatterSubsystem  human;      // Create a single matter sub-system (for the apple)
   mbs.setMatterSubsystem( human );    // Add the matter sub-system to the system.

   // 0. The ground's right-handed, orthogonal x,y,z unit vectors are directed with x horizontally right and y vertically upward.
   // 1. Create a gravity vector that is straight down (in the ground's frame)
   // 2. Create a uniform gravity sub-system
   // 3. Add the gravity sub-system to the multibody system
   Vec3 gravityVector( 0, 0, 0 );
   UniformGravitySubsystem gravity( gravityVector );
   mbs.addForceSubsystem( gravity );

   // Create the mass, center of mass, and inertia properties for the book
   const Real subjectTotalMassInKG = 56.7;
   const Real subjectTotalHeightInMM = 1651;
   const bool subjectIsFemale = true;

   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,    0.0 };

   Real segmentScaledLengthMM[]   = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };
   Real segmentMassInKg[]         = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };
   Real segmentMassCenterLength[] = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };
   Real segmentIxx[]              = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };
   Real segmentIyy[]              = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };
   Real segmentIzz[]              = {    0.0,     0.0,         0.0,        0.0,      0.0,      0.0,       0.0,    0.0 };


   // Write out each of the resuls
   for( int i=0;  i<8;  i++ )
   {
      const char *segmentNamei = segmentName[i];
      const Real segmentLengthi = segmentLengthInMM[i];

      // Get the standard body segment mass properties
      const StandardBodySegmentMassProperties*  standardSegment = StandardBodySegmentMassProperties::getStandardHumanBodySegmentMassProperties( subjectIsFemale, segmentNamei );
      if( !standardSegment ) return false;

      // Scale the properties with the given information
      const ScaledBodySegmentMassProperties scaledSegment( *standardSegment, subjectTotalMassInKG, subjectTotalHeightInMM, segmentLengthi );

      // Store segment length and segment mass
      segmentScaledLengthMM[i] = scaledSegment.getSegmentLengthInMM();

      segmentMassInKg[i] = scaledSegment.getMassInKG();

      // Get the segment's distance from its origin anatomical marker to its mass center distance
      segmentMassCenterLength[i] = scaledSegment.getMassCenterLengthInMM();

      // 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();
      segmentIxx[i] = segmentMassInKg[i] * 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();
      segmentIyy[i] = segmentMassInKg[i] * 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();
      segmentIzz[i] = segmentMassInKg[i] * pow(kzz,2);

   }

   // Lump head, arms and torso together.  Approximate COM as torso COM
   Real HATMass = segmentMassInKg[0]+segmentMassInKg[1]+segmentMassInKg[2]+segmentMassInKg[3];
   Real adjustedHATIxx = segmentIxx[1]/segmentMassInKg[1]*HATMass;
   Real adjustedHATIyy = segmentIyy[1]/segmentMassInKg[1]*HATMass;
   Real adjustedHATIzz = segmentIzz[1]/segmentMassInKg[1]*HATMass;


   // The location of the segments' center of mass is a vector from the book's
   // origin expressed in the "x, y, z"' unit vectors fixed in the book's frame.
   // Example: The vector(0,0,0) locates the book's center of mass at the book's origin.
   // Example: The vector(1,0,0) locates the book's center of mass 1 unit in the "x" direction from the book's origin.
   const Vec3  footCenterOfMassLocation( 0, 0, 0);
   const Vec3  shankCenterOfMassLocation( 0, 0, 0);
   const Vec3  thighCenterOfMassLocation( 0, 0, 0);
   const Vec3  HATCenterOfMassLocation( 0, 0, 0);

   // Create the segments' inertia matrix about its origin for the "x, y, z" unit vectors fixed in the book's frame.
   // Note: The 3x3 inertia matrix is symmetric - so only 6 elements need to be defined.
   // Ixx, Iyy, Izz  are moments  of inertia (    diagonal terms in the matrix)
   // Ixy, Ixz, Iyz  are products of inertia (off-diagonal terms in the matrix)
   const Inertia  footInertiaMatrix( segmentIxx[7], segmentIyy[7], segmentIzz[7]  );
   const Inertia  shankInertiaMatrix( segmentIxx[6], segmentIyy[6], segmentIzz[6] );
   const Inertia  thighInertiaMatrix( segmentIxx[5], segmentIyy[5], segmentIzz[5] );
   const Inertia  HATInertiaMatrix( adjustedHATIxx, adjustedHATIyy, adjustedHATIzz );

   // The MassProperties class holds the mass, center of mass, and inertia properties of a rigid body.
   // Although the next line creates an instance of the MassProperties class for each segment,
   // it does not get associated with the segment until the  addRigidBody  method.
   MassProperties  footMassProperties(  segmentMassInKg[7], footCenterOfMassLocation,  footInertiaMatrix  );
   MassProperties  shankMassProperties( segmentMassInKg[6], shankCenterOfMassLocation, shankInertiaMatrix );
   MassProperties  thighMassProperties( segmentMassInKg[5], thighCenterOfMassLocation, thighInertiaMatrix );
   MassProperties  HATMassProperties( HATMass, HATCenterOfMassLocation, HATInertiaMatrix );

   // The motion of the segment is related to the motion of the ground via "mobilizers"
   // The "mobilizers" specify the allowable motion of the segment to the ground.
   // More specifically, the motion of an "outboard body" (e.g., the segment)
   // to its inboard body (e.g., the ground) is specified by first constructing:
   // 1. An  "outboard frame"  on the ground (which hooks to the "outboard body" - the segment )
   // 2. An  "inboard  frame"  on the segment  (which hooks to the "inboard  body" - the ground)

   // The orientation and position of the  outboard frame  from the ground's frame is specified below.
   // The outboard frame's axes are aligned with the ground's axes and its origin is coincident with the ground's origin.
   // In other words, for this simple problem the outboard frame and the ground frame are identical.
   const Transform outboardFrameTransformFromGround;  // The default constructor is the identity transform

   // The orientation and position of the  inboard  frame  from the segment's  frame is specified below.
   // The inboard frame's  axes are aligned with the segment's  axes and its origin is coincident with the segment's  origin
   // In other words, for this simple problem the inboard  frame and the segment frame are identical.
   // Although the inboard frame can be constructed in a simple manner analogous to the outboardFrameTransformFromGround (above)
   // it is worthwhile to look at the details of the rotation matrix and position vector in the transform:
   // a. The rotation matrix relating the InboardFrame's x,y,z axes to the SegmentFrame's x,y,z axes is specified InboardFrame_SegmentFrame.
   // b. The position of the InboardFrame's origin from the SegmentFrame origin, expressed in terms of the SegmentFrame's "x, y, z" unit vectors.
   const Rotation   inboardFrameOrientationInFoot;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromFootOrigin( 0, -1000, 0 );
   const Transform  inboardFrameTransformFromFoot( inboardFrameOrientationInFoot, inboardFrameOriginLocationFromFootOrigin );

   const Rotation   inboardFrameOrientationInShank;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromShankOrigin( 0, -2000, 0 );
   const Transform  inboardFrameTransformFromShank( inboardFrameOrientationInShank, inboardFrameOriginLocationFromShankOrigin );

   const Rotation   inboardFrameOrientationInThigh;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromThighOrigin( 0, -3000, 0 );
   const Transform  inboardFrameTransformFromThigh( inboardFrameOrientationInThigh, inboardFrameOriginLocationFromThighOrigin );

   const Rotation   inboardFrameOrientationInHAT;   // ( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3       inboardFrameOriginLocationFromHATOrigin( 0, -4000, 0 );
   const Transform  inboardFrameTransformFromHAT( inboardFrameOrientationInHAT, inboardFrameOriginLocationFromHATOrigin );

   // The model is a 2D simplification, with the rigid bodies moving in the saggital plane
   // so we specify planar mobilizers
   Mobilizer footToGroundMobilizer = Mobilizer::Torsion;
   const BodyId footBodyId = human.addRigidBody( footMassProperties, inboardFrameTransformFromFoot, GroundId, outboardFrameTransformFromGround, footToGroundMobilizer );

   Mobilizer shankToGroundMobilizer = Mobilizer::Torsion;
   const BodyId shankBodyId = human.addRigidBody( shankMassProperties, inboardFrameTransformFromShank, GroundId, outboardFrameTransformFromGround, shankToGroundMobilizer );

   Mobilizer thighToGroundMobilizer = Mobilizer::Torsion;
   const BodyId thighBodyId = human.addRigidBody( thighMassProperties, inboardFrameTransformFromThigh, GroundId, outboardFrameTransformFromGround, thighToGroundMobilizer );

   Mobilizer HATToGroundMobilizer = Mobilizer::Torsion;
   const BodyId HATBodyId = human.addRigidBody( HATMassProperties, inboardFrameTransformFromHAT, GroundId, outboardFrameTransformFromGround, HATToGroundMobilizer );

   // Create a state for this system.
   // Define appropriate states for this multi-body system.
   // Set the initial time to 0.0
   State s;
   mbs.realize( s );
   s.setTime( 0.0 );

   // Set the initial values for the configuration variables (x,y,z)
   human.setMobilizerQ( s, shankBodyId , 0,  0.0 );
   human.setMobilizerU( s, shankBodyId , 0,  0.0 );

   human.setMobilizerQ( s, thighBodyId , 0,  0.0 );
   human.setMobilizerU( s, thighBodyId , 0,  0.0 );

   human.setMobilizerQ( s, HATBodyId , 0,  0.0 );
   human.setMobilizerU( s, HATBodyId , 0,  0.0 );

   // Create a study using the Runge Kutta Merson integrator (alternately use the CPodesIntegrator)
   RungeKuttaMerson myStudy( mbs, s );

   // Set the numerical accuracy for the integrator
   myStudy.setAccuracy( 1.0E-7 );

   // The next statement does lots of accounting
   myStudy.initialize();

   // Open a file to record the simulation results (they are also displayed on screen)
   FILE *outputFile = FileOpenWithMessageIfCannotOpen( "NewtonsBookForPlottingResults.txt", "w" );
   WriteStringToFile(   "time          wx          wy          wz          Hx          Hy          Hz          Htotal          KE \n",  outputFile );
   WriteStringToScreen( "time          wx          wy          wz          Hx          Hy          Hz          Htotal          KE \n" );

   // Visualize results with VTKReporter
   VTKReporter animationResults( mbs );

   const Vec3 footHalfLengths(  segmentScaledLengthMM[7]/2, segmentScaledLengthMM[7]/2 , segmentScaledLengthMM[7]/2 );
   const Vec3 shankHalfLengths(  segmentScaledLengthMM[6]/2, segmentScaledLengthMM[6]/2 , segmentScaledLengthMM[6]/2 );
   const Vec3 thighHalfLengths(  segmentScaledLengthMM[5]/2, segmentScaledLengthMM[5]/2 , segmentScaledLengthMM[5]/2 );
   const Vec3 HATHalfLengths(  segmentScaledLengthMM[1]/2, segmentScaledLengthMM[1]/2 , segmentScaledLengthMM[1]/2 );

   // For visualization purposes only, create a red brick with side equal to segment length
   //  located at the origin for now
   DecorativeBrick footBrick = DecorativeBrick(footHalfLengths);
   footBrick.setColor(Blue);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   footBrick.setOpacity(0.0);   // 0.0 is solid and 1.0 is transparent

   DecorativeBrick shankBrick = DecorativeBrick(shankHalfLengths);
   shankBrick.setColor(Red);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   shankBrick.setOpacity(0.0);   // 0.0 is solid and 1.0 is transparent

   DecorativeBrick thighBrick = DecorativeBrick(thighHalfLengths);
   thighBrick.setColor(Red);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   thighBrick.setOpacity(0.0);   // 0.0 is solid and 1.0 is transparent

   DecorativeBrick HATBrick = DecorativeBrick(HATHalfLengths);
   HATBrick.setColor(Green);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   HATBrick.setOpacity(1.0);   // 0.0 is solid and 1.0 is transparent

   // Decorate the segment with the red sphere at the segment's origin
   const Rotation  segmentToRedBrickOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3      segmentOriginToRedBrickOriginLocation( 0, 0, 0 );
   const Transform segmentToRedBrickTransform( segmentToRedBrickOrientation, segmentOriginToRedBrickOriginLocation );

   animationResults.addDecoration( footBodyId,  segmentToRedBrickTransform, footBrick );
   animationResults.addDecoration( shankBodyId, segmentToRedBrickTransform, shankBrick );
   animationResults.addDecoration( thighBodyId, segmentToRedBrickTransform, thighBrick );
   animationResults.addDecoration( HATBodyId,   segmentToRedBrickTransform, HATBrick );

   // Set the numerical integration step and the time for the simulation to run
   const Real dt = 0.01;
   const Real finalTime = 4.0;

   // Run the simulation and print the results
   while( 1 )
   {
      // Query for results to be printed
      Real time = s.getTime();
      Real kineticEnergy = mbs.getKineticEnergy(s);
      Real uniformGravitationalPotentialEnergy = mbs.getPotentialEnergy(s);
      Real mechanicalEnergy = kineticEnergy + uniformGravitationalPotentialEnergy;

      // Locate the book origin's from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors.
      // Extract the book's y-location from this vector.
      //const Vec3 bookLocation = book.calcBodyOriginLocationInBody( s, bookBodyId, GroundId );
      //Real yLocation = bookLocation[1];

      //// Get the book origin's angular velocity in ground, expressed in terms of the ground's "x,y,z" unit vectors.
      //// Extract the book's wx, wy, wz velocites from this vector.
      //const Vec3 bookAngularVelocityGround = human.calcBodyAngularVelocityInBody( s, footBodyId, GroundId );
      //
      //// Get rotation matrix from Ground to Book and calculate angular velocity in book frame.
      //const Rotation groundToBookRotationMatrix = human.getBodyRotation( s,  footBodyId );
      //const InverseRotation bookToGroundRotationMatrix =  groundToBookRotationMatrix.invert();
      //const Vec3 bookAngularVelocityInBook = bookToGroundRotationMatrix*bookAngularVelocityGround;
      //Real xAngularVelocityInBook = bookAngularVelocityInBook[0];
      //Real yAngularVelocityInBook = bookAngularVelocityInBook[1];
      //Real zAngularVelocityInBook = bookAngularVelocityInBook[2];

      //// Get the book angular Momentum about the body origin in ground, expressed in terms of the ground's "x,y,z" unit vectors.
      //// Extract the book's Hx, Hy, Hz velocites from this vector.
      //const SpatialVec bookMomentum = human.calcBodyMomentumAboutBodyOriginInGround( s, footBodyId );
      ////Real xAngularMomentum = bookMomentum[0][0];
      ////Real yAngularMomentum = bookMomentum[0][1];
      ////Real zAngularMomentum = bookMomentum[0][2];
      ////Real totalAngularMomentum = sqrt(dot(bookMomentum[0],bookMomentum[0]));

      //Real xAngularMomentum = xAngularVelocityInBook*segmentIxx;
      //Real yAngularMomentum = yAngularVelocityInBook*segmentIyy;
      //Real zAngularMomentum = zAngularVelocityInBook*Isegmentzz;
      //Real totalAngularMomentum = sqrt(pow(xAngularVelocityInBook*Ixx,2)+pow(yAngularVelocityInBook*Iyy,2)+pow(zAngularVelocityInBook*Izz,2));

      //// Print results to screen
      //WriteDoubleToFile( time,                 2, stdout );
      //WriteDoubleToFile( xAngularVelocityInBook,            4, stdout );
      //WriteDoubleToFile( yAngularVelocityInBook,            4, stdout );
      //WriteDoubleToFile( zAngularVelocityInBook,            4, stdout );
      //WriteDoubleToFile( xAngularMomentum,            4, stdout );
      //WriteDoubleToFile( yAngularMomentum,            4, stdout );
      //WriteDoubleToFile( zAngularMomentum,            4, stdout );
      //WriteDoubleToFile( totalAngularMomentum,            4, stdout );
      //WriteDoubleToFile( kineticEnergy,     7, stdout );
      //WriteStringToScreen( "\n" );
      //// Print results to file
      //WriteDoubleToFile( time,                 2, outputFile );
      //WriteDoubleToFile( xAngularVelocityInBook,            4, outputFile );
      //WriteDoubleToFile( yAngularVelocityInBook,            4, outputFile );
      //WriteDoubleToFile( zAngularVelocityInBook,            4, outputFile );
      //WriteDoubleToFile( xAngularMomentum,            4, outputFile );
      //WriteDoubleToFile( yAngularMomentum,            4, outputFile );
      //WriteDoubleToFile( zAngularMomentum,            4, outputFile );
      //WriteDoubleToFile( totalAngularMomentum,            4, outputFile );
      //WriteDoubleToFile( kineticEnergy,     7, outputFile );
      //WriteStringToFile( "\n", outputFile );

      // Animate the results for this step
      animationResults.report(s);

      // Check if integration has completed
      if( time >= finalTime ) break;

      // Increment time step
      myStudy.step( time + dt);
   }

   // Simulation completed properly
   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;
}