//-----------------------------------------------------------------------------
// File:     HelloVector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Vector Calculations
// Author:   Sanjay Dastoor - May 2, 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; }


int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] )
{
	FILE *outputFile = FileOpenWithMessageIfCannotOpen("HelloVector.txt","w");
	
	Vec3 v(2,3,4);
	Vec3 w(5,-6,7);

	//Compute desired values based on vectors v and w

	Vec3 v10 = 10*v;
	Vec3 vDiv10 = v/10;
	Vec3 sumvw = v+w;
	Vec3 diffvw = v-w;
	Real dotvw = dot(v,w);
	Vec3 crossvw = cross(v,w);
	Vec3 crosswv = cross(w,v);
	Real vv = dot(v,v);
	Vec3 vvv = dot(v,v)*v;
	Real normv = sqrt(vv);
	Real normw = sqrt(dot(w,w));
	Vec3 vUnit = v/normv;
	Real anglevw_rad = acos(dotvw/(normv*normw));
	Real anglevw_deg = anglevw_rad*180/3.14;
	Mat33 dyadicvw = outer(v,w);
	Mat33 dyadicwv = outer(w,v);

	//Write computed values to output file

	WriteStringToFile("10v = \n",outputFile);
	WriteVec3ToFile(v10,3,outputFile);
	
	WriteStringToFile("\n\nv/10 = \n",outputFile);
	WriteVec3ToFile(vDiv10,3,outputFile);
	
	WriteStringToFile("\n\nv+w = \n",outputFile);
	WriteVec3ToFile(sumvw,3,outputFile);
	
	WriteStringToFile("\n\nv-w = \n",outputFile);
	WriteVec3ToFile(diffvw,3,outputFile);

	WriteStringToFile("\n\ndot(v,w) = \n",outputFile);
	WriteDoubleToFile(dotvw,3,outputFile);

	WriteStringToFile("\n\ncross(v,w) = \n",outputFile);
	WriteVec3ToFile(crossvw,3,outputFile);

	WriteStringToFile("\n\ncross(w,v) = \n",outputFile);
	WriteVec3ToFile(crosswv,3,outputFile);

	WriteStringToFile("\n\nv^2 = \n",outputFile);
	WriteDoubleToFile(vv,3,outputFile);

	WriteStringToFile("\n\nv^3 = \n",outputFile);
	WriteVec3ToFile(vvv,3,outputFile);

	WriteStringToFile("\n\n|v| = \n",outputFile);
	WriteDoubleToFile(normv,3,outputFile);

	WriteStringToFile("\n\n|w| = \n",outputFile);
	WriteDoubleToFile(normw,3,outputFile);

	WriteStringToFile("\n\nUnitvector(v) = \n",outputFile);
	WriteVec3ToFile(vUnit,3,outputFile);

	WriteStringToFile("\n\nAngle from v to w (rad) = \n",outputFile);
	WriteDoubleToFile(anglevw_rad,3,outputFile);

	WriteStringToFile("\n\nAngle from v to w (deg) = \n",outputFile);
	WriteDoubleToFile(anglevw_deg,3,outputFile);

	WriteStringToFile("\n\nv*w = \n",outputFile);
	WriteMat33ToFile(dyadicvw,3,outputFile);

	WriteStringToFile("\n\nw*v = \n",outputFile);
	WriteMat33ToFile(dyadicwv,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.%dg", 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;
}