#include <math.h>

/// @todo TAKE THIS OUT
class Vector
{
  public:
    Vector() {this->x = this->y = this->z = 0; };
    Vector (float x, float y, float z) { this->x=x; this->y = y; this->z = z; };
    inline const Vector& operator/= (float f) {if ((f == 0.0)) {this->x=0;this->y=0;this->z=0;} else {this->x /= f; this->y /= f; this->z /= f;} return *this; };

    float x, y, z;
    inline Vector cross (const Vector& v) const { return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x ); }
    inline float len() const { return sqrt (x*x+y*y+z*z); }
};



class Matrix
{

  public:
    Matrix ( float m11, float m12, float m13,
             float m21, float m22, float m23,
             float m31, float m32, float m33 )
    {
      this->m11 = m11; this->m12 = m12; this->m13 = m13;
      this->m21 = m21; this->m22 = m22; this->m23 = m23;
      this->m31 = m31; this->m32 = m32; this->m33 = m33;
    };

    Matrix operator+ (const Matrix& m) const {
      return Matrix (this->m11 + m.m11, this->m12 + m.m12, this->m13 + m.m13,
                     this->m21 + m.m21, this->m22 + m.m22, this->m23 + m.m23,
                     this->m31 + m.m31, this->m32 + m.m32, this->m33 + m.m33);
    }

    Matrix operator- (const Matrix& m) const {
      return Matrix (this->m11 - m.m11, this->m12 - m.m12, this->m13 - m.m13,
                     this->m21 - m.m21, this->m22 - m.m22, this->m23 - m.m23,
                     this->m31 - m.m31, this->m32 - m.m32, this->m33 - m.m33);
    }

    Matrix operator* (float k) const {
      return Matrix(this->m11 - k, this->m12 - k, this->m13 - k,
                    this->m21 - k, this->m22 - k, this->m23 - k,
                    this->m31 - k, this->m32 - k, this->m33 - k);
    }

    Vector operator* (const Vector& v) const {
      return Vector (this->m11*v.x + this->m12*v.y + this->m13*v.z,
                     this->m21*v.x + this->m22*v.y + this->m23*v.z,
                     this->m31*v.x + this->m32*v.y + this->m33*v.z );
    }


    Matrix getTransposed() const {
      return Matrix( this->m11, this->m21, this->m31,
                     this->m12, this->m22, this->m32,
                     this->m13, this->m23, this->m33);
    }

    void toVectors(Vector& m1, Vector& m2, Vector& m3) const {
      m1 = Vector(this->m11, this->m12, this->m13);
      m2 = Vector(this->m21, this->m22, this->m23);
      m3 = Vector(this->m31, this->m32, this->m33);
    }

    Vector getEigenValues() const;
    void getEigenVectors(Vector& a, Vector& b, Vector& c) const;

    /// @todo optimize
    static Matrix identity() { return Matrix (1,0,0, 0,1,0, 0,0,1); }

    void debug() const;


  public:
    float m11;    float m12;     float m13;
    float m21;    float m22;     float m23;
    float m31;    float m32;     float m33;

};