#ifndef ALGEBRA_H
#define ALGEBRA_H

#include <math.h>

typedef float real;

#ifndef M_PI
#define M_PI       3.14159265358979323846264338
#endif

class Vector3DW
{
public:

  Vector3DW()
  {
	x_ = 0;
	y_ = 0;
	z_ = 0;
	w_ = 1;
	norm_ = 0;
  };

  Vector3DW(real x, real y, real z)
  {
	x_ = x;
	y_ = y;
	z_ = z;
	w_ = 1;
	norm_ = 0;
  };

  Vector3DW(real x, real y, real z, real w)
  {
	x_ = x;
	y_ = y;
	z_ = z;
	w_ = w;
	norm_ = 0;
  };

  Vector3DW(real x, real y, real z, real w, real norm)
  {
	x_ = x;
	y_ = y;
	z_ = z;
	w_ = w;
	norm_ = norm;
  };

  Vector3DW operator+(Vector3DW vec2)
  {
	return Vector3DW(x_+vec2.x_, y_+vec2.y_, z_+vec2.z_);
  };

  Vector3DW operator-(Vector3DW vec2)
  {
	return Vector3DW(x_-vec2.x_, y_-vec2.y_, z_-vec2.z_);
  };

  real operator*(Vector3DW vec2)
  {
	return (x_*vec2.x_ + y_*vec2.y_ + z_*vec2.z_);
  };

  Vector3DW operator*(real a)
  {
	return Vector3DW(x_*a, y_*a, z_*a, w_);
  };

  Vector3DW operator%(Vector3DW vec2)
  {
	return Vector3DW(y_*vec2.z_ - z_*vec2.y_, z_*vec2.x_ - x_*vec2.z_, x_*vec2.y_ - y_*vec2.x_);
  };

  real getX() { return x_; };
  real getY() { return y_; };
  real getZ() { return z_; };
  real getW() { return w_; };

  Vector3DW getCopy()
  {
	return Vector3DW(x_, y_, z_, w_, norm_);
  };

  void getCoords(real& x, real& y, real& z) 
  { 
	x = x_; 
	y = y_; 
	z = z_; 
  };

  void setX(real x) { x_ = x; norm_ = 0; };
  void setY(real y) { y_ = y; norm_ = 0; };
  void setZ(real z) { z_ = z; norm_ = 0; };
  void setW(real w) { w_ = w; };

  real getNorm()
  {
	if(norm_==0) norm_ = (real)sqrt(x_*x_ + y_*y_ + z_*z_);
	return norm_;
  };

  void normalize()
  {
	if(norm_==0) getNorm();
	x_ = x_ / norm_;
	y_ = y_ / norm_;
	z_ = z_ / norm_;
	norm_ = (real)1;
  };

  Vector3DW projectOnto(Vector3DW &vec)
  {
	return vec*((getCopy()*vec)/(vec*vec));
  };

  Vector3DW reflectAround(Vector3DW &vec)
  {
	return getCopy()+(projectOnto(vec)-getCopy())*(real)2;
  };

protected:
  real x_;
  real y_;
  real z_;
  real w_;

  real norm_;
};

class Matrix4x4
{
public:

  Matrix4x4()
  {
	for(int i=0; i<16; i++) a[i] = 0;
  };

  Matrix4x4(real a11, real a12, real a13, real a14,
			real a21, real a22, real a23, real a24,
			real a31, real a32, real a33, real a34,
			real a41, real a42, real a43, real a44)
  {
	a[0] = a11;
	a[1] = a21;
	a[2] = a31;
	a[3] = a41;
	a[4] = a12;
	a[5] = a22;
	a[6] = a32;
	a[7] = a42;
	a[8] = a13;
	a[9] = a23;
	a[10] = a33;
	a[11] = a43;
	a[12] = a14;
	a[13] = a24;
	a[14] = a34;
	a[15] = a44;
  };

  real operator[](int i)
  {
	return a[i];
  };

  Matrix4x4 operator*(Matrix4x4 mat2)
  {
	return Matrix4x4(
		a[0]*mat2[0]+a[4]*mat2[1]+a[8]*mat2[2]+a[12]*mat2[3],
		a[0]*mat2[4]+a[4]*mat2[5]+a[8]*mat2[6]+a[12]*mat2[7],
		a[0]*mat2[8]+a[4]*mat2[9]+a[8]*mat2[10]+a[12]*mat2[11],
		a[0]*mat2[12]+a[4]*mat2[13]+a[8]*mat2[14]+a[12]*mat2[15],
		a[1]*mat2[0]+a[5]*mat2[1]+a[9]*mat2[2]+a[13]*mat2[3],
		a[1]*mat2[4]+a[5]*mat2[5]+a[9]*mat2[6]+a[13]*mat2[7],
		a[1]*mat2[8]+a[5]*mat2[9]+a[9]*mat2[10]+a[13]*mat2[11],
		a[1]*mat2[12]+a[5]*mat2[13]+a[9]*mat2[14]+a[13]*mat2[15],
		a[2]*mat2[0]+a[6]*mat2[1]+a[10]*mat2[2]+a[14]*mat2[3],
		a[2]*mat2[4]+a[6]*mat2[5]+a[10]*mat2[6]+a[14]*mat2[7],
		a[2]*mat2[8]+a[6]*mat2[9]+a[10]*mat2[10]+a[14]*mat2[11],
		a[2]*mat2[12]+a[6]*mat2[13]+a[10]*mat2[14]+a[14]*mat2[15],
		a[3]*mat2[0]+a[7]*mat2[1]+a[11]*mat2[2]+a[15]*mat2[3],
		a[3]*mat2[4]+a[7]*mat2[5]+a[11]*mat2[6]+a[15]*mat2[7],
		a[3]*mat2[8]+a[7]*mat2[9]+a[11]*mat2[10]+a[15]*mat2[11],
		a[3]*mat2[12]+a[7]*mat2[13]+a[11]*mat2[14]+a[15]*mat2[15]);
  };

  Vector3DW operator*(Vector3DW vec)
  {
	return Vector3DW(
		a[0]*vec.getX()+a[4]*vec.getY()+a[8]*vec.getZ()+a[12]*vec.getW(),
		a[1]*vec.getX()+a[5]*vec.getY()+a[9]*vec.getZ()+a[13]*vec.getW(),
		a[2]*vec.getX()+a[6]*vec.getY()+a[10]*vec.getZ()+a[14]*vec.getW(),
		a[3]*vec.getX()+a[7]*vec.getY()+a[11]*vec.getZ()+a[15]*vec.getW());
  };

  real getElement(int i, int j)
  {
	return a[4*(j-1)+(i-1)];
  }

protected:
  real a[16];
};

class matrixFactory
{
public:

  static Matrix4x4 makeIdentityMatrix()
  {
	return Matrix4x4(1, 0, 0, 0,
					 0, 1, 0, 0,
					 0, 0, 1, 0,
					 0, 0, 0, 1);
  };

  static Matrix4x4 makeTranslationMatrix(real dx, real dy, real dz)
  {
	return Matrix4x4(1, 0, 0, dx,
					 0, 1, 0, dy,
					 0, 0, 1, dz,
					 0, 0, 0, 1);
  };

  static Matrix4x4 makeScalingMatrix(real sx, real sy, real sz)
  {
	return Matrix4x4(sx, 0 , 0 , 0,
					 0 , sy, 0 , 0,
					 0 , 0 , sz, 0,
					 0 , 0 , 0 , 1);
  };

  static Matrix4x4 makeRotationMatrix(real angle, real ex, real ey, real ez)
  {
	real norm = (real) sqrt(ex*ex + ey*ey + ez*ez);
	angle = (angle* (real) M_PI)/180;

	if(norm >= 1e-9) 
	{
	  real cos_a, sin_a;
	  cos_a = (real) cos(angle);
	  sin_a = (real) sin(angle);
	  ex /= norm; ey /= norm; ez /= norm;

	  return Matrix4x4(cos_a+(1-cos_a)*ex*ex, ex*ey*(1-cos_a)-ez*sin_a, ez*ex*(1-cos_a)+ey*sin_a, 0,
					   ex*ey*(1-cos_a)+ez*sin_a, cos_a+(1-cos_a)*ey*ey, ey*ez*(1-cos_a)-ex*sin_a, 0,
					   ex*ez*(1-cos_a)-ey*sin_a, ey*ez*(1-cos_a)+ex*sin_a, cos_a+(1-cos_a)*ez*ez, 0,
					   0, 0, 0, 1);
	} else {
	  return makeIdentityMatrix();
	}
  };

};

#endif