#include "SFVec3f.h"
#include "SFPoint3f.h"
#include "SFString.h"

#include <sstream>

using namespace std;

namespace X3DTK {

const SFVec3f SFVec3f::null(0.0f, 0.0f, 0.0f);

// constructors, destructor
SFVec3f::SFVec3f() 
: x(0.0f), y(0.0f), z(0.0f)
{
}

SFVec3f::SFVec3f(float x, float y, float z)
{
  this->x = x;
  this->y = y;
  this->z = z;
}

SFVec3f::SFVec3f(const SFVec3f &v)
{
  x = v.x;
  y = v.y;
  z = v.z;
}

SFVec3f &SFVec3f::operator= (const SFVec3f &v)
{
  x = v.x;
  y = v.y;
  z = v.z;
  return *this;
}

SFVec3f &SFVec3f::operator+= (const SFVec3f &v)
{
  x += v.x;
  y += v.y;
  z += v.z;
  return *this;
}

SFVec3f &SFVec3f::operator-= (const SFVec3f &v)
{
  x -= v.x;
  y -= v.y;
  z -= v.z;
  return *this;
}

SFVec3f::SFVec3f(const SFString &s)
{
  istringstream iss(s, istringstream::in);
  iss >> x >> y >> z;
}

SFVec3f::SFVec3f(const SFPoint3f &P)
{
  x = P.x;
  y = P.y;
  z = P.z;
}

// operations on vectors
//overloading of the operators +, -, *
bool operator== (const SFVec3f &v1, const SFVec3f &v2)
{
  return ((v1.x == v2.x) && (v1.y == v2.y) && (v1.z == v2.z));
}

bool operator!= (const SFVec3f &v1, const SFVec3f &v2)
{
  return ((v1.x != v2.x) || (v1.y != v2.y) || (v1.z != v2.z));
}

SFVec3f operator+ (const SFVec3f &v1, const SFVec3f &v2)
{
  SFVec3f res;
  res.x = v1.x + v2.x;
  res.y = v1.y + v2.y;
  res.z = v1.z + v2.z;
  return res;
}

SFVec3f operator- (const SFVec3f &v1, const SFVec3f &v2)
{
  SFVec3f res;
  res.x = v1.x - v2.x;
  res.y = v1.y - v2.y;
  res.z = v1.z - v2.z;
  return res;
}

SFVec3f operator- (const SFVec3f &v)
{
  SFVec3f res;
  res.x = -v.x;
  res.y = -v.y;
  res.z = -v.z;
  return res;
}

//scalar product
float operator* (const SFVec3f &v1, const SFVec3f &v2)
{
  return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}

SFVec3f operator* (const float a, const SFVec3f &v)
{
  SFVec3f res;
  res.x = a * v.x;
  res.y = a * v.y;
  res.z = a * v.z;
  return res;
}

//crossproduct 
SFVec3f crossprod(const SFVec3f &v1, const SFVec3f &v2)
{
  SFVec3f res;
  res.x = v1.y * v2.z - v1.z * v2.y;
  res.y = -v1.x * v2.z + v1.z * v2.x;
  res.z = v1.x * v2.y - v1.y * v2.x;
  return res;
}

//norm2 
float SFVec3f::norm() const
{
  return (float)sqrt(x * x + y * y + z * z);
}

SFVec3f SFVec3f::normalize()
{
  float r = sqrtf(x * x + y * y + z * z);
  if (r != 0.0f)
  {
    x /= r;
    y /= r;
    z /= r;
  }
  return *this;
}

SFVec3f SFVec3f::normalized() const
{
  float r = sqrtf(x * x + y * y + z * z);
  if (r != 0.0f)
    return SFVec3f(x/r, y/r, z/r);
  return SFVec3f(0.0f, 0.0f, 0.0f);
}

SFString SFVec3f::toSFString() const
{
  return "\"" + SFString::number(x) + SFString(" ") 
              + SFString::number(y) + SFString(" ")
              + SFString::number(z) + "\"";
}

}