#include "MathFloat.h"

float VectorDotProduct(int length, float *vector1, float *vector2 )
{
float dotProduct = 0;
int i;

for (i = 0; i < length; i++)
{
dotProduct += vector1[i] * vector2[i];
}

return dotProduct;
}


////////////////////////////////////////////////////////////////////////////////
// Vector Cross Product
// Compute the cross product of vectors a and b with length 3
// Place result in vector C
//
// Call as: vectorDotProduct(c, a, b)
////////////////////////////////////////////////////////////////////////////////

void VectorCrossProduct(float *vectorC, float *vectorA, float *vectorB)
{
vectorC[0] = (vectorA[1] * vectorB[2]) - (vectorA[2] * vectorB[1]);
vectorC[1] = (vectorA[2] * vectorB[0]) - (vectorA[0] * vectorB[2]);
vectorC[2] = (vectorA[0] * vectorB[1]) - (vectorA[1] * vectorB[0]);
}

////////////////////////////////////////////////////////////////////////////////
// Multiply a vector by a scalar
// Mulitply vector a with length m by a scalar
// Place result in vector b
//
// Call as: vectorScale(m, b, a, scalar)
////////////////////////////////////////////////////////////////////////////////

void VectorScale(int length, float *scaledVector, float *inputVector, float scalar)
{
int i;
for (i = 0; i < length; i++)
{
scaledVector[i] = inputVector[i] * scalar;
}
}

////////////////////////////////////////////////////////////////////////////////
// Compute sum of 2 vectors
// Add vector a to vector b, both of length m
// Place result in vector c
//
// Call as: vectorAdd(m, c, b, a)
////////////////////////////////////////////////////////////////////////////////

void VectorAdd(int length, float* vectorC, float* vectorA, float *vectorB)
{
int i;
for(i = 0; i < length; i++)
{
vectorC[i] = vectorA[i] + vectorB[i];
}
}

////////////////////////////////////////////////////////////////////////////////
// Compute difference   of 2 vectors
// Subtract  vector a to vector b, both of length m
// Place result in vector c
//
// Call as: vectorAdd(m, c, b, a)
////////////////////////////////////////////////////////////////////////////////

void VectorSubtract(int length, float* vectorC, float* vectorA, float *vectorB)
{
int i;
for(i = 0; i < length; i++)
{
vectorC[i] = vectorA[i] - vectorB[i];
}
}


////////////////////////////////////////////////////////////////////////////////
// Matrix Multiply
// Multiply matrix A times matrix B, matrix A dimension m x n, matrix B dimension n x p
// Result placed in matrix C, dimension m x p
//
// Call as: matrixMultiply(m, n, p, C, A, B)
////////////////////////////////////////////////////////////////////////////////

void MatrixMultiply(int aRows, int aCols_bRows, int bCols, float *matrixC, float* matrixA, float* matrixB)
{
int i,j,k;
for (i = 0; i < aRows * bCols; i++)
  {
    matrixC[i] = 0.0;
  }
for (i = 0; i < aRows; i++)
{
for(j = 0; j < aCols_bRows; j++)
{
for(k = 0; k < bCols; k++)
{
matrixC[i * bCols + k] += matrixA[i * aCols_bRows + j] * matrixB[j * bCols + k];
}
}
}
}



////////////////////////////////////////////////////////////////////////////////
// Matrix Addition
// Add matrix A to matrix B, dimensions m x n
// Result placed in matrix C, dimension m x n
//
// Call as: matrixAdd(m, n, C, A, B)
////////////////////////////////////////////////////////////////////////////////

void MatrixAdd(int rows, int cols, float* matrixC, float* matrixA, float* matrixB)
{
int i;
for (i = 0; i < rows * cols; i++)
{
matrixC[i] = matrixA[i] + matrixB[i];
}
}

////////////////////////////////////////////////////////////////////////////////
// Matrix Subtraction
// Subtract matrix A from matrix B, dimensions m x n
// Result placed in matrix C, dimension m x n
//
// Call as: matrixSubtract(m, n, C, A, B)
////////////////////////////////////////////////////////////////////////////////

void MatrixSubtract(int rows, int cols, float *matrixC, float* matrixA, float* matrixB)
{
int i;
for (i = 0; i < rows * cols; i++)
{
matrixC[i] = matrixA[i] - matrixB[i];
}
}


////////////////////////////////////////////////////////////////////////////////
// Matrix Scaling
// Scale matrix A, dimensions m x n, by a scaler, S
// Result placed in matrix C, dimension m x n
//
// Call as: matrixScale(m, n, C, S, B)
////////////////////////////////////////////////////////////////////////////////

void MatrixScale(int rows, int cols, float* matrixC, float scaler, float* matrixA)
{
int i;
for (i= 0; i < rows * cols; i++)
{
matrixC[i] = scaler * matrixA[i];
}
}

////////////////////////////////////////////////////////////////////////////////
// 3 x 3 Matrix Transpose
// Compute 3 x 3 Transpose of A
// Result placed in matrix C, dimension 3 x 3
//
// Call as: Transpose3x3(C, A)
////////////////////////////////////////////////////////////////////////////////

void MatrixTranspose3x3(float * matrixC, float* matrixA)
{
matrixC[0] = matrixA[0];
matrixC[1] = matrixA[3];
matrixC[2] = matrixA[6];
matrixC[3] = matrixA[1];
matrixC[4] = matrixA[4];
matrixC[5] = matrixA[7];
matrixC[6] = matrixA[2];
matrixC[7] = matrixA[5];
matrixC[8] = matrixA[8];
}




// Alternate method to calculate arctangent from: http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
float ArcTan2(float y, float x) 
{
float coeff_1 = PI/4;
float coeff_2 = 3*coeff_1;
float abs_y = fabs(y)+1e-10; // kludge to prevent 0/0 condition
float r, angle;

if (x >= 0) 
{
r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
}
else 
{
r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
if (y < 0)
{
return(-angle); // negate if in quad III or IV
}
else
{
return(angle);
}
}





