/***********************************************************************
 * SimpleVec.cpp
 *
 * Jack W. Crenshaw
 * Embedded Systems Design
 * Dec. 2006

 * This is a very simple set of vector operations done in the old
 * Fortran style. Its value lies in its efficiency.  With the
 * exception of a few asserts, no error checking is done.  The
 * user is expected to know what she's doing.
 *
 * Most importantly, there is no dynamic memory allocation -- ever.
 *
 * The plan is to use these primitive functions as the basis for
 * a proper C++ vector class, but of course they can also be called
 * directly.
 *
 *		 The coding style of these functions has gone through several iterations,
 * driven by a desire to balance efficiency, readability, and ease of use.
 * Recent benchmarks done with Microsoft Visual C++, Version 6.0, have
 * driven the style  * used here.  When compiled using Release mode, the
 * code is very efficient.
 *
 * In previous versions, I have shown two sets of functions: One for
 * n-dimensional, and one specialized for the ubiquitoue 3-vector.  The
 * functions are so short, there is no great size penalty to be paid.
 * In this current version, however, I use default values for the size
 * parameter, to reduce the size of the file.  The user should recognize
 * that this approach precludes most optimizations.  For most efficiency
 * you should reconstruct the 3-vector functions.
 *
 */

#include <iostream>
#include <cmath>
#include <cassert>
#include "SimpleVec.h"

using namespace std;

//************************************************************
// Input a vector from console
void vGet(double a[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 cin >> a[i];
}

// Output a vector to console
void vPut(const double a[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 cout << a[i] << ' ';
		 cout << '\n';
}

// Arithmetic Operators for n-vectors

// Clear a vector
void vClear(double a[ ], const size_t n){
		 vSet(a, 0, n);
}

// Negate a vector
void vNegate(double a[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 a[i] = -a[i];
}

// Set all the elements of a vector to a scalar
void vSet(double a[ ], const double s, const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 a[i] = s;
}

// Multiply a vector by a scalar
void vScale(double a[ ], const double s,const  size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 a[i] *= s;
}

// Copy a vector (b = a)
void vCopy(const double a[ ], double b[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 b[i] = a[i];
}

// Add two vectors (c = a + b)
// Vector c can coincide with a or b
void vAdd(const double a[ ], const double b[ ], double c[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 c[i] = a[i] + b[i];
}

// Subtract two vectors (c = a - b)
// Vector c can coincide with a or b
void vSub(const double a[ ], const double b[ ], double c[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 c[i] = a[i] - b[i];
}

// Return the dot product of two vectors (dot(a, b) = a*b)
// Vector b can coincide with vector a
double vDot(const double a[ ], const double b[ ] , const size_t n){
		 double sum = 0;
		 for(size_t i=0; i<n; ++i)
		 		 sum += a[i] * b[i];
		 return sum;
}

// Take the cross product of two vectors (c = a x b)
// This operation is only defined for 3-vectors.  If n > 3,
// only the first three elements are used.
// c cannot coincide with a or b.
void vCross(const double a[ ], const double b[ ] , double c[ ], const size_t n){
		 assert(n >= 3);
		 assert(c != a);
		 assert(c != b);
		 c[0] = a[1] * b[2] - a[2] * b[1];
		 c[1] = a[2] * b[0] - a[0] * b[2];
		 c[2] = a[0] * b[1] - a[1] * b[0];
}

// Return the norm of a vector
double vNorm(const double a[ ], const size_t n){
		 return sqrt(vDot(a, a, n));
}

// Convert a vector to a unit vector
// (a = a/|a|)
// If a = 0, the vector returned will have a one in
// the first element.
void vUnitize(double a[ ], const size_t n){
		 double magnitude = vNorm(a, n);
		 if(magnitude != 0)
		 		 vScale(a, 1/magnitude, n);
		 else{
		 		 vClear(a, n);
		 		 a[0] = 1;
		 }
}

// comparison operators
// Return true if condition is met
// i.e. vIsLess returns true of a < b
bool vIsEqual(const double a[ ], const double b[ ], const size_t n){
		 for(size_t i=0; i<n; ++i)
		 		 if(a[i] != b[i])
		 		 		 return false;
		 return true;
}


bool vIsUnequal(const double a[ ], const double b[ ], const size_t n){
		 return !vIsEqual(a, b, n);
}


bool vIsLess(const double a[ ], const double b[ ], const size_t n){
		 return (vNorm(a, n) < vNorm(b, n));
}


bool vIsGreater(const double a[ ], const double b[ ], const size_t n){
		 return vNorm(a, n) > vNorm(b, n);
}


bool vIsLessOrEqual(const double a[ ], const double b[ ], const size_t n){
		 return !vIsGreater(a, b, n);
}


bool vIsGreaterOrEqual(const double a[ ], const double b[ ], const size_t n){
		 return !vIsLess(a, b, n);
}
/**********************************************************************
 * SimpleVec.h
 *
 * simple fortran-style vector package (_NOT_ class!)
 *
 *		 Jack W. Crenshaw
 * Embedded Systems Design
 * Dec. 2006
 *
 * This is a very simple set of vector operations done in the old
 * Fortran style.  Its value is that the operations are just about
 * as efficient as it's possible to get, short of coding them in
 * assembly language.  There is no range checking, no parameter
 * checking, no nothing (with the exception of the cross product
 * operator, which does use asserts).  The user is expected to 
 * know what she's doing.
 *
 * Most importantly, there is no dynamic memory allocation -- ever. 
 *
 * These functions are suitable for linking with, and being used by,
 * a more modern C++ vector class.
 *
 */


#pragma once

// Input a vector from console
void vGet(double a[ ], size_t n = 3);

// Output a vector to console
void vPut(const double a[ ], size_t n = 3);

// Clear a vector
void vClear(double a[ ], size_t n = 3);

// Negate a vector
void vNegate(double a[ ], const size_t n = 3);

// Set all the elements of a vector to a scalar
void vSet(double a[ ], const double s, const size_t n = 3);

// Multiply a vector by a scalar
void vScale(double a[ ], const double s, const size_t n = 3);

// Copy a vector (b = a)
void vCopy(const double a[ ], double b[ ], const size_t n = 3);

// Add two vectors (c = a + b)
// Vector c can coincide with a or b
void vAdd(const double a[ ], const double b[ ], double c[ ], const size_t n = 3);

// Subtract two vectors (c = a - b)
// Vector c can coincide with a or b
void vSub(const double a[ ], const double b[ ], double c[ ], const size_t n = 3);

// Return the dot product of two vectors (dot(a, b) = a*b)
// Vector b can coincide with vector a
double vDot(const double a[ ], const double b[ ] , const size_t n = 3);

// Take the cross product of two vectors (c = a x b)
// This operation is only defined for 3-vectors.  If n > 3,
// only the first three elements are used.
// c cannot coincide with a or b.
void vCross(const double a[ ], const double b[ ] , double c[ ], const size_t n = 3);

// Return the norm of a vector
double vNorm(const double a[ ], const size_t n = 3);

// Convert a vector to a unit vector
// (a = a/|a|)
// If a = 0, the vector returned will have a one in
// the first element.
void vUnitize(double a[ ], const size_t n = 3);

// comparison operators
// Return true if condition is met
// i.e. vIsLess returns true if a < b
bool vIsEqual(const double a[ ], const double b[ ], const size_t n = 3);
bool vIsUnequal(const double a[ ], const double b[ ], const size_t n = 3);
bool vIsLess(const double a[ ], const double b[ ], const size_t n = 3);
bool vIsGreater(const double a[ ], const double b[ ], const size_t n = 3);
bool vIsLessOrEqual(const double a[ ], const double b[ ], const size_t n = 3);
bool vIsGreaterOrEqual(const double a[ ], const double b[ ], const size_t n = 3);

/**********************************************************************
 * Vectors.cpp
 *
 * A C++ vector class
 *
 *		 Jack W. Crenshaw
 *  Embedded Systems Design
 *  4/20/07
 *
 * This file defines a C++ class Vector, which includes the functions
 * needed to perform vector algebra. Contrast this with the STL
 * class vector, which does not support the mathematical operations
 * that define vector arithmetic.
 *
 * Revision History
 *
 *		 		 4/20/07		 Created
*/

#include <cassert>
#include "Vectors.h"

using namespace std;

/******************************************************************************
 * Class definitions
 ******************************************************************************/

// default constructor; creates a null vector. INITIALIZE BEFORE USE
Vector::Vector(void)
{
		 sz = 0; p = 0;
}


// copy constructor
Vector::Vector(const Vector & a)
{
		 sz = a.sz;
		 p = new double [sz];
		 vCopy(a.p, p, sz);
}


// create from array
Vector::Vector(const double a[], size_t n)
{
		 sz = n;
		 p = new double [sz];
		 vCopy(a, p, sz);
}


// create from scalar elements
Vector::Vector(const double x, const double y, const double z)
{
		 sz = 3;
		 p = new double [sz];
		 p[0] = x; p[1] = y; p[2] = z;
}


// destructor -- must release storage
Vector::~Vector(){
		 delete [] p;
}


// initialize.  Use this to initialize a null vector
void Vector::Init(size_t size)
{
		 if(sz < size)
		 {
		 		 delete[] p;
		 		 p = new double[size];
		 }
		 sz = size;
		 vClear(p, sz);
}


// Assignment operators.  Use these to assign to an 
// ecisting vector

// set every element to s
Vector & Vector::operator =(double s)
{
		 vSet(p, s, sz);
		 return *this;
}


// assign as copy
Vector & Vector::operator =(const Vector &a)
{
		 if(&a != this)
		 {
		 		 if(sz != a.sz)
		 		 {
		 		 		 sz = a.sz;
		 		 		 delete[] p;
		 		 		 p = new double[sz];
		 		 }
		 		 vCopy(a.p, p, sz);
		 }
		 return *this;
}


// Get the size of a vector
size_t Vector::Size(void)
{
		 return sz;
}


size_t Size(const Vector & a)
{
		 return a.sz;
}


// Access functions -- get element by reference

// Fortran-style indexing (one-based)
double & Vector::operator ()(size_t i)
{
		 assert(i > 0);
		 assert(i <= sz);
		 return p[i-1];
}


// C-style indexing (zero-based)
double & Vector::operator[](size_t i)
{
		 assert(i < sz);
		 return p[i];
}


// stream I/O functions
istream& operator >>(istream &is, Vector &a){
		 for(size_t i=0; i<a.sz; ++i)
		 		 is >> a.p[i];
		 return is;
}


ostream& operator <<(ostream &os, const Vector &a){
		 for(size_t i=0; i<a.sz; ++i)
		 		 os << a.p[i] << ' ';
		 return os;
}


// Comparison operators
bool Vector::operator ==(const Vector &a){
		 assert(sz == a.sz);
		 return vIsEqual(p, a.p, sz);
}


bool Vector::operator !=(const Vector &a){
		 assert(sz == a.sz);
		 return vIsUnequal(p, a.p, sz);
}


bool Vector::operator <(const Vector &a){;
		 assert(sz == a.sz);
		 return vIsLess(p, a.p, sz);
}


bool Vector::operator >(const Vector &a){
		 assert(sz == a.sz);
		 return vIsGreater(p, a.p, sz);
}


bool Vector::operator <=(const Vector &a){
		 assert(sz == a.sz);
		 return vIsLessOrEqual(p, a.p, sz);
}


bool Vector::operator >=(const Vector &a){
		 assert(sz == a.sz);
		 return vIsGreaterOrEqual(p, a.p, sz);
}


// Math operations

// vector magnitude (norm)
double Vector::Norm(void){
		 return vNorm(p, sz);
}


double Norm(const Vector &a){
		 return vNorm(a.p, a.sz);
}


// convert a vector to unit length
Vector & Vector::Unitize(void){
		 vUnitize(p, sz);
		 return *this;
}


Vector Unitize(const Vector &a){
		 Vector retval(a);
		 vUnitize(retval.p, retval.sz);
		 return retval;
}


// negate a in place
Vector & Vector::Negate(void){
		 vNegate(p, sz);
		 return *this;
}


// unary minus
Vector operator -(const Vector &a){
		 Vector retval(a);
		 vNegate(retval.p, retval.sz);
		 return retval;
}


// vector add/subtract in place
// vector add in place
Vector & Vector::operator +=(const Vector &a){
		 vAdd(p, a.p, p, sz);
		 return *this;
}


// vector subtract in place
Vector & Vector::operator -=(const Vector &a){
		 vSub(p, a.p, p, sz);
		 return *this;
}


// vector addition
Vector operator +(const Vector &a, const Vector &b){
		 Vector retval(a);
		 retval += b;
		 return retval;
}


// vector subtraction
Vector operator -(const Vector &a, const Vector &b){
		 Vector retval(a);
		 retval -= b;
		 return retval;
}


// multiply vector by scalar in place
Vector & Vector::operator *=(double s){
		 vScale(p, s, sz);
		 return *this;
}


// divide vector by scalar in place
Vector & Vector::operator /=(double s){
		 assert(s != 0.0);
		 vScale(p, 1/s, sz);
		 return *this;
}


// multiply by scalar
Vector operator *(double s, const Vector &a){
		 Vector retval(a);
		 retval *= s;
		 return retval;
}


// divide by scalar
Vector operator /(const Vector &a, double s){
		 assert(s != 0.0);
		 Vector retval(a);
		 retval /= s;
		 return retval;
}


// dot product
double Vector::operator *(const Vector &a){
		 assert(a.sz == sz);
		 return vDot(p, a.p, sz);
}


double operator *(const Vector &a, const Vector &b){
		 assert(a.sz == b.sz);
		 return vDot(a.p, b.p, a.sz);
}


// cross product
// input vectors may be dimensioned more than 3.
// only the first three elements will be used
Vector operator ^(const Vector &a, const Vector &b)
{
		 assert(a.sz >= 3);
		 assert(b.sz >= 3);
		 double retarray[3];
		 vCross(a.p, b.p, retarray, 3);
		 Vector retval(retarray);
		 return retval;
}
/**********************************************************************
 * Vectors.h
 *
 * A C++ vector class
 *
 *		 Jack W. Crenshaw
 *  Embedded Systems Design
 *  4/20/07
 *
 * This file defines a C++ class Vector, which includes the functions
 * needed to perform vector algebra. Contrast this with the STL
 * class vector, which does not support the mathematical operations
 * that define vector arithmetic.
 *
 * Revision History
 *
 *		 		 4/20/07		 Created
*/

#pragma once

#include <iostream>
#include "SimpleVec.h"

using namespace std;

class Vector{
		 size_t sz;
		 double *p;
public:
		 // Constructors, et al
		 Vector();		 		 // creates a null vector. INITIALIZE BEFORE USE
		 Vector(const Vector &a);		 		 		 		 // copy constructor
		 Vector(const double a[], size_t n = 3);		 // create from array
		 Vector(const double x, const double y,  // create from scalar elements
		 		 const double z = 0.0);
		 ~Vector();		 		 		 		 		 		 		 		 // destructor
		 
		 // initialize.  Use this to initialize a null vector
		 void Init(size_t n = 3);
		 
		 // Assignment operators.  Use these to assign to an 
		 // ecisting vector

		 Vector & operator =(const double s);		 // set every element to s
		 Vector & operator =(const Vector &a);		 // assign as copy

		 // Size functions

		 // get the size of a vector
		 size_t Size(void);
		 friend size_t Size(const Vector & a);

		 // Access functions -- get element by reference
		 double & operator ()(size_t i);		 		 // Fortran-style indexing (one-based) 
		 double & operator [](size_t i);		 		 // C-style indexing (zero-based) 

		 // Stream I/O functions
		 friend ostream& operator <<(ostream &os, const Vector &a);
		 friend istream& operator >>(istream &is, Vector &a);

		 // Comparison operators
		 bool operator ==(const Vector &a);
		 bool operator !=(const Vector &a);
		 bool operator < (const Vector &a);
		 bool operator > (const Vector &a);
		 bool operator <=(const Vector &a);
		 bool operator >=(const Vector &a);

		 // Math operators
		 double Norm(void);
		 friend double Norm(const Vector &a);
		 Vector & Unitize(void);
		 friend Vector Unitize(const Vector & a);
		 Vector & Negate(void);		 		 		 		 		 
		 friend Vector operator -(const Vector & a);
		 
		 // Arithmetic operators
		 // use +=, -=, etc. where feasible. +, -, etc. create temporary
		 Vector & operator +=(const Vector &a);
		 Vector & operator -=(const Vector &a);
		 friend Vector operator +(const Vector &a, const Vector &b);		 
		 friend Vector operator -(const Vector &a, const Vector &b);		 
		 Vector & operator *=(double s);		 		 		 		 
		 Vector & operator /=(double s);		 		 		 		 
		 friend Vector operator *(double s, const Vector &a);
		 friend Vector operator /(const Vector &a, double s);

		 // Dot and cross product.  Cross product operator is ^
		 double operator *(const Vector &a);
		 friend double operator *(const Vector &a, const Vector &b);

		 // Cross product
		 // input vectors may be dimensioned more than 3.
		 // only the first three elements will be used
		 friend Vector operator ^(const Vector &a, const Vector &b);
};
// VecTest.cpp : Defines the entry point for the console application.
//
// Jack W. Crenshaw
// Embedded Systems Design
// 
// Created 07/08/07

#include <iostream>
#include <cassert>
#include "Vectors.h"

using namespace std;

int main(int argc, char* argv[]){
		 // test default constructor
		 Vector(a);
		 // see what happens if we don't init
		 assert(a.Size() == 0);
		 // and what happens if we do
		 a.Init();
		 assert(a.Size() == 3);
		 // what if we re-init
		 a.Init(4);
		 assert(a.Size() == 4);

		 // test access operators
		 assert((a[0] == 0)&&(a[1] == 0)&&(a[2] == 0)&&(a[3] == 0));
		 a(1)=1; a(2)=2; a(3)=3; a(4)=4; 
		 assert((a[0] == 1)&&(a[1] == 2)&&(a[2] == 3)&&(a[3] == 4));

		 // test copy constructor
		 Vector b(a);
		 assert(b.Size() == 4);
		 assert((b[0] == 1)&&(b[1] == 2)&&(b[2] == 3)&&(b[3] == 4));

		 // test construct from array
		 double x[4] = {4, 3, 2, 1};
		 Vector c(x,3);  // note we only use 3 elements
		 assert(c.Size() == 3);
		 assert((c[0]==4)&&(c[1]==3)&&(c[2]==2));

		 // test construct from scalar elements
		 Vector d(3,4);
		 assert(Size(d) == 3);
		 assert((d[0]==3)&&(d[1]==4)&&(d[2]==0));

		 // Test scalar assignment operator (and Norm)
		 a = 0;
		 assert(Norm(a) == 0);

		 // Test vector assignment operator (and ==)
		 a = c;
		 assert(a == c);

		 // Test comparison operators
		 assert(!(a != c));
		 assert(a != d);
		 assert(!(a == d));
		 assert(d < a);
		 assert(!(d >= a));
		 assert(a > d);
		 assert(!(a <= d));
		 assert(a <= a);
		 assert(a >= c);


		 // Test vector norm
		 assert(d.Norm() == 5);
		 assert(Norm(d) == 5);

		 // test Unitize
		 d.Unitize();
		 assert(Norm(d) == 1);
		 Vector e = Unitize(d);
		 assert(Norm(e) == 1);

		 // test negate and unary minus		 
		 a = -c;
		 assert(a != c);
		 c.Negate();
		 assert(a == c);

		 // test in-place addition and subtraction (and scaling)
		 a += c;
		 assert(a/2 == c);
		 assert(0.5*a == c);
		 c *= 2;
		 assert(a == c);
		 c /= 2;
		 a -= c;
		 assert(a == c);

		 // test addition and subtraction
		 b = a + c;
		 assert(b/2 == a);
		 b = b - c;
		 assert(b == a);

		 // test dot and cross product
		 a = -a;
		 c *= -1;
		 c[1] = 1;
		 assert(a*c == 23);
		 d = a^c;
		 assert((d[0] == 4)&&(d[1] == 0)&&(d[2] == -8));

		 return 0;
}