//-----------------------------------------------------------------------------
// File:     HelloVector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Performs vector operations and writes result to file.
// Author:   Melanie Fox
// Date:     5/03/07
//-----------------------------------------------------------------------------
// 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.14159265358979
#define INPUT_ANGLE_ARRAY_SIZE 100
#define PRECISION_ARRAY_SIZE 20
#define LOWER_INPUT_ANGLE_LIMIT 180
#define UPPER_INPUT_ANGLE_LIMIT 720
#define DEFAULT_PRECISION_VALUE 5
#define DEFAULT_ANGLE_VALUE 360
#define LOWER_PRECISION_LIMIT 1
#define UPPER_PRECISION_LIMIT 17
#define INPUT_FILENAME "HelloVectorIn.txt"
#define OUTPUT_FILENAME "HelloVectorResults.txt"
#define SIZE_OF_INPUT_STRING 5
#define PRECISION 5
//-----------------------------------------------------------------------------
// Prototypes for local functions (functions not called by code in other files)
//-----------------------------------------------------------------------------
bool GetStringFromFile( char inputString[], unsigned long maxSizeOfString, FILE *fptr )  {return( fgets( inputString, maxSizeOfString, fptr ) );}
bool GetStringFromKeyboard( char inputString[], unsigned long maxSizeOfString )          {return( GetStringFromFile( inputString, maxSizeOfString, stdin ) );}
bool WriteStringToFile( const char outputString[], FILE *fptr )  {return( fprintf( fptr, "%s\n", outputString ) );}
bool WriteStringToScreen( const char outputString[] )            {return( WriteStringToFile( outputString, stdout ) );}
bool WriteDoubleToFile( double x, int precision, FILE *fptr );
bool GetDoubleRowElementsFromFile( double *array, unsigned int numberOfCols, FILE *fptr );
bool GetDoubleMatrixElementsFromFile( double *array, unsigned int numberOfRows, unsigned int numberOfCols, FILE *fptr );
bool WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr );
bool WriteDoubleMatrixElementsToFile( const double *array, unsigned int numberOfRows, unsigned int numberOfCols, int precision, FILE *fptr );
const char* ConvertStringToDouble( const char *s, double &returnValue, double defaultValue );
const char* ConvertStringToLong( const char *s, long &returnValue, long defaultValue );
double ConvertFromRadiansToDegrees( double angleInRadians ) {return( angleInRadians*180.0/PI );}
double ConvertFromDegreesToRadians( double angleInDegrees ) {return( angleInDegrees*PI/180.0 );}
FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
void AddMatrices( const double *arrayA, const double *arrayB, double *arraySum, unsigned int numberOfElements );

bool WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );
bool WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );
//-----------------------------------------------------------------------------
int  main( void )
{
   //Open file containing input matrix
   Vec3 v(2,3,4);
   Vec3 w(5,-6,7);

   //Open output file 
   const char outputFile[50] = OUTPUT_FILENAME;
   const char fileAttributeOut[2] = "w";
   FILE *pointerToFile = FileOpenWithMessageIfCannotOpen( outputFile, fileAttributeOut );
   if( pointerToFile == NULL ) return 1;

   //Write result to output file
   WriteStringToFile( "\n10v = ", pointerToFile );
   WriteVec3ToFile(   10*v, PRECISION, pointerToFile );
   WriteStringToFile( "\nv*10 = ", pointerToFile );
   WriteVec3ToFile(   v*10, PRECISION, pointerToFile );
   WriteStringToFile( "\nv/10 = ", pointerToFile );
   WriteVec3ToFile(   v/10, PRECISION, pointerToFile );
   WriteStringToFile( "\nv+w = ", pointerToFile );
   WriteVec3ToFile(   v+w, PRECISION, pointerToFile );
   WriteStringToFile( "\nv-w = ", pointerToFile );
   WriteVec3ToFile(   v-w, PRECISION, pointerToFile );
   WriteStringToFile( "\ndotvw = ", pointerToFile );
   double dotvw = dot(v,w);
   WriteDoubleToFile( dotvw, PRECISION, pointerToFile );
   WriteStringToFile( "\ncrossvw = ", pointerToFile );
   WriteVec3ToFile(   cross(v,w), PRECISION, pointerToFile );
   WriteStringToFile( "\ncrosswv = ", pointerToFile );
   WriteVec3ToFile(   cross(w,v), PRECISION, pointerToFile );
   WriteStringToFile( "\nv^2 = ", pointerToFile );
   WriteDoubleToFile( dot(v,v), PRECISION, pointerToFile );
   WriteStringToFile( "\nv^3 = ", pointerToFile );
   double dotvv = dot(v,v);
   WriteVec3ToFile(   v*dotvv, PRECISION, pointerToFile );
   WriteStringToFile( "\nmag(v) = ", pointerToFile );
   double magv = sqrt(pow(v[0],2)+pow(v[1],2)+pow(v[2],2));
   WriteDoubleToFile( magv, PRECISION, pointerToFile );
   double magw = sqrt(pow(w[0],2)+pow(w[1],2)+pow(w[2],2));
   WriteStringToFile( "\nmag(w) = ", pointerToFile );
   WriteDoubleToFile( magw, PRECISION, pointerToFile );
   WriteStringToFile( "\nUnitVector(v) = ", pointerToFile );
   WriteVec3ToFile(   v/magv, PRECISION, pointerToFile );
   WriteStringToFile( "\nangle(v,w) in rad = ", pointerToFile );
   WriteDoubleToFile( acos(dotvw/(magv*magw)), PRECISION, pointerToFile );
   WriteStringToFile( "\nangle(v,w) in deg = ", pointerToFile );
   WriteDoubleToFile( acos(dotvw/(magv*magw))*180.0/PI, PRECISION, pointerToFile );
   WriteStringToFile( "\nv*w = ", pointerToFile );
   WriteMat33ToFile(  outer(v,w), PRECISION, pointerToFile );
   WriteStringToFile( "\nw*v = ", pointerToFile );
   WriteMat33ToFile(  outer(w,v), PRECISION, pointerToFile );

   //Close output file
   fclose(pointerToFile);

   // A normal program exit returns 0 (other return values usually signal an error)
   return 0;
}
//-----------------------------------------------------------------------------
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 = PRECISION;
   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;
}
//-----------------------------------------------------------------------------
const char* ConvertStringToDouble( const char *s, double &returnValue, double defaultValue )
{
   // Default return value (in case the string is not a valid number)
   returnValue = defaultValue;
   // Check if s is a NULL string or "&^%$#" or "abc" or "123huh"
   if( s )
   {
      // Use the standard math function strtod to parse the number
      char *pointerToCharacterAfterNumber = NULL;
      double x = strtod( s, &pointerToCharacterAfterNumber );

      // Ensure the number was not too large (overflow), such as 1.0E+999 or -1.0E-999
      if( errno==ERANGE  &&  x!=0.0 ) return NULL;

      // Ensure the character after the number is a space or '\0', not 'a' or 'z' or ...
      char characterAfterNumber = pointerToCharacterAfterNumber ? *pointerToCharacterAfterNumber : 'z';
      if( characterAfterNumber == '\0' || isspace(characterAfterNumber) )
      {
         returnValue = x;
         return pointerToCharacterAfterNumber;
      }
   }
return NULL;
}
//-----------------------------------------------------------------------------
const char* ConvertStringToLong( const char *s, long &returnValue, long defaultValue )
{
   // Default return value (in case the string is not a valid number)
   returnValue = defaultValue;
   // Check if s is a NULL string or "&^%$#" or "abc" or "123huh"
   if( s )
   {
      // Use the standard math function strtod to parse the number
      char *pointerToCharacterAfterNumber = NULL;
      double x = strtol( s, &pointerToCharacterAfterNumber, 10 );

      // Ensure the number was not too large (overflow), such as 1.0E+999 or -1.0E-999
      if( errno==ERANGE ) return NULL;

      // Ensure the character after the number is a space or '\0', not 'a' or 'z' or ...
      char characterAfterNumber = pointerToCharacterAfterNumber ? *pointerToCharacterAfterNumber : 'z';
      if( characterAfterNumber == '\0' || isspace(characterAfterNumber) )
      {
         returnValue = x;
         return pointerToCharacterAfterNumber;
      }
   }
return NULL;
}
//-----------------------------------------------------------------------------
FILE* FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute )
{
     FILE *pointerToFile = fopen( filename, attribute );
	 //Display error message and return null pointer to main if file cannot be opened
	 if( pointerToFile == NULL)
	 {
       WriteStringToScreen( "File could not be opened." );
		 getchar();
		 return 0;
	 }
	 return pointerToFile;
}
//-----------------------------------------------------------------------------
bool GetDoubleRowElementsFromFile( double *array, unsigned int numberOfCols, FILE *fptr )
{
   // Get the string representing the row from the file (return false on error)
   char inputString[2000];
   if( !GetStringFromFile( inputString, 2000, fptr ) ) return false;

   // Get each double-precision number in the string and put it into M
   const char *ptrToCharacterAfterDouble = inputString;
   for( unsigned int i=0;  i<numberOfCols;  i++ )
   {
       // Each time ConvertStringToDouble returns, it points to the character after the last valid number
       // If the number is not valid, ConvertStringToDouble returns NULL, else put the number into the array
       double numberInString;
       ptrToCharacterAfterDouble = ConvertStringToDouble( ptrToCharacterAfterDouble, numberInString, 0.0 );
       if( !ptrToCharacterAfterDouble ) return false;
       array[i] = numberInString;
   }
   return true;
}
//-----------------------------------------------------------------------------
bool GetDoubleMatrixElementsFromFile( double *array, unsigned int numberOfRows, unsigned int numberOfCols, FILE *fptr )
{
   for( unsigned int i=0;  i<numberOfRows;  i++ )
   {
      double *rowArray = &array[ i * numberOfCols ];
      if( !GetDoubleRowElementsFromFile( rowArray, numberOfCols, fptr ) ) return false;
   }
   return true;
}
//-----------------------------------------------------------------------------
bool WriteDoubleRowElementsToFile( const double *array, unsigned int numberOfCols, int precision, FILE *fptr )
{
   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 each row, write the row then write a carriage return
   for( int i = 0; i < numberOfRows; i++ )
   {
      const double *rowArray = &array[ i * numberOfCols ];
      if( !WriteDoubleRowElementsToFile( rowArray, numberOfCols, precision, fptr ) ) return false;
      WriteStringToFile( "\r", fptr );
   }
   return true;
}
//-----------------------------------------------------------------------------
void AddMatrices( const double *arrayA, const double *arrayB, double *arraySum, unsigned int numberOfElements )
{
   for( int i=0; i<numberOfElements; i++ ) *arraySum++ = *arrayA++ + *arrayB++;
}
//-----------------------------------------------------------------------------
bool WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr )
{
   for( unsigned int i=0;  i<3;  i++ )
   {
      if( !WriteDoubleToFile( v[i], precision, fptr ) ) return false;
   }
   return true;
}
//-----------------------------------------------------------------------------
bool WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr )
{
   //For each row, write the row then write a carriage return
   for( unsigned int j=0; j<3; j++)
   {
      for( unsigned int i=0;  i<3;  i++ )
      {
         if( !WriteDoubleToFile( m[j][i], precision, fptr ) ) return false;//m[i+j*3]
      }
      WriteStringToFile( "\r", fptr );
   }
   return true;
}