//-----------------------------------------------------------------------------
// File:     InertiaPropertiesOfPDBMolecules.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates the mass, center of mass, and inertia properties of
//           molecules that are downloaded from www.pdb.orb
// Author:   Paul Mitiguy - April 24, 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"
using namespace SimTK;
using namespace std;
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void );
bool  GetStringFromFile(     char inputString[], unsigned long maxSizeOfString, FILE *fptr )  { return fgets( inputString, maxSizeOfString, fptr ) != NULL; }
bool  GetStringFromKeyboard( char inputString[], unsigned long maxSizeOfString )              { return GetStringFromFile( inputString, maxSizeOfString, stdin ); }
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != EOF; }
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteIntegerToFile( int x, FILE *fptr )                       { return fprintf( fptr, "%d", x) >= 0; }
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
static double ConvertFromRadiansToDegrees ( double angleInRadians );
static double ConvertFromDegreesToRadians ( double angleInDegrees );

// New Function
void UpdateBoundingBoxDimension(const Vec3 &vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction);

//-----------------------------------------------------------------------------
// 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 = "PolygonMassDistributionResults.txt";
   FILE *fptr = FileOpenWithMessageIfCannotOpen( outputFileName, "w" );
   if( !fptr ) return false;

   // Define Vertex Positions of the 5 Points Defining the Polygon
   Vec3 posV0InNFrame(0,-1,0);
   Vec3 posV1InNFrame(1,0,0); 
   Vec3 posV2InNFrame(0,3,0);
   Vec3 posV3InNFrame(-1,4,0); 
   Vec3 posV4InNFrame(-2,2,0); 

  
   WriteStringToFile("Polygon Mass Properties:\n\n",fptr);
   // Calculate Area - This is Planar so We use a Simplified Model
   Vec3 polygonAreaSum(0,0,0);
   polygonAreaSum += cross(posV0InNFrame,posV1InNFrame);
   polygonAreaSum += cross(posV1InNFrame,posV2InNFrame);
   polygonAreaSum += cross(posV2InNFrame,posV3InNFrame);
   polygonAreaSum += cross(posV3InNFrame,posV4InNFrame);
   polygonAreaSum += cross(posV4InNFrame,posV0InNFrame);

   // For a planar we just need to look at the z vector
   double polygonArea = polygonAreaSum(2)*.5;

   // Calculate Mass with Given Density
   double polygonDensity = 1.0; //(kg/m^2)
   double polygonMass = polygonArea*polygonDensity;

   // For an Object of Uniform Density We can Treat the System as 5 Equal Lumped Masses
   Inertia particleInertia0(posV0InNFrame,polygonMass/5);
   Inertia particleInertia1(posV1InNFrame,polygonMass/5);
   Inertia particleInertia2(posV2InNFrame,polygonMass/5);
   Inertia particleInertia3(posV3InNFrame,polygonMass/5);
   Inertia particleInertia4(posV4InNFrame,polygonMass/5);

   Inertia inertiaOfSystemBAboutBo = particleInertia0 + particleInertia1 + particleInertia2 + particleInertia3 + particleInertia4;
   Vec3 polygonCenterOfMass = (polygonMass/5*posV0InNFrame + polygonMass/5*posV1InNFrame + polygonMass/5*posV2InNFrame +polygonMass/5*posV3InNFrame + polygonMass/5*posV4InNFrame)/polygonMass;
   Inertia inertiaOfSystemBfromCBToBo(polygonCenterOfMass,polygonMass);
   Inertia inertiaOfSystemBAboutCM = inertiaOfSystemBAboutBo; //- inertiaOfSystemBfromCBToBo;

   WriteStringToFile("Area= ",fptr);
   WriteDoubleToFile(polygonArea,5,fptr);
   WriteStringToFile("  ",fptr);
   WriteStringToFile("Mass=  ",fptr);
   WriteDoubleToFile(polygonMass,5,fptr);
   WriteStringToFile("\n",fptr);

   WriteStringToFile("Position of Center of Mass= ",fptr);
   WriteVec3ToFile(polygonCenterOfMass,5,fptr);
   WriteStringToFile("\n",fptr);

   WriteStringToFile("Moment of Inertia of B about Bcm \n",fptr);
   WriteMat33ToFile(inertiaOfSystemBAboutCM.toMat33(),5,fptr);
   WriteStringToFile("\n\n",fptr);

   // Mass Distribution Results
   WriteStringToFile("Polygon Mass Distribution Results:\n\n",fptr);
   for(double i = 0.0; i <= 90.0; i++)
   {
      Rotation RFromBToN = RFromBToN.aboutZ(ConvertFromDegreesToRadians(-i));
      Rotation RFromNToB = RFromBToN.aboutZ(ConvertFromDegreesToRadians(i));
      
      //Bounding Box Defined as [ xMin xMax yMin yMax zMin zMax ]
      Vec6 boundingBox;
      Vec3 xUnitRotationVector = RFromNToB(0);
      Vec3 yUnitRotationVector = RFromNToB(1);
      Vec3 zUnitRotationVector = RFromNToB(2);

      UpdateBoundingBoxDimension( posV0InNFrame, xUnitRotationVector, yUnitRotationVector, zUnitRotationVector, boundingBox, true);
      UpdateBoundingBoxDimension( posV1InNFrame, xUnitRotationVector, yUnitRotationVector, zUnitRotationVector, boundingBox, false);
      UpdateBoundingBoxDimension( posV2InNFrame, xUnitRotationVector, yUnitRotationVector, zUnitRotationVector, boundingBox, false);
      UpdateBoundingBoxDimension( posV3InNFrame, xUnitRotationVector, yUnitRotationVector, zUnitRotationVector, boundingBox, false);
      UpdateBoundingBoxDimension( posV4InNFrame, xUnitRotationVector, yUnitRotationVector, zUnitRotationVector, boundingBox, false);
      
      WriteStringToFile("Theta= ",fptr);
      WriteDoubleToFile(i,5,fptr);
      WriteStringToFile("\n",fptr);

      WriteStringToFile("xMin=  ",fptr);
      WriteDoubleToFile(boundingBox(0),5,fptr);
      WriteStringToFile("  ",fptr);
      WriteStringToFile("xMax=  ",fptr);
      WriteDoubleToFile(boundingBox(1),5,fptr);
      WriteStringToFile("\n",fptr);

      WriteStringToFile("yMin=  ",fptr);
      WriteDoubleToFile(boundingBox(2),5,fptr);
      WriteStringToFile("  ",fptr);
      WriteStringToFile("yMax=  ",fptr);
      WriteDoubleToFile(boundingBox(3),5,fptr);
      WriteStringToFile("\n",fptr);

      double areaBoundingBox = abs(boundingBox(1)-boundingBox(0))*abs(boundingBox(3)-boundingBox(2));

      WriteStringToFile("Bounding Rectangle Area:  ",fptr);
      WriteDoubleToFile(areaBoundingBox,5,fptr);
      WriteStringToFile("\n\n",fptr);
   }
   
   // Close File
   fclose(fptr);

   return true;
}
//-----------------------------------------------------------------------------
void UpdateBoundingBoxDimension(const Vec3 &vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction)
{
   if(firstCallToFunction)
   {
      boundingBox(0) = dot(xUnitVector,vertexPosition);
      boundingBox(1) = dot(xUnitVector,vertexPosition);

      boundingBox(2) = dot(yUnitVector,vertexPosition);
      boundingBox(3) = dot(yUnitVector,vertexPosition);
   } else {
      double xtest = dot(xUnitVector,vertexPosition);
      double ytest = dot(yUnitVector,vertexPosition);

      if(xtest < boundingBox(0)) boundingBox(0) = xtest; 
      if(xtest > boundingBox(1)) boundingBox(1) = xtest; 
      if(ytest < boundingBox(2)) boundingBox(2) = ytest; 
      if(ytest > boundingBox(3)) boundingBox(3) = ytest; 

   }

}
//-----------------------------------------------------------------------------
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;
}
//-----------------------------------------------------------------------------
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      double vi = v(i); // or v[i];
      if( !WriteDoubleToFile( vi, precision, fptr ) ) return false;
   }
   return true;
}


//-----------------------------------------------------------------------------
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      // Row3 &vi = m[i];   // m[i] returns the row and m(i) returns the column
      // if( !WriteVec3ToFile( vi, precision, fptr ) ) return false;
      for( unsigned int j=0;  j<3;  j++ )
      {
         const Real mij = m[i][j];
         if( !WriteDoubleToFile( mij, precision, fptr ) ) return false;
      }
      if( i<=1 && !WriteStringToFile( "\n", fptr ) ) return false;
   }
   return true;
}
//-----------------------------------------------------------------------------
static double ConvertFromRadiansToDegrees ( double angleInRadians )
{
   return angleInRadians * 180/3.1415926535897932384626433832795028841971;
}

//-----------------------------------------------------------------------------
static double ConvertFromDegreesToRadians ( double angleInDegrees )
{
 return angleInDegrees * 3.1415926535897932384626433832795028841971/180;
}