#include "vector.h"
#include "proj.h"

double v3_magnitude(double v[3])
{
  return ( sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]) );
}


/* instead of doing 2 v3_mag, save sqrt ops
	return -1 if 1>2, 0 eq, 1 if 2>1
*/
int v3_v1Compv2(double v1[3], double v2[3])
{

	double v1_mag_sq = v1[X]*v1[X] + v1[Y]*v1[Y] + v1[Z]*v1[Z];
	double v2_mag_sq = v2[X]*v2[X] + v2[Y]*v2[Y] + v2[Z]*v2[Z];

	if (v1_mag_sq == v2_mag_sq) return 0;
	else if (v1_mag_sq < v2_mag_sq) return -1;
	else return 1;

}

void v3_assign(double vTo[3], double vFrom[3])
{
	vTo[X] = vFrom[X];
	vTo[Y] = vFrom[Y];
	vTo[Z] = vFrom[Z];
}


void v3_assign(double vTo[3], double x, double y, double z)
{
	vTo[X] = x;
	vTo[Y] = y; 
	vTo[Z] = z;
}


double *v3_assign(double vFrom[3])
{
	double *vTo = (double *)malloc( sizeof(double) * 3 );
	vTo[X] = vFrom[X];
	vTo[Y] = vFrom[Y];
	vTo[Z] = vFrom[Z];

	return vTo;
}


void v3_crossp(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[Y] * v2[Z] - v1[Z] * v2[Y];
	vres[Y] = -v1[X] * v2[Z] + v1[Z] * v2[X];
	vres[Z] = v1[X] * v2[Y] - v1[Y] * v2[X];

}

double *v3_crossp(double v1[3], double v2[3])
{
  double *vres  = (double *)malloc( sizeof(double) * 3 );

	vres[X] = v1[Y] * v2[Z] - v1[Z] * v2[Y];
	vres[Y] = -v1[X] * v2[Z] + v1[Z] * v2[X];
	vres[Z] = v1[X] * v2[Y] - v1[Y] * v2[X];
	return vres;
}


void v3_dotp(double v1[3], double v2[3], double &a)
{
	a = (v1[X] * v2[X] + v1[Y] * v2[Y] + v1[Z] * v2[Z] );
}


double v3_dotp(double v1[3], double v2[3])
{
  double a = (v1[X] * v2[X] + v1[Y] * v2[Y] + v1[Z] * v2[Z] );

  return a;
}


double v3_cos_bw2v(double v1[3], double v2[3])
{
  return ( (v1[X] * v2[X] + v1[Y] * v2[Y] + v1[Z] * v2[Z] ) / 
		  v3_magnitude(v1) / v3_magnitude(v2) ) ;
}


double v3_sin_bw2v( double v1[3], double v2[3])
{
  double v3[3];
  
  v3_crossp(v1, v2, v3);
  return ( v3_magnitude(v3) /  (v3_magnitude(v1) * v3_magnitude(v2)) );

}


/* vect vprod[3] = v[3] * s */
void v3_scale(double v[3], double s, double vprod[3])
{
	vprod[X] = v[X] * s;
	vprod[Y] = v[Y] * s;
	vprod[Z] = v[Z] * s;
}


/* vect vprod[3] = v[3] * s */
double *v3_scale(double v[3], double s)
{
  double *vprod = (double *)malloc( sizeof(double) * 3 );
  vprod[X] = v[X] * s;
  vprod[Y] = v[Y] * s;
  vprod[Z] = v[Z] * s;

  return vprod;
}


void v3_add(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[X] + v2[X];
	vres[Y] = v1[Y] + v2[Y];
	vres[Z] = v1[Z] + v2[Z];
}


double *v3_add(double v1[3], double v2[3])
{
  
  double *vres = (double *)malloc( sizeof(double) * 3 );
	vres[X] = v1[X] + v2[X];
	vres[Y] = v1[Y] + v2[Y];
	vres[Z] = v1[Z] + v2[Z];
	return vres;
}


void v3_sub(double v1[3], double v2[3], double vres[3])
{

	vres[X] = v1[X] - v2[X];
	vres[Y] = v1[Y] - v2[Y];
	vres[Z] = v1[Z] - v2[Z];
}


double *v3_sub(double v1[3], double v2[3])
{
  double *vres = (double *)malloc( sizeof(double) * 3 );
	vres[X] = v1[X] - v2[X];
	vres[Y] = v1[Y] - v2[Y];
	vres[Z] = v1[Z] - v2[Z];
	return vres;
}


/* in case of tiny mag, make the vect 0 */
void v3_normalize(double v[3], double v_unit[3])
{
	double mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
	double mag_inv;

	if ( (mag > (-NEAR_ZERO)) && (mag < NEAR_ZERO) ) {
		v_unit[X] = 0;
		v_unit[Y] = 0;
		v_unit[Z] = 0;
	}
	else {
		mag_inv = 1 / mag;
		v_unit[X] = v[X] * mag_inv;
		v_unit[Y] = v[Y] * mag_inv;
		v_unit[Z] = v[Z] * mag_inv;
	}
}


double *v3_normalize(double v[3])
{
  double *v_unit = (double *)malloc( sizeof(double) * 3 );
	double mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
	double mag_inv;

	if ( (mag > (-NEAR_ZERO)) && (mag < NEAR_ZERO) ) {
		v_unit[X] = 0;
		v_unit[Y] = 0;
		v_unit[Z] = 0;
	}
	else {
		mag_inv = 1 / mag;
		v_unit[X] = v[X] * mag_inv;
		v_unit[Y] = v[Y] * mag_inv;
		v_unit[Z] = v[Z] * mag_inv;
	}

	return v_unit;
}


/* vect vprod[3] = v[3] * s */
void v4_scale(double v[4], double s, double vres[4])
{
  vres[0] = v[0] * s;
  vres[1] = v[1] * s;
  vres[2] = v[2] * s;
  vres[3] = v[3] * s;
}


void v4_add(double v1[4], double v2[4], double vres[4])
{

  vres[0] = v1[0] + v2[0];
  vres[1] = v1[1] + v2[1];
  vres[2] = v1[2] + v2[2];
  vres[3] = v1[3] + v2[3];

}


void v4_sub(double v1[4], double v2[4], double vres[4])
{

  vres[0] = v1[0] - v2[0];
  vres[1] = v1[1] - v2[1];
  vres[2] = v1[2] - v2[2];
  vres[3] = v1[3] - v2[3];

}

void v4_assign(double vTo[4], double vFrom[4])
{
	vTo[0] = vFrom[0];
	vTo[1] = vFrom[1];
	vTo[2] = vFrom[2];
	vTo[3] = vFrom[3];
}


void v4_assign(double vTo[3], double x, double y, double z, double w)
{
	vTo[X] = x;
	vTo[Y] = y; 
	vTo[Z] = z;
	vTo[W] = w;

}


// set vect to a const
void v4_set(double vTo[4], double c)
{
	vTo[0] = c;
	vTo[1] = c;
	vTo[2] = c;
	vTo[3] = c;
}


/* in case of tiny mag, make the vect 0 */
void v4_normalize(double v[4], double v_unit[4])
{
	double mag = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
	double mag_inv;

	if ( (mag > (-NEAR_ZERO)) && (mag < NEAR_ZERO) ) {
		v_unit[0] = 0;
		v_unit[1] = 0;
		v_unit[2] = 0;
		v_unit[3] = 0;
	}
	else {
		mag_inv = 1 / mag;
		v_unit[0] = v[0] * mag_inv;
		v_unit[1] = v[1] * mag_inv;
		v_unit[2] = v[2] * mag_inv;
		v_unit[3] = v[3] * mag_inv;

	}
}
 

// vector v2 = glmat * v1, glmat is a col major mat
void glmat44_mult_vec41(double *mat, double *v1, double *v2) 
{
	for (int i=0; i<4; i++) 
		v2[i] = mat[i]*v1[0] + mat[i+4]*v1[1] + mat[i+8]*v1[2] + mat[i+12]*v1[3];

}



// vector v2 = mat * v1, mat is regular row major
void mat44_mult_vec41(double *mat, double *v1, double *v2) 
{
	for (int i=0; i<4; i++) 
		v2[i] = mat[i*4]*v1[0] + mat[i*4+1]*v1[1] + mat[i*4+2]*v1[2] + mat[i*4+3]*v1[3];

}