#include <iostream>
#include <fstream>
#include <iomanip>
#include "Constants.h"
#include <math.h>
#include "vtr.h"

using namespace std;

vtr::vtr(fp_t xval, fp_t yval, fp_t zval){
  x = xval;
  y = yval;
  z = zval;
}

void vtr::reset(){
  x = y = z = 0.0;
}

void vtr::negate(){
  x = -x;
  y = -y;
  z = -z;
}

void vtr::setvtr(fp_t xval, fp_t yval, fp_t zval){
  x = xval;
  y = yval;
  z = zval;
}

void vtr::addvtr(fp_t xval, fp_t yval, fp_t zval){
  x += xval;
  y += yval;
  z += zval;
}

void vtr::subvtr(fp_t xval, fp_t yval, fp_t zval){
  x -= xval;
  y -= yval;
  z -= zval;
}

fp_t vtr::getx() const {return x;}
fp_t vtr::gety() const {return y;}
fp_t vtr::getz() const {return z;}

vtr vtr::operator+(const vtr &operand2) const{
  vtr sum;
  sum.setvtr(x + operand2.x, y + operand2.y, z + operand2.z);
  return sum;
}

vtr vtr::operator-(const vtr &operand2) const{
  vtr sub;
  sub.setvtr(x - operand2.x, y - operand2.y, z - operand2.z);
  return sub;
}

vtr vtr::operator*(const vtr &operand2) const{
  vtr product;
  fp_t xval, yval, zval;

  xval = y * operand2.z - z * operand2.y;
  yval = -(x * operand2.z - z * operand2.x);
  zval = x * operand2.y - y * operand2.x;
  product.setvtr(xval, yval, zval);

  return product;
}

vtr vtr::operator*(const fp_t &val) const{
  vtr product;
  product.setvtr(x * val, y * val, z * val);
  return product;
}

vtr vtr::operator/(const fp_t &val) const{
  vtr division;
  division.setvtr(x / val, y / val, z / val);
  return division;
}

vtr &vtr::operator=(const vtr &right){
  x = right.x;
  y = right.y;
  z = right.z;

  return *this;
}

fp_t dotP(vtr v1, vtr v2){
  fp_t product;
  product = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
  return product;
}

void vtr::unitvtr(){
  fp_t magnitude;

  magnitude = x * x + y * y + z * z;
  magnitude = sqrt(magnitude);
  x = x / magnitude;
  y = y / magnitude;
  z = z / magnitude;
}

int vtr::operator==(const vtr &right) const{
  fp_t xr = right.getx();
  if (fabs(x - xr) > ZERO_10) 
    return NO;
  xr = right.gety();
  if (fabs(y - xr) > ZERO_10) 
    return NO;
  xr = right.getz();
  if (fabs(z - xr) > ZERO_10) 
    return NO;
  return YES;
}

int vtr::operator!=(const vtr &right) const{
  fp_t xr = right.getx();
  if (fabs(x - xr) > ZERO_10) 
    return YES;
  xr = right.gety();
  if (fabs(y - xr) > ZERO_10) 
    return YES;
  xr = right.getz();
  if (fabs(z - xr) > ZERO_10) 
    return YES;
  return NO;
}

int vtr::operator>(const vtr &right) const{
  fp_t r = right.getx();
  if (x > r + ZERO_10) 
    return YES;
  if (x < r - ZERO_10) 
    return NO;
  r = right.gety();
  if (y > r + ZERO_10) 
    return YES;
  if (y < r - ZERO_10) 
    return NO;
  r = right.getz();
  if (z > r + ZERO_10)  
    return YES;
  if (z < r - ZERO_10) 
    return NO;
  return NO;
}

int vtr::operator<(const vtr &right) const{
  fp_t r = right.getx();
  if (x > r + ZERO_10) 
    return NO;
  if (x < r - ZERO_10) 
    return YES;
  r = right.gety();
  if (y > r + ZERO_10) 
    return NO;
  if (y < r - ZERO_10) 
    return YES;
  r = right.getz();
  if (z > r + ZERO_10) 
    return NO;
  if (z < r - ZERO_10) 
    return YES;
  return NO;  
}

void vtr::Scale(fp_t scale){
  fp_t mag;

  mag = magnitude();
  mag = scale / mag;

  x *= mag;
  y *= mag;
  z *= mag;
}

void vtr::print(){
  cout << "(" << x << ", " << y << ", " << z << ")" << endl;
}

fp_t vtr::magSquare(){
  return (x * x + y * y + z * z);
}

fp_t vtr::magnitude(){
  fp_t mag;

  mag = x * x + y * y + z * z;
  mag = sqrt(mag);

  return mag;
}

fp_t Length(vtr vv){return vv.magnitude();}

fp_t Determinant(vtr a, vtr b, vtr c){
  return (a.x * (b.y * c.z - b.z * c.y) - 
          a.y * (b.x * c.z - c.x * b.z) +
          a.z * (b.x * c.y - c.x * b.y));
}


fp_t findArea(vtr *ndArray, int ndNum){
  int i;
  fp_t aera=0.0;
  for (i = 0; i < ndNum - 1; i++) 
    aera += triArea(ndArray[0], ndArray[(i + 1) % ndNum], ndArray[(i + 2) % ndNum]);

  return aera; 
}

fp_t triArea(vtr nd0, vtr nd1, vtr nd2){
  fp_t area;
  vtr t0 = nd2 - nd1;
  vtr t1 = nd0 - nd2;
  vtr normal = t0 * t1;
  area = 0.5 * normal.magnitude();
  return area;
}