//-----------------------------------------------------------------------------
// File:     HelloVector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Calculate dot-products and cross product in Simbody
//-----------------------------------------------------------------------------
// 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  DoRequiredTasks( void );
double  ConvertFromRadiansToDegrees( double angleInRadians )  { return angleInRadians * 57.295779513082320876798154814105170332405472466564321549160243861; }
double  ConvertFromDegreesToRadians( double angleInDegrees )  { return angleInDegrees * 0.017453292519943295769236907684886127134428718885417254560971914402; }
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 );
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  WriteDoubleMatrixElementsToFile( const double *array, unsigned int numberOfRows, unsigned int numberOfCols, int precision, FILE *fptr );
bool  WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr );

// Writes a Vec3 to specified file by extracting the 3 scalar values, returns true if successful
bool    WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );

// Writes a 3X3 matrix to specified file by extracting the 9 scalar values, returns true if successful. 
bool    WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );


//-----------------------------------------------------------------------------
// 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 failed.  Press  Enter  to finish: " : "\n\nProgram succeeded.\n\nPlease see 'HelloVectorResults.txt' file for calculated results. \n\nPress  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 *pFile = FileOpenWithMessageIfCannotOpen("HelloVectorResults.txt","w");
	if (!pFile) return 1;

   // Lab 3.1
   WriteStringToFile("Lab 3.1: Calculating dot products and cross products in Simbody:\n\n\n",pFile);

   //Defining and writing the vector v to file
   const Vec3 v(2,3,4);
   const char stri[200] = "The following 3 numbers are the nx,ny,nz components of the vector v:";
   WriteStringToFile((stri),pFile);
   WriteVec3ToFile(v,1,pFile);
   WriteStringToFile("\n\n",pFile);


   //Defining and writing the vector w to the file
   const Vec3 w(5,-6,7);
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector w:\n",pFile);
   WriteVec3ToFile(w,1,pFile);
   WriteStringToFile("\n\n",pFile);
   
   //Defining and writing the vector v*10 to the file
   const Vec3 vTimesTen = 10*v;
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector 10 * v:\n",pFile);
   WriteVec3ToFile(vTimesTen,1,pFile);
   WriteStringToFile("\n\n",pFile);
      
   //Defining and writing the vector v*10 to the file
   const Vec3 TenTimesV = v*10;
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v * 10:\n",pFile);
   WriteVec3ToFile(TenTimesV,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the vector v/10 to the file
   const Vec3 vDivideBy10 = v / 10;
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v / 10:\n",pFile);
   WriteVec3ToFile(vDivideBy10,1,pFile);
   WriteStringToFile("\n\n",pFile);
   
   //Defining and writing the vector v + vector w to the file   
   const Vec3 sumVW = v+w;
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v + vector w:\n",pFile);
   WriteVec3ToFile(sumVW,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the vector v - vector w to the file
   const Vec3 subtractVW = v - w;   
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v * 10:\n",pFile);
   WriteVec3ToFile(subtractVW,1,pFile);
   WriteStringToFile("\n\n",pFile);
      
   //Defining and writing the vector v dotted with vector w to the file
   double dotVW = dot(v,w);   
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v dotted with vector w:\n",pFile);
   WriteDoubleToFile(dotVW,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the vector v crossed with vector w
   const Vec3 crossVW = cross(v,w);   
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v crossed with vector w:\n",pFile);
   WriteVec3ToFile(crossVW,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the vector w crossed with vector v to the file
   const Vec3 crossWV = cross(w,v);   
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector w crossed wth vector v:\n",pFile);
   WriteVec3ToFile(crossWV,1,pFile);
   WriteStringToFile("\n\n",pFile);


   //Defining and writing the scalar value as a result of v squared to the file
   double squareV = dot(v,v);   
   WriteStringToFile("The following scalar value is a result of squaring v:\n",pFile);
   WriteDoubleToFile(squareV,1,pFile);
   WriteStringToFile("\n\n",pFile);   
   
   //Defining and writing the vector v cubed to the file
   const Vec3 cubeV = v*squareV;   
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the vector v cubed:\n",pFile);
   WriteVec3ToFile(cubeV,1,pFile);
   WriteStringToFile("\n\n",pFile);


   //Defining and writing the magnitude of vector v
   double magV = sqrt(squareV);   
   WriteStringToFile("The following scalar value is the magnitude of the vector v:\n",pFile);
   WriteDoubleToFile(magV,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the scalar value of the vector W squared
   double squareW = dot(w,w);   
   WriteStringToFile("The following scalar value is a result of squaring vector W:\n",pFile);
   WriteDoubleToFile(squareW,1,pFile);
   WriteStringToFile("\n\n",pFile);
   
   //Defining and writing the magnitude of the vector W
   double magW = sqrt(squareW);   
   WriteStringToFile("The following scalar value is the magnitude of vector W:\n",pFile);
   WriteDoubleToFile(magW,1,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the unit vector of V to the file
   const Vec3 vUnitVec = v / magV;
   WriteStringToFile("The following 3 numbers are the nx,ny,nz components of the unit vector V:\n",pFile);
   WriteVec3ToFile(vUnitVec,6,pFile);
   WriteStringToFile("\n\n",pFile);

   //Defining and writing the angle between the vectors V and W file
   double angleBtwnVWinRadians = acos(dotVW/(magV * magW));
   WriteStringToFile("The following scalar value the angle between vector v and vector w in radians W:\n",pFile);
   WriteDoubleToFile(angleBtwnVWinRadians,6,pFile);
   WriteStringToFile("\n\n",pFile);
   
   //Defining and writing the angle between the vectors V and W in degrees to file
   double angleBtwnVWinDegrees=ConvertFromRadiansToDegrees(angleBtwnVWinRadians);
   WriteStringToFile("The following scalar value the angle between vector v and vector w in degrees W:\n",pFile);
   WriteDoubleToFile(angleBtwnVWinDegrees,1,pFile);
   WriteStringToFile("\n\n",pFile);

   // Defining and writing the 3X3 Matrix that is the result of the outer product of vectors v and w to file
   Mat33 VWouter = outer(v,w);
   WriteStringToFile("The following 3 by 3 matrix is a result of the outer product of vector v and vector w:\n",pFile);
   WriteMat33ToFile(VWouter,1,pFile);
   WriteStringToFile("\n",pFile);

   // Defining and writing the 3X3 matrix that is a result of the outer product of vectors w and v to file
   Mat33 WVouter = outer(w,v);
   WriteStringToFile("The following 3 by 3 matrix is a result of the outer product of vector w and vector v:\n",pFile);
   WriteMat33ToFile(WVouter,1,pFile);
   WriteStringToFile("\n",pFile);
   
   // A normal program exit returns 0 (other return values usually signal an error)
   return 0;
}


//-----------------------------------------------------------------------------
bool    WriteVec3ToFile( const Vec3 &v, 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 = 15;
   int fieldWidth = precision + 8;


   //Extracing the nx, ny, nz component from the Vec3 for printing
   Real AnyVectorNx=v[0];
   Real AnyVectorNy=v[1];
   Real AnyVectorNz=v[2];

   for (unsigned int i=0; i<3; i++)
   {
      // Create the format specifier and print the number
      char format[20];
      sprintf( format, " %%- %d.%df", fieldWidth, precision );
   
      //Returns a error message if unable to write double
      fprintf( fptr, format, v[i]);
   }
return 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.%df", fieldWidth, precision );
   return fprintf( fptr, format, x ) >= 0;
}

//___________________________________________________________________________________________

bool    WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   // Initializes variables that will establish the size of the matrix
   // a) Number of rows
   // b) Number of columns
   // c) Number of total elements
   const unsigned int numberOfRows = 3;
   const unsigned int numberOfCols = 3;
   const unsigned int numberOfElements = numberOfRows * numberOfCols;
   
   // double array used for storing the 9 scalar values
   double M[ numberOfElements ];
   
   // for loop that will cycle through and extract the nine numbers
   unsigned int i=0;
   for(unsigned int x=0; x<3; x++)
   {
      for (unsigned int y=0; y<3; y++)
      {
         M[i] = m[x][y];
         i=i+1;
      }
   }
   //Calls function that writes a double value to a specified file
   WriteDoubleMatrixElementsToFile(M,3,3,1,fptr);
      return 0;
}


//-----------------------------------------------------------------------------
bool  WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr )
{
   
   // for loop that will cycle through and write elements of row elements of a matrix to a file
   for( unsigned int i=0;  i<numberOfCols;  i++ )
   {
      if( !WriteDoubleToFile( array[i], precision, fptr ) ) return false;
   }
   return true;
}


//-----------------------------------------------------------------------------
bool  WriteDoubleMatrixElementsToFile( const double *array, unsigned int numberOfRows, unsigned int numberOfCols, int precision, FILE *fptr )
{
   // for loop that will call the function that will write rows of a matrix to a file x times (x is the number of columns in a matrix)
   for( unsigned int i=0;  i<numberOfRows;  i++ )
   {
      const double *rowArray = &array[ i * numberOfCols ];
      if( !WriteDoubleRowElementsToFile( rowArray, numberOfCols, precision, fptr ) ) return false;
      WriteStringToFile( "\n", fptr );
   }
   return true;
}


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