//-----------------------------------------------------------------------------
// File:     polygonMassDistributionProperties.cpp 
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Finds bounding box around a polygon
//-----------------------------------------------------------------------------
// 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;
//-----------------------------------------------------------------------------
#define PI 3.141592653589793238462643

//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool   RunPolygonMassDistribution( 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 );
double ConvertFromDegreesToRadians( double angleInDegrees ) {return( angleInDegrees*PI/180.0 );}
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[] )
{
   // Simulate the multibody system
   bool simulationSucceeded = RunPolygonMassDistribution();

   // Keep the screen displayed until the user presses the Enter key
   WriteStringToScreen( "\n\n Press  Enter  to terminate the program: " );
   getchar();

   // 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 simulationSucceeded == true ? 0 : 1;
}



//-----------------------------------------------------------------------------
bool  RunPolygonMassDistribution( void )
{
   // Open a file to record the simulation results (they are also displayed on screen)
   FILE *outputFile = FileOpenWithMessageIfCannotOpen( "PolygonMassDistributionResults.txt", "w" );

   Vec6 boundingBox( 0, 0, 0, 0, 0, 0 );

   Vec3  BxExpressedInB( 1, 0, 0 );
   Vec3  ByExpressedInB( 0, 1, 0 );
   Vec3  BzExpressedInB( 0, 0, 1 );

   Vec3 r_B0_No_N( 0, -1, 0 );
   Vec3 r_B1_No_N( 1, 0, 0 );
   Vec3 r_B2_No_N( 0, 3, 0 );
   Vec3 r_B3_No_N( -1, 4, 0 );
   Vec3 r_B4_No_N( -2, 2, 0 );

   Real minBoundingBoxArea = 999.9;
   Real minTheta = 999.9;

   for( Real thetaInDegrees = 0; thetaInDegrees <= 90; thetaInDegrees++ )
   {
      Real thetaInRads = ConvertFromDegreesToRadians( thetaInDegrees );
      Rotation  N_B = Rotation::aboutZ( thetaInRads );
      Vec3  BxExpressedInN = N_B*BxExpressedInB;
      Vec3  ByExpressedInN = N_B*ByExpressedInB;
      Vec3  BzExpressedInN = N_B*BzExpressedInB;

      UpdateBoundingBoxDimension( r_B0_No_N, BxExpressedInN, ByExpressedInN, BzExpressedInN, boundingBox, true );
      UpdateBoundingBoxDimension( r_B1_No_N, BxExpressedInN, ByExpressedInN, BzExpressedInN, boundingBox, false );
      UpdateBoundingBoxDimension( r_B2_No_N, BxExpressedInN, ByExpressedInN, BzExpressedInN, boundingBox, false );
      UpdateBoundingBoxDimension( r_B3_No_N, BxExpressedInN, ByExpressedInN, BzExpressedInN, boundingBox, false );
      UpdateBoundingBoxDimension( r_B4_No_N, BxExpressedInN, ByExpressedInN, BzExpressedInN, boundingBox, false );

      Real boundingBoxArea = (boundingBox[1]-boundingBox[0])*(boundingBox[3]-boundingBox[2]);
      if( boundingBoxArea < minBoundingBoxArea ) 
      {
         minBoundingBoxArea = boundingBoxArea;
         minTheta = thetaInDegrees;
      }

      WriteStringToFile(   "\n\n theta = ",  outputFile );
      WriteDoubleToFile( thetaInDegrees, 5, outputFile );
      WriteStringToFile(   "\n xMin = ",  outputFile );
      WriteDoubleToFile( boundingBox[0], 5, outputFile );
      WriteStringToFile(   " xMax = ",  outputFile );
      WriteDoubleToFile( boundingBox[1], 5, outputFile );
      WriteStringToFile(   "\n yMin = ",  outputFile );
      WriteDoubleToFile( boundingBox[2], 5, outputFile );
      WriteStringToFile(   " yMax = ",  outputFile );
      WriteDoubleToFile( boundingBox[3], 5, outputFile );
      WriteStringToFile(   "\n Bounding rectangle area: ",  outputFile );
      WriteDoubleToFile( boundingBoxArea, 5, outputFile );
   }

   WriteStringToFile(   "\n\n Min bounding rectangle area: ",  outputFile );
   WriteDoubleToFile( minBoundingBoxArea, 5, outputFile );
   WriteStringToFile(   " at theta =  ",  outputFile );
   WriteDoubleToFile( minTheta, 5, outputFile );

   // Simulation completed properly
   return true;
}


//-----------------------------------------------------------------------------
void UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector,const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction )
{
   Real xLocation = dot( vertexPosition, xUnitVector );
   Real yLocation = dot( vertexPosition, yUnitVector );
   Real zLocation = dot( vertexPosition, zUnitVector );

   // 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;
   if( firstCallToFunction==1 || zLocation < boundingBox[4] ) boundingBox[4]  = zLocation;
   if( firstCallToFunction==1 || zLocation > boundingBox[5] ) boundingBox[5]  = zLocation;
}


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