//-----------------------------------------------------------------------------
// File:     PolygonMassDistributionProperties.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Finds a bounding box around a polygon as axes are rotated.
//-----------------------------------------------------------------------------
// 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  WriteStringToFile(   const char outputString[], FILE *fptr );  
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
bool  WriteDoubleToScreen( double x, int precision) { return WriteDoubleToFile(x, precision, stdout); }
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
void  UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector,
                                  const Vec3 &yUnitVector, const Vec3 &zUnitVector,
                                  Vec6 &boundingBox, bool firstCallToFunction);
Real  ConvertFromDegreesToRadians( Real angleInDegrees ){return angleInDegrees/57.2957795;/*1 radian is approximately 57.3 degrees*/}

//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] ){

   // Vectors to locations of each of the vertices in the N basis
   const Vec3 B0Loc(  0, -1, 0);
   const Vec3 B1Loc(  1,  0, 0);
   const Vec3 B2Loc(  0,  3, 0);
   const Vec3 B3Loc( -1,  4, 0);
   const Vec3 B4Loc( -2,  2, 0);

   // Set unit vectors for the N basis.
   const Vec3 nx( 1, 0, 0);
   const Vec3 ny( 0, 1, 0);
   const Vec3 nz( 0, 0, 0);

   // Set initial unit vectors for the B basis expressed in the N basis
   Vec3 bx(1, 0, 0);
   Vec3 by(0, 1, 0);
   Vec3 bz(0, 0, 1);


   Rotation myNewMat;

   //set the intital value of theta to 0
   Real theta = 0.0;

   //Go ahead and open the file to write
   FILE* myFile = FileOpenWithMessageIfCannotOpen("PolygonMassDistributionResults.txt", "w");

   while( theta <= 90){
      //get a rotation matrix to rotate
      Rotation myRotMat = myNewMat.aboutZ(ConvertFromDegreesToRadians(theta));
      bx = myRotMat * nx;
      by = myRotMat * ny;
      bz = myRotMat * nz;

      //Set the initial values for the bounding box to all zeroes
      //This is (xMin, xMax, yMin, yMax, zMin, zMax)
      Vec6 boundingBox(0, 0, 0, 0, 0, 0);

      //Get the bounding box dimension by updating the dimensions for each max
      UpdateBoundingBoxDimension( B0Loc, bx, by, bz, boundingBox, 1);
      UpdateBoundingBoxDimension( B1Loc, bx, by, bz, boundingBox, 0);
      UpdateBoundingBoxDimension( B2Loc, bx, by, bz, boundingBox, 0);
      UpdateBoundingBoxDimension( B3Loc, bx, by, bz, boundingBox, 0);
      UpdateBoundingBoxDimension( B4Loc, bx, by, bz, boundingBox, 0);

      //Calculate the width, height, and area
      Real boundingWidth = boundingBox[1]-boundingBox[0];
      Real boundingHeight = boundingBox[3]-boundingBox[2];
      Real boundingArea = boundingWidth * boundingHeight;


      //write out these values to the screen
      WriteStringToScreen("\n\ntheta=   ");
      WriteDoubleToScreen(theta, 6);
      WriteStringToScreen("\nxMin =   ");
      WriteDoubleToScreen(boundingBox[0], 6);
      WriteStringToScreen("   xMax =   ");
      WriteDoubleToScreen(boundingBox[1], 6);
      WriteStringToScreen("\nyMin =   ");
      WriteDoubleToScreen(boundingBox[2], 6);
      WriteStringToScreen("   yMax =   ");
      WriteDoubleToScreen(boundingBox[3], 6);
      WriteStringToScreen("\nBounding rectangle area:   ");
      WriteDoubleToScreen(boundingArea, 6);

      
      //write out these values to the file
      WriteStringToFile("\n\ntheta=   ", myFile);
      WriteDoubleToFile(theta, 6, myFile);
      WriteStringToFile("\nxMin =   ", myFile);
      WriteDoubleToFile(boundingBox[0], 6, myFile);
      WriteStringToFile("   xMax =   ", myFile);
      WriteDoubleToFile(boundingBox[1], 6, myFile);
      WriteStringToFile("\nyMin =   ", myFile);
      WriteDoubleToFile(boundingBox[2], 6, myFile);
      WriteStringToFile("   yMax =   ", myFile);
      WriteDoubleToFile(boundingBox[3], 6, myFile);
      WriteStringToFile("\nBounding rectangle area:   ", myFile);
      WriteDoubleToFile(boundingArea, 6, myFile);
      

      theta ++;

   }

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

   return 0;
   

}

//-------------------------------------------------------------------------------
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { 
   return fputs( outputString, fptr ) >=0;

}

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

//-----------------------------------------------------------------------------
void  UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector,
                                  const Vec3 &yUnitVector, const Vec3 &zUnitVector,
                                  Vec6 &boundingBox, bool firstCallToFunction){

   Real vertexX = dot(vertexPosition, xUnitVector);
   Real vertexY = dot(vertexPosition, yUnitVector);
   Real vertexZ = dot(vertexPosition, zUnitVector);
   
   if(firstCallToFunction){
      //if this is the first call to the function, set all the edges of the bounding box to
      //the current values of x, y, and z
      boundingBox[0] = vertexX;
      boundingBox[1] = vertexX;
      boundingBox[2] = vertexY;
      boundingBox[3] = vertexY;
      boundingBox[4] = vertexZ;
      boundingBox[5] = vertexZ;
}   
   else{
      //if this is not the first call, check to see if the current values are greater/less than
      //the previously stored max/mins

      if(vertexX < boundingBox[0])
         boundingBox[0] = vertexX;
      else if(vertexX > boundingBox[1])
         boundingBox[1] = vertexX;
      if(vertexY < boundingBox[2])
         boundingBox[2] = vertexY;
      else if(vertexY > boundingBox[3])
         boundingBox[3] = vertexY;
      if(vertexZ < boundingBox[4])
         boundingBox[4] = vertexZ;
      else if(vertexZ > boundingBox[5])
         boundingBox[5] = vertexZ;

   }


}