//-----------------------------------------------------------------------------
// File:     HelloVector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Performs several operations on two specified vectors
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// 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 );
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 );
double ConvertFromDegreesToRadians( double angleInDegrees )  { return angleInDegrees * 0.017453292519943295769236907684886127134428718885417254560971914402; }


//-----------------------------------------------------------------------------
int  main( void )
{
   // Define the coordinates of the vertices in N
   double thetaInDegreesMax = 90;
   Vec3 pointB0( 0, -1, 0);
   Vec3 pointB1( 1, 0, 0);
   Vec3 pointB2( 0, 3, 0);
   Vec3 pointB3( -1, 4, 0);
   Vec3 pointB4( -2, 2, 0);

   // Define generic unit vecors
   Vec3 xUnitVector( 1, 0, 0);
   Vec3 yUnitVector( 0, 1, 0);
   Vec3 zUnitVector( 0, 0, 1);

   //Initialize the bounding box vector  
   Vec6 boundingBox;

   // Write results to file HelloVectorResults.txt
   FILE *fptr1 = FileOpenWithMessageIfCannotOpen( "PolygonMassDistributionResults.txt", "w+" );

   // Calculate the bounding rectangle at degrees 0-90
   for( int i=0;  i <= (int)thetaInDegreesMax;  i++ )
   {
      Real thetaInRads = ConvertFromDegreesToRadians( i );

      // Calculate the rotation matrix from N_B
      Rotation  N_B = Rotation::aboutZ( thetaInRads );

      // Determine the positions of the vertices expressed in B
      Vec3 pointB0inB = N_B.transpose()*pointB0;
      Vec3 pointB1inB = N_B.transpose()*pointB1;
      Vec3 pointB2inB = N_B.transpose()*pointB2;
      Vec3 pointB3inB = N_B.transpose()*pointB3;
      Vec3 pointB4inB = N_B.transpose()*pointB4;

      // Find the maximum x and y positions in B to define the bounding box
      UpdateBoundingBoxDimension( pointB0inB, xUnitVector, yUnitVector, zUnitVector, boundingBox, true );
      UpdateBoundingBoxDimension( pointB1inB, xUnitVector, yUnitVector, zUnitVector, boundingBox, false );
      UpdateBoundingBoxDimension( pointB2inB, xUnitVector, yUnitVector, zUnitVector, boundingBox, false );
      UpdateBoundingBoxDimension( pointB3inB, xUnitVector, yUnitVector, zUnitVector, boundingBox, false );
      UpdateBoundingBoxDimension( pointB4inB, xUnitVector, yUnitVector, zUnitVector, boundingBox, false );

      // Find the Area of the bounding box
      Real xWidth = boundingBox[1]-boundingBox[0];
      Real yWidth = boundingBox[3]-boundingBox[2];
      Real boundingRectangleArea = xWidth * yWidth;

      // Print results
      fprintf( fptr1, "theta = " );
      WriteDoubleToFile( i, 5, fptr1);
      fprintf( fptr1, "\nxmin =  " );
      WriteDoubleToFile( boundingBox[0], 5, fptr1);
      fprintf( fptr1, "    xmax =  " );
      WriteDoubleToFile( boundingBox[1], 5, fptr1);
      fprintf( fptr1, "\nymin =  " );
      WriteDoubleToFile( boundingBox[2], 5, fptr1);
      fprintf( fptr1, "    ymax =  " );
      WriteDoubleToFile( boundingBox[3], 5, fptr1);
      fprintf( fptr1, "\nBounding rectangle area =  " );
      WriteDoubleToFile( boundingRectangleArea, 5, fptr1);
      fprintf( fptr1, "\n\n" );
   }

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

}

//-----------------------------------------------------------------------------

FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute)
{
	FILE *stream;
	if( (stream = fopen( filename, attribute )) == NULL)
	{
		printf("cannot open file, press enter to exit program.\n");
	}
	else
	{
		printf("results file opened, press enter to continue.\n");
	}
	int key2 = getchar();
	return stream;
}
//-----------------------------------------------------------------------------
void UpdateBoundingBoxDimension( const Vec3 &vertexPosition, const Vec3 &xUnitVector, const Vec3 &yUnitVector, const Vec3 &zUnitVector, Vec6 &boundingBox, bool firstCallToFunction )
{
   // set current min and max values
   Real xMin=boundingBox[0];
   Real xMax=boundingBox[1];
   Real yMin=boundingBox[2];
   Real yMax=boundingBox[3];
   Real zMin=boundingBox[4];
   Real zMax=boundingBox[5];

   // Find distance to vertex in Bx, By, and Bz directions
   Real xLocation = dot(xUnitVector, vertexPosition);
   Real yLocation = dot(yUnitVector, vertexPosition);
   Real zLocation = dot(zUnitVector, vertexPosition);

   // Compare the min/max values (and possibly reassign) with the x, y, z, values of each particle.
   if( firstCallToFunction == true || xLocation < xMin ) xMin = xLocation;
   if( firstCallToFunction == true || yLocation < yMin ) yMin = yLocation;
   if( firstCallToFunction == true || zLocation < zMin ) zMin = zLocation;
   if( firstCallToFunction == true || xLocation > xMax ) xMax = xLocation;
   if( firstCallToFunction == true || yLocation > yMax ) yMax = yLocation;
   if( firstCallToFunction == true || zLocation > zMax ) zMax = zLocation;

   // update the bounding box values
   boundingBox[0] =  xMin;
   boundingBox[1] =  xMax;
   boundingBox[2] =  yMin;
   boundingBox[3] =  yMax;
   boundingBox[4] =  zMin;
   boundingBox[5] =  zMax;
}
//------------------------------------------------------------------------------
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;
}