//-----------------------------------------------------------------------------
// File:     MassDistrubtionAndBBox.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculates the centroid, area and bounding box for rotating polygon
// Author:   Paul Mitiguy - April 24, 2007  modified by Mandy Koop 5/10/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 "ChemicalElement.h"
using namespace SimTK;
using namespace std;
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool  DoRequiredTasks( void );
double  ConvertFromRadiansToDegrees( double angleInRadians )  { return angleInRadians * 57.295779513082320876798154814105170332405472466564321549160243861; }
double  ConvertFromDegreesToRadians( double angleInDegrees )  { return angleInDegrees * 0.017453292519943295769236907684886127134428718885417254560971914402; }
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 );
const char*  ConvertStringToDouble( const char *s, double &returnValue, double defaultValue );

// Returns the number of molecules in the multibody system.  Note: Check the status of successReadingPdb before continuing.
int  AddMoleculesFromPdbFileToSimbodyMatterSubsystem( SimbodyMatterSubsystem &allTheMolecules, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );
int  AddBoundingBoxToSimbodyMatterSubsystem (SimbodyMatterSubsystem &allTheMolecules);
int  CalculateMassPropertiesOfPdbMolecule( MassProperties &massProperties, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );
int  SetMobilizerStatesForBoundingBox( SimbodyMatterSubsystem &allTheMolecules, State &state);
int  SetMobilizerStatesFromPdbFile( SimbodyMatterSubsystem &allTheMolecules, State &state, const char *nameOfPdbFile, char selectedPdbChain, bool includeHETAM, bool &successReadingPdb );

// Sets the decorative geometry for all molecules to spheres
void  DecorateAllTheMoleculesWithSpheres( SimbodyMatterSubsystem &allTheMolecules, VTKReporter &viewer );
void  DecorateBoundingBox( SimbodyMatterSubsystem &allTheMolecules, Vec3 boundingBoxWidth, Vec6 boundingBox, VTKReporter &viewer );

// Calculates the bounding box for the origins of all the bodies in the matter subsystem.
// Returns 6 real numbers in the following order:  minX, maxX,  minY, maxY,  minZ, maxZ
//Vec6  CalculateBoundingBoxOfBodyOriginsUsingGroundXYZDirections( const SimbodyMatterSubsystem &allTheBodies, const State &state, int numberOfBodies );
void UpdateBoundingBoxDimension (const Vec3 vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction);

// Takes a single line from a PDB file and parses it.  Returns its ChemicalElement (if there is one).
// const ChemicalElement*  GetChemicalElementAndPositionFromPDBInputLine( const char *inputLine, char selectedPdbChain, bool includeHETAM, Vec3 &atomPositionFromLaboratoryOrigin, bool &successReadingPdb );

// Takes a single line from a PDB file and parses it looking for the atom's x,y,z positions from the laboratory origin
Vec3  GetAtomPositionFromPdbLine( const char *inputLine, bool &successReadingPdb );


//-----------------------------------------------------------------------------
// 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.  \n Please see file PolygonMassDistributionResults.txt for results. \n 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 )
{
    // Function opens file for writing results to and returns error is unable to open 
   FILE *fptr = FileOpenWithMessageIfCannotOpen("PolygonMassDistributionResults.txt","w");
	if (!fptr) return 1;
   
   // Setting variables to store minimum values of theta and bounding box area
   int minTheta = 99;
   double minArea = 999;
   int theta = 0;

   // Setting variable that will store the precision that each value is written to file
   int precision = 5;

   // In the while loop below, the vertices from the polygon will be expressed in the B frame
   // and one by one will be sent to the UpdateBoundingBoxDimension functon to check if it is 
   // a minimum or maximum x/y value.  The while function increments theta (the offset angle between 
   // frame N and frame B, by 1 deg each cycle. 
   while (theta < 91) 
   {
      // Converts angle in degrees to radians
      Real angleInRadians= ConvertFromDegreesToRadians(theta);
      
      // Creates a rotation matrix (with one rotation about the z axis, i.e. Nz = Bz).
      // Given a vector in frame B, it will then re-express it in frame N.
      Rotation N_B = Rotation::aboutZ(angleInRadians);
      Rotation B_N = ~(N_B);
      
      // Vertexposition 1: B0 expressed in frame N.
      const Vec3 B0n(0,-1,0);

      // Expressing Bo in frame B
      const Vec3 B0b = B_N*B0n;

      // Initializing unit Vecs to use in finding the boundingBoxDimension
      const Vec3 xUnitVector(1,0,0);
      const Vec3 yUnitVector(0,1,0);
      const Vec3 zUnitVector(0,0,1);
      
      // Initializing the bounding box vector which store the 
      // xmin, xmax, ymin, ymax, zmin, zmax respectively.
      Vec6 boundingBox(0,0,0,0,0,0);

      // Checking to see if vertex B0 expressed in frame B is a min or max
      UpdateBoundingBoxDimension ( B0b, xUnitVector, yUnitVector, zUnitVector, boundingBox,true);

      // Checking to see if vertex B1 expressed in frame B is a min or max by first 
      // a) define vertex in frame N
      // b) express in in frame B
      // c) use UpdateBoundingBoxDimension function to determine if it is a minimum or maximum x/y value
      Vec3 B1n(1,0,0);
      Vec3 B1b = B_N*B1n;
      UpdateBoundingBoxDimension ( B1b, xUnitVector, yUnitVector, zUnitVector, boundingBox,false);


       // Checking to see if vertex B2 expressed in frame B is a min or max by first 
      // a) define vertex in frame N
      // b) express in in frame B
      // c) use UpdateBoundingBoxDimension function to determine if it is a minimum or maximum x/y value// Checking to see if vertex B2 expressed in frame B is a min or max
      Vec3 B2n(0,3,0);
      Vec3 B2b = B_N*B2n;
      UpdateBoundingBoxDimension ( B2b, xUnitVector, yUnitVector, zUnitVector, boundingBox,false);

      // Checking to see if vertex B3 expressed in frame B is a min or max by first 
      // a) define vertex in frame N
      // b) express in in frame B
      // c) use UpdateBoundingBoxDimension function to determine if it is a minimum or maximum x/y value      
      Vec3 B3n(-1,4,0);
      Vec3 B3b = B_N*B3n;
      UpdateBoundingBoxDimension ( B3b, xUnitVector, yUnitVector, zUnitVector, boundingBox,false);
   

      // Checking to see if vertex B4 expressed in frame B is a min or max by first 
      // a) define vertex in frame N
      // b) express in in frame B
      // c) use UpdateBoundingBoxDimension function to determine if it is a minimum or maximum x/y value      
      Vec3 B4n(-2,2,0);
      Vec3 B4b = B_N*B4n;
      UpdateBoundingBoxDimension ( B4b, xUnitVector, yUnitVector, zUnitVector, boundingBox,false);
      
      // Writing results to file
      WriteStringToFile( "theta=", fptr );
      WriteDoubleToFile( theta, precision, fptr );
      WriteStringToFile("\n", fptr);

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

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

      // Calculating the area for the bounding box for this value of theta
      Real xWidth = boundingBox[1] - boundingBox[0];
      Real yWidth = boundingBox[3] - boundingBox[2];
      Real zWidth = boundingBox[5] - boundingBox[4];
      Real area = xWidth * yWidth;

      // Writing results to file
      WriteStringToFile( "Bounding rectangle area:", fptr );
      WriteDoubleToFile( area, precision, fptr );
      WriteStringToFile("\n\n",fptr);

      // Checking to see if this is the minimum area for all values of theta
      theta++;
      if(minArea > area) 
      {
         minArea = area;
         minTheta = theta;
      }
   }

   
   // Writing minimum area to file.
   WriteStringToFile( "The Min. Bounding rectangle area", fptr );
   WriteDoubleToFile( minArea, precision, fptr );
   WriteStringToFile("\n\n",fptr);
 
   // Writing theta to file that corresponds to minimum area.
   WriteStringToFile( "The Min. bounding rectangle is at theta", fptr );
   WriteDoubleToFile( minTheta, precision, fptr );
   WriteStringToFile("\n\n",fptr);

   return true;
}



//-----------------------------------------------------------------------------
void UpdateBoundingBoxDimension (const Vec3 vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction)
{
   // Keep track of the minimum (most negative) and maximum (most positive) of the vertex position.

   // Extracting the bx and by components of each vector (by dotting with respective unit Vector)
   
   Real xLocation = dot(vertexPosition,xUnitVector);
   Real yLocation = dot(vertexPosition, yUnitVector);

      // Compare the min/max values (and possibly reassign) with the x, y, z, values of each particle.
      if( firstCallToFunction==1 || xLocation < boundingBox[0] ) boundingBox[0] = xLocation;
      if (firstCallToFunction==1 || xLocation > boundingBox[1] ) boundingBox[1] = xLocation;
      if( firstCallToFunction==1 || yLocation < boundingBox[2] ) boundingBox[2] = yLocation;
      if (firstCallToFunction==1 || yLocation > boundingBox[3] ) boundingBox[3] = yLocation;
   }


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