//-----------------------------------------------------------------------------
// File:     MassProperties.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculations for mass properties of rigid bodies
// Author:   Sanjay Dastoor - May 11, 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  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 );
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { return fputs( outputString, fptr ) != EOF; }
void UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction );
double convertDegToRad(double inputAngleInDegrees) {return inputAngleInDegrees*3.1416/180.0;}


int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
	FILE *outputFile = FileOpenWithMessageIfCannotOpen("PolygonMassDistributionResults.txt","w");
	
	
	//declare minimum storage areas to keep data
	double minArea = 0;
	double minAngle = 0;

	//declare bounding box
	Vec6 boundingBox(0,0,0,0,0,0);
	
	//unit vectors in frame B
	Vec3 bxInB(1,0,0);
	Vec3 byInB(0,1,0);
	Vec3 bzInB(0,0,1);

	//vectors to vertices in N frame
	Vec3 r_B0_NO(0,-1,0);
	Vec3 r_B1_NO(1,0,0);
	Vec3 r_B2_NO(0,3,0);
	Vec3 r_B3_NO(-1,4,0);
	Vec3 r_B4_NO(-2,2,0);

   
	for (int thetaInDeg=0; thetaInDeg<=90; thetaInDeg++)
	{ 
		//convert degrees to radians
		double thetaInRad = convertDegToRad(thetaInDeg);

		//create rotation matrix about Z
		Rotation N_B = Rotation::aboutZ(thetaInRad);

		//calculate b unit vectors in N frame, using converted position
		Vec3 bxInN = N_B*bxInB;
		Vec3 byInN = N_B*byInB;
		Vec3 bzInN = N_B*bzInB;

		//update bounding box, one vertex at a time
		UpdateBoundingBoxDimension( r_B0_NO, bxInN, byInN, bzInN, boundingBox, true );
		UpdateBoundingBoxDimension( r_B1_NO, bxInN, byInN, bzInN, boundingBox, false );
		UpdateBoundingBoxDimension( r_B2_NO, bxInN, byInN, bzInN, boundingBox, false );
		UpdateBoundingBoxDimension( r_B3_NO, bxInN, byInN, bzInN, boundingBox, false );
		UpdateBoundingBoxDimension( r_B4_NO, bxInN, byInN, bzInN, boundingBox, false );

		//calculate bounding box area
		double boundingBoxArea = (boundingBox[1]-boundingBox[0])*(boundingBox[3]-boundingBox[2]);

		//check for minimum values
		if (thetaInDeg == 0 || boundingBoxArea < minArea) 
		{
			minArea = boundingBoxArea;
			minAngle = thetaInDeg;
		}

		//Write all data to text file
		WriteStringToFile("\n\ntheta = ",outputFile);
		WriteDoubleToFile(thetaInDeg,3,outputFile);
		WriteStringToFile("\nxMin = ", outputFile);
		WriteDoubleToFile(boundingBox[0],3,outputFile);
		WriteStringToFile("    xMax = ", outputFile);
		WriteDoubleToFile(boundingBox[1],3,outputFile);
		WriteStringToFile("\nyMin = ", outputFile);
		WriteDoubleToFile(boundingBox[2],3,outputFile);
		WriteStringToFile("    yMax = ", outputFile);
		WriteDoubleToFile(boundingBox[3],3,outputFile);
		WriteStringToFile("\nBounding Rectangle Area = ", outputFile);
		WriteDoubleToFile(boundingBoxArea,3,outputFile);
	}
}


//-----------------------------------------------------------------------------
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute )
{
   // Try to open the file
   FILE *fptr = fopen( filename, attribute );

   // If unable to open the file, issue a message
   if( !fptr )
   {
      printf( "\n\n Unable to open the file: " );
      printf( filename );
      printf( "\n\n" );
   }

   return fptr;
}


//-----------------------------------------------------------------------------
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 + 1;

   // 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 )
{
	//write vector in format (v[0],v[1],v[2])

   if( !WriteStringToFile("(",fptr)) return false;
   if( !WriteDoubleToFile( v[0], precision, fptr ) ) return false;
   if( !WriteStringToFile(",",fptr)) return false;
   if( !WriteDoubleToFile( v[1], precision, fptr ) ) return false;
   if( !WriteStringToFile(",",fptr)) return false;
   if( !WriteDoubleToFile( v[2], precision, fptr ) ) return false;
   if( !WriteStringToFile(")",fptr)) return false;
   
   return true;
}


//-----------------------------------------------------------------------------
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      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;
}



//-----------------------------------------------------------------------------
void UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction )
{
	//use unit vectors to calculate x/y/z location from vertex position
	double xLocation = dot(vertexPosition, xUnitVector);
	double yLocation = dot(vertexPosition, yUnitVector);
	double zLocation = dot(vertexPosition, zUnitVector);

	//set bounding box to location if location is larger or it's the first run
	if ( firstCallToFunction == true || xLocation < boundingBox[0] )	boundingBox[0] = xLocation;
	if ( firstCallToFunction == true || xLocation > boundingBox[1] )	boundingBox[1] = xLocation;
	if ( firstCallToFunction == true || yLocation < boundingBox[2] )	boundingBox[2] = yLocation;
	if ( firstCallToFunction == true || yLocation > boundingBox[3] )	boundingBox[3] = yLocation;
	if ( firstCallToFunction == true || zLocation < boundingBox[4] )	boundingBox[4] = zLocation;
	if ( firstCallToFunction == true || zLocation > boundingBox[5] )	boundingBox[5] = zLocation;

}