//-----------------------------------------------------------------------------
// File:     HelloVector.cpp
// Class:    None
// Parent:   None
// Children: None
// Purpose:  Takes a vector and prints out various quantities
//-----------------------------------------------------------------------------
// 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  WriteStringToFile(   const char outputString[], FILE *fptr );  
bool  WriteStringToScreen( const char outputString[] )              { return WriteStringToFile( outputString, stdout ); }
bool  WriteDoubleToFile( double x, int precision, FILE *fptr );
FILE*  FileOpenWithMessageIfCannotOpen( const char *filename, const char *attribute );
double  ConvertFromRadiansToDegrees( double angleInRadians ){return angleInRadians*57.295779513082320876798154814105;/*1 radian is approximately 57.3 degrees*/}
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr );
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr );



//-----------------------------------------------------------------------------
// The executable program starts here
//-----------------------------------------------------------------------------
int  main( int numberOfCommandLineArguments, char *arrayOfCommandLineArguments[] ){

   // these are the given vecotrs to do the calculations on
   const Vec3 v( 2,  3, 4);
   const Vec3 w( 5, -6, 7);

   Vec3 vec3result;
   double doubleResult;
   Mat33 mat33result;
   FILE *myFile = FileOpenWithMessageIfCannotOpen( "HelloVectorResults.txt", "w");

   vec3result = 10*v; // calculate 10 times vector v
   WriteStringToFile( "10v = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   doubleResult = sqrt(dot(v,v))*10; // calculate 10 times magnitude of v
   WriteStringToFile( "\n\n\n\n mag(v)*10 = ", myFile);
   WriteDoubleToFile( doubleResult, 8, myFile);

   doubleResult = sqrt(dot(v,v))/10; // calculate magnitude of v over 10
   WriteStringToFile( "\n\n\n\n mag(v)/10 = ", myFile);
   WriteDoubleToFile( doubleResult, 8, myFile);

   vec3result = v + w;
   WriteStringToFile( "\n\n\n\n v + w = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   vec3result = v - w;
   WriteStringToFile( "\n\n\n\n v - w = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   doubleResult =  dot(v,w); // calculate magnitude of v dot w
   WriteStringToFile( "\n\n\n\n v dot w = ", myFile);
   WriteDoubleToFile( doubleResult, 8, myFile);

   
   vec3result = cross(v,w); // calculate v cross w
   WriteStringToFile( "\n\n\n\n v cross w = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   
   vec3result = cross(w, v); // calculate w cross v
   WriteStringToFile( "\n\n\n\n w cross v = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   doubleResult = dot(v,v); //calculate v^2
   WriteStringToFile( "\n\n\n\n v^2 = ", myFile);
   WriteDoubleToFile(doubleResult, 8, myFile);

   
   vec3result = dot(v,v)*v; // calculate v^3
   WriteStringToFile( "\n\n\n\n v^3 = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);

   
   doubleResult = sqrt(dot(v,v)); // calculate mag(v)
   WriteStringToFile( "\n\n\n\n mag(v) = ", myFile);
   WriteDoubleToFile(doubleResult, 8, myFile);

   doubleResult = sqrt(dot(w, w)); // calculate mag(w)
   WriteStringToFile( "\n\n\n\n mag(w) = ", myFile);
   WriteDoubleToFile(doubleResult, 8, myFile);

   vec3result = v/(sqrt(dot(v,v)));  //calculate a unit vector in the direction of v
   WriteStringToFile( "\n\n\n\n UnitVector(v) = ", myFile);
   WriteVec3ToFile(vec3result, 8, myFile);
   
   doubleResult = asin(sqrt(dot(cross(v,w),cross(v,w)))/sqrt(dot(v,v))/sqrt(dot(w,w))); // calculate the angle between v and w
   WriteStringToFile( "\n\n\n\n angle(v,w) in radians = ", myFile);
   WriteDoubleToFile(doubleResult, 8, myFile);

   doubleResult = convertRadiansToDegrees(doubleResult); // convert previous angle to degrees
   WriteStringToFile( "\n\n\n\n angle(v,w) in degrees = ", myFile);
   WriteDoubleToFile(doubleResult, 8, myFile);

   mat33result = v*~w; // calculate outer prod of v and w
   WriteStringToFile( "\n\n\n\n v * w = ", myFile);
   WriteMat33ToFile(mat33result, 8, myFile);

   mat33result= w*~v; // calculate outer prod of w and v
   WriteStringToFile( "\n\n\n\n w * v = ", myFile);
   WriteMat33ToFile(mat33result, 8, myFile);



}



//-------------------------------------------------------------------------------
bool  WriteStringToFile(   const char outputString[], FILE *fptr )  { 
   return fputs( outputString, fptr ) >=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.%dE", fieldWidth, precision );
   return fprintf( fptr, format, x ) >= 0;
}


//-----------------------------------------------------------------------------
bool  WriteVec3ToFile( const Vec3 &v, int precision, FILE *fptr ){

   bool output;
   output = WriteStringToFile( "\n[ ", fptr);
   if(!output)
      return output;
   
   output=WriteDoubleToFile( v[0], precision, fptr);
   if(!output)
      return output;

   output=WriteStringToFile( " , ", fptr);
   if(!output)
      return output;

      output=WriteDoubleToFile( v[1], precision, fptr);
   if(!output)
      return output;

   output=WriteStringToFile( " , ", fptr);
   if(!output)
      return output;

      output=WriteDoubleToFile( v[2], precision, fptr);
   if(!output)
      return output;

   output=WriteStringToFile( " ]", fptr);
   return output;

}





//-----------------------------------------------------------------------------
bool  WriteMat33ToFile( const Mat33 &m, int precision, FILE *fptr ) {

   bool output;
   Row3 row=m[0];
   Vec3 rowTransp=~row;

   output = WriteVec3ToFile(rowTransp, precision, fptr);
   if(!output)
      return output;

   row=m[1];
   rowTransp=~row;

   output = WriteVec3ToFile(rowTransp, precision, fptr);
   if(!output)
      return output;

   row=m[2];
   rowTransp=~row;

   output = WriteVec3ToFile(rowTransp, precision, fptr);
   return output;
      
}