#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "graphicsMath.h"

// calculates the length of v
double VectorLength(Vector_t v) {
  double length = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);

  return(length);
}

// normalizes v to length 1
void VectorNormalize(Vector_t v) {
  double length = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);

  v[0] = v[0] / length;
  v[1] = v[1] / length;
  v[2] = v[2] / length;
  v[3] = 1.0;
}

// sets vector v to be the normalized vector created by x, y, and z
void VectorSetNormalized(double x, double y, double z, Vector_t v) {
  double length = sqrt(x*x + y*y + z*z);

  v[0] = x / length;
  v[1] = y / length;
  v[2] = z / length;
  v[3] = 1.0;
}

// sets v to be vector a crossed with vector b
void VectorCross(Vector_t a, Vector_t b, Vector_t v) {
  Vector_t t;

  t[0] = a[1]*b[2] - a[2]*b[1];
  t[1] = a[2]*b[0] - a[0]*b[2];
  t[2] = a[0]*b[1] - a[1]*b[0];
  t[3] = 1.0;

  v[0] = t[0];
  v[1] = t[1];
  v[2] = t[2];
  v[3] = t[3];
}

// copies from to to
void VectorCopy(Vector_t to, Vector_t from) {
  to[0] = from[0];
  to[1] = from[1];
  to[2] = from[2];
  to[3] = from[3];
}

// initializes a position using homogeneous coordinates
void VectorInit(Vector_t v) {
  v[0] = v[1] = v[2] = 0.0;
  v[3] = 1.0;
}

// makes a homogeneous vector out of x, y, and z
void MakeVector(double x, double y, double z, Vector_t v) {
  v[0] = x;
  v[1] = y;
  v[2] = z;
  v[3] = 1.0;
}

// prints out a vector nicely. Use stdout or stderr to send it to the console.
void VectorPrint(Vector_t v, FILE *fp) {
  fprintf(fp, "\n(%.3f, %.3f, %.3f, %.3f)\n\n", v[0], v[1], v[2], v[3]);
}

// Adds two vectors and returns the result in t = v1 + v2
void VectorAdd(Vector_t v1, Vector_t v2, Vector_t t) {
  t[0] = v1[0] + v2[0];
  t[1] = v1[1] + v2[1];
  t[2] = v1[2] + v2[2];
  t[3] = 1.0;
}

// Subtracts two vectors and returns the result in t = v1 - v2
void VectorSub(Vector_t v1, Vector_t v2, Vector_t t) {
  t[0] = v1[0] - v2[0];
  t[1] = v1[1] - v2[1];
  t[2] = v1[2] - v2[2];
  t[3] = 1.0;
}

// Multiples a vector by a scaler
void VectorMulScalar(Vector_t v, double d) {
  v[0] = v[0]*d;
  v[1] = v[1]*d;
  v[2] = v[2]*d;
  v[3] = 1.0;
}

// returns the dot product of two vectors
double VectorDot(Vector_t v1, Vector_t v2) {
  return(v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]);
}