//-----------------------------------------------------------------------------
// File:     AppleWithHorizontalControl.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Simulates apple with torque control
// Author:   Mary Elting - May 30, 2007
//-----------------------------------------------------------------------------
#include "StandardCppHeadersAndNamespace.h"
#include "SimTKsimbody.h"
#include "UserForceControlHorizontal.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 );


//-----------------------------------------------------------------------------
// 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 (for the apple)
   mbs.setMatterSubsystem( matterSubsystem );    // Add the matter sub-system to the system.

   // Create the mass, center of mass, and inertia properties for the apple
   const Real  massOfApple = 0.142;

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

   // Create the apple's inertia matrix about its origin for the "x, y, z" unit vectors fixed in the apple'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)
   // The following assumes an apple 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 I = massOfApple * 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  appleInertiaMatrix( Ixx, Iyy, Izz, Ixy, Ixz, Iyz );

   // 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 the apple,
   // it does not get associated with the apple until the  addRigidBody  method.
   MassProperties  appleMassProperties( massOfApple, appleCenterOfMassLocation, appleInertiaMatrix );

   // The Apple's motion relative to ground is specified by a "mobilizer" (sometimes called a "joint").
   // The Mobilizer has an  "Inboard  Frame"  fixed on the "Inboard  Body" (Ground)
   //               and an  "Outboard Frame"  fixed on the "Outboard Body" (Apple)

   // The Inboard Frame's axes are aligned    with the Ground's axes.
   // The Inboard origin       is  coincident with the Ground's origin.
   const Rotation   inboardFrameOrientationInGround;   // Default constructor is identity matrix
   const Vec3       inboardFrameOriginLocationFromGroundOrigin( 0, 0, 0 );
   const Transform  inboardFrameTransformFromGround( inboardFrameOrientationInGround, inboardFrameOriginLocationFromGroundOrigin );

   // The Outboard Frame's axes are aligned with the Apple's axes.
   // The Outboard origin       is  offset  from the Apple's origin in the Apple's y-direction.
   const Rotation   outboardFrameOrientationInApple;     // Default constructor is identity matrix
   const Vec3       outboardFrameOriginLocationFromAppleOrigin( 0, 0, 0 );
   const Transform  outboardFrameTransformFromApple( outboardFrameOrientationInApple, outboardFrameOriginLocationFromAppleOrigin );

   // There are many ways that the OutboardFrame can move relative to the InboardFrame.
   // The following allows translation along the Mobilizer's InboardFrame's "x,y,z" axes.
   Mobilizer AppleToGroundMobilizer = Mobilizer::Cartesian;
   const BodyId appleBodyId = matterSubsystem.addRigidBody( appleMassProperties, outboardFrameTransformFromApple, GroundId, inboardFrameTransformFromGround, AppleToGroundMobilizer );


   // Add a horizontal force force sub-system to this multi-body system.
   GeneralForceElements userForceElements;
   mbs.addForceSubsystem( userForceElements );
   // Although "new" was used to allocate this UserForce, do not "delete" it.
   // This bug will be fixed in the next version of Simbody so it can take an object from the stack or heap.
   // For now, addUserForce takes ownership of the allocated item and takes care of deleting it at the end.
   
   //UserForceCostHorizontal *appliedForceOnApple = new UserForceCostHorizontal( appleBodyId);
   //userForceElements.addUserForce( appliedForceOnApple );
   
   //this force applies torque control  
   UserForceControlHorizontal *controlForceOnApple = new UserForceControlHorizontal( appleBodyId, massOfApple, 1.0, 1.0);
   userForceElements.addUserForce( controlForceOnApple );

   // 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)
   matterSubsystem.setMobilizerQ( s, appleBodyId, 0,  0.0 );
   matterSubsystem.setMobilizerQ( s, appleBodyId, 1,  0.0 );
   matterSubsystem.setMobilizerQ( s, appleBodyId, 2,  0.0 );

   // Set the initial values for the motion variables
   matterSubsystem.setMobilizerU( s, appleBodyId, 0,  0.0 );
   matterSubsystem.setMobilizerU( s, appleBodyId, 1,  0.0 );
   matterSubsystem.setMobilizerU( s, appleBodyId, 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( "AppleHorizontalForceControlResults.txt", "w" );
   WriteStringToFile(   "time  xLocation    xVelocity   yLocation   yVelocity  mechanicalEnergy\n",  outputFile );
   WriteStringToScreen( "time  xLocation    xVelocity   yLocation   yVelocity  mechanicalEnergy\n" );

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

   // For visualization purposes only, create a red sphere
   const Real radiusOfApple = 3.6576/100;  // 1.44 inches (3.6576 cm)
   DecorativeSphere redSphere = DecorativeSphere( radiusOfApple );
   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 apple with the red sphere at the apple's origin
   const Rotation  appleToRedSphereOrientation; //( 1,0,0, 0,1,0, 0,0,1 );
   const Vec3      appleOriginToRedSphereOriginLocation( 0, 0, 0 );
   const Transform appleToRedSphereTransform( appleToRedSphereOrientation, appleOriginToRedSphereOriginLocation );
   animationResults.addDecoration( appleBodyId, appleToRedSphereTransform, redSphere );

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

   // 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 apple origin's from the ground's origin, expressed in terms of the ground's "x,y,z" unit vectors.
      // Extract the apple's y-location from this vector.
      const Vec3 appleLocation = matterSubsystem.calcBodyOriginLocationInBody( s, appleBodyId, GroundId );
      Real xLocation = appleLocation[0];
      Real yLocation = appleLocation[1];

      // Get the apple origin's velocity in ground, expressed in terms of the ground's "x,y,z" unit vectors.
      // Extract the apple's y-velocity from this vector.
      const Vec3 appleVelocity = matterSubsystem.calcBodyOriginVelocityInBody( s, appleBodyId, GroundId );
      Real xVelocity = appleVelocity[0];
      Real yVelocity = appleVelocity[1];

      // Get the apple origin's acceleration in ground, expressed in terms of the ground's "x,y,z" unit vectors.
      // Extract the apple's y-acceleration from this vector.
      // const Vec3 appleAcceleration = matterSubsystem.calcBodyOriginAccelerationInBody( s, appleBodyId, GroundId );
      // Real yAcceleration = appleAcceleration[1];


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

      // Animate the results for this step
      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;
}