//-----------------------------------------------------------------------------
// File:     ProjectileMotion.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Simulates projectile motion
// Author:   Paul Mitiguy - April 11, 2007
//-----------------------------------------------------------------------------
#include "StandardCppHeadersAndNamespace.h"
#include "UserForceAirResistanceProjectile.h"	
#include "SimTKsimbody.h"
using namespace SimTK;
//-----------------------------------------------------------------------------


//-----------------------------------------------------------------------------
// 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 );
double  ConvertFromRadiansToDegrees( double angleInRadians )  { return angleInRadians * 57.295779513082320876798154814105170332405472466564321549160243861;   }
double  ConvertFromDegreesToRadians( double angleInDegrees )  { return angleInDegrees * 0.017453292519943295769236907684886127134428718885417254560971914402; }
clock_t  SleepAppropriateAmountOfTime( clock_t numberOfClockTicksAtLastDisplay, Real integrationStepDt, Real slowDownFactor );


//-----------------------------------------------------------------------------
// 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 )
{
   // 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  matterSubsystem;      // Create a single matter sub-system
   mbs.setMatterSubsystem( matterSubsystem );    // 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, -9.8, 0 );
   UniformGravitySubsystem gravity( gravityVector );
   mbs.addForceSubsystem( gravity );

   // Create the mass, center of mass, and inertia properties for the baseball
   // The following is for a regulation baseball of radius 1.44 inches (3.6576 cm) with a radius of gyration of 0.91 inches (2.3114 cm)
   // which approximates a perfect sphere of radius 1.44 inches (3.6576 cm)
   const Real  massOfBaseball = 0.142;
   const Vec3  baseballCenterOfMassLocation( 0, 0, 0 );
   const Real I = massOfBaseball * pow( 0.023114, 2 );  // Inertia = mass * radiusOfGyration^2
   const Real  Ixx = I,  Iyy = I,   Izz = I;
   const Real  Ixy = 0,  Ixz = 0,   Iyz = 0;
   const Inertia  baseballInertiaMatrix( Ixx, Iyy, Izz, Ixy, Ixz, Iyz );
   MassProperties  baseballMassProperties( massOfBaseball, baseballCenterOfMassLocation, baseballInertiaMatrix );

   // The baseball's motion is related to ground via "mobilizers".
   const Transform  inboardFrameTransformFromGround;    // The default constructor is the identity transform
   const Transform outboardFrameTransformFromBaseball;  // The default constructor is the identity transform
   Mobilizer baseballToGroundMobilizer = Mobilizer::Cartesian;
   const BodyId baseballBodyId = matterSubsystem.addRigidBody( baseballMassProperties, outboardFrameTransformFromBaseball, GroundId, inboardFrameTransformFromGround, baseballToGroundMobilizer );

   // Add an air-resistance force sub-system to this multi-body system.
   GeneralForceElements userForceElements;
   mbs.addForceSubsystem( userForceElements );
   
   UserForceAirResistanceProjectile *airResistanceOnBaseball = new UserForceAirResistanceProjectile( baseballBodyId, 0.002 );
   userForceElements.addUserForce( airResistanceOnBaseball );

   // 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 mobilizers (x,y,z)
   matterSubsystem.setMobilizerQ( s, baseballBodyId, 0,  0.0 );
   matterSubsystem.setMobilizerQ( s, baseballBodyId, 1,  0.0 );
   matterSubsystem.setMobilizerQ( s, baseballBodyId, 2,  0.0 );

   // Set the initial values for the motion variables
   Real launchAngle = ConvertFromDegreesToRadians( 35 );
   Real xInitialVelocity = 44.7 *cos( launchAngle );
   Real yInitialVelocity = 44.7 *sin( launchAngle );
   matterSubsystem.setMobilizerU( s, baseballBodyId, 0,  xInitialVelocity );
   matterSubsystem.setMobilizerU( s, baseballBodyId, 1,  yInitialVelocity );
   matterSubsystem.setMobilizerU( s, baseballBodyId, 2,  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( "ProjectileMotionResults.txt", "w" );
   WriteStringToFile(   "time      xLocation    yLocation  mechanicalEnergy  xDifferenceFromExact  yDifferenceFromExact \n",  outputFile );
   WriteStringToScreen( "time      xLocation    yLocation  mechanicalEnergy  xDifferenceFromExact  yDifferenceFromExact \n" );

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

   // For visualization purposes only, create a red sphere
   const Real radiusOfBaseball = 3.6576/100;  // 1.44 inches (3.6576 cm)
   DecorativeSphere redSphere = DecorativeSphere( radiusOfBaseball );
   redSphere.setColor(Red);     // Can also specify a Vec3 with rgb which scale from 0 to 1
   redSphere.setOpacity(0.0);   // 0.0 is solid and 1.0 is transparent

   // Decorate the baseball with the red sphere at the baseball's origin (identity transform)
   const Transform baseballToRedSphereTransform;
   animationResults.addDecoration( baseballBodyId, baseballToRedSphereTransform, redSphere );

   // Set the numerical integration step and the time for the simulation to run
   const Real integrationStepDt = 0.01;
   const Real finalTime = 5.23;
   const Real finalTimeCompare = finalTime - 0.01*integrationStepDt;

   // Run the simulation and print the results
   // Keep track of initial clock_t to slow results
   clock_t numberOfClockTicksAtLastDisplay = 0;
   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 baseball origin's from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors.
      // Extract the baseball's y-location from this vector.
      const Vec3 baseballLocation = matterSubsystem.calcBodyOriginLocationInBody( s, baseballBodyId, GroundId );
      Real xLocation = baseballLocation[0];
      Real yLocation = baseballLocation[1];

      // Exact analytical results and difference of numerical integration results
      Real xExact = xInitialVelocity * time;
      Real yExact = yInitialVelocity * time - 4.9 * time * time;
      Real xDifferenceFromExact = xExact - xLocation;
      Real yDifferenceFromExact = yExact - yLocation;

      // Print results to screen
      WriteDoubleToFile( time,                 2, stdout );
      WriteDoubleToFile( xLocation,            5, stdout );
      WriteDoubleToFile( yLocation,            5, stdout );
      WriteDoubleToFile( mechanicalEnergy,     7, stdout );
      WriteDoubleToFile( xDifferenceFromExact, 7, stdout );
      WriteDoubleToFile( yDifferenceFromExact, 7, stdout );
      WriteStringToScreen( "\n" );
      // Print results to file
      WriteDoubleToFile( time,                 2, outputFile );
      WriteDoubleToFile( xLocation,            5, outputFile );
      WriteDoubleToFile( yLocation,            5, outputFile );
      WriteDoubleToFile( mechanicalEnergy,     7, outputFile );
      WriteDoubleToFile( xDifferenceFromExact, 7, outputFile );
      WriteDoubleToFile( yDifferenceFromExact, 7, outputFile );
      WriteStringToFile( "\n", outputFile );

      // Wait an appropriate amount of time before animating results for this step
      numberOfClockTicksAtLastDisplay = SleepAppropriateAmountOfTime( numberOfClockTicksAtLastDisplay, integrationStepDt, 4.0 );
      animationResults.report(s);

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

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

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


//--------------------------------------------------------------------------
clock_t  SleepAppropriateAmountOfTime( clock_t numberOfClockTicksAtLastDisplay, Real integrationStepDt, Real slowDownFactor )
{
   // Ensure we have moved past the next "wake-up" time
   clock_t  numberOfClockTicksPerIntegrationStep = (clock_t)(slowDownFactor * (integrationStepDt * CLOCKS_PER_SEC + 1.0E-15));
   clock_t  waitUntilThisClockTime = numberOfClockTicksAtLastDisplay + numberOfClockTicksPerIntegrationStep;
   while( waitUntilThisClockTime > clock() );
   return waitUntilThisClockTime;
}