class Mat4d {
public:
    Vec4d row[4]; // double vector
public:
    // Default constructor:
    Mat4d() {
    }
    // Constructor to broadcast the same value into all elements:
    Mat4d(double d) :
    row{Vec4d(d), Vec4d(d), Vec4d(d), Vec4d(d)} {
    }
    // Constructor to build from two Vec2d:
    Mat4d(const Vec4d &a0, const Vec4d &a1, const Vec4d &a2, const Vec4d &a3=Vec4d(0)) :
    row{a0, a1, a2, a3} {
    }
    // Constructor to build from two Vec2d:
    Mat4d(double a00, double a01, double a02, double a03,
          double a10, double a11, double a12, double a13,
          double a20, double a21, double a22, double a23,
          double a30, double a31, double a32, double a33) :
    row{Vec4d(a00, a01, a02, a03), Vec4d(a10, a11, a12, a13), Vec4d(a20, a21, a22, a23), Vec4d(a30, a31, a32, a33)} {
    }
    // Constructor to build from two Vec2d:
    Mat4d(double a00, double a01, double a02,
          double a10, double a11, double a12,
          double a20, double a21, double a22) :
    row{Vec4d(a00, a01, a02, 0), Vec4d(a10, a11, a12, 0), Vec4d(a20, a21, a22, 0), Vec4d(0)} {
    }
    // Member function to load from array (unaligned)
    Mat4d &load(double const * p)
    {
        row[0].load(p);
        row[1].load(p+4);
        row[2].load(p+9);
        row[3].load(p+12);
        return *this;
    }

    static Mat4d I() { return Mat4d(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1); }

    // Member function to load from array, aligned by 32
    // You may use load_a instead of load if you are certain that p points to an address
    // divisible by 32
    Mat4d & load_a(double const * p) {
        row[0].load_a(p);
        row[1].load_a(p+4);
        row[2].load_a(p+8);
        row[3].load_a(p+12);
        return *this;
    }
    // Member function to store into array (unaligned)
    void store(double * p) const {
        row[0].store(p);
        row[1].store(p+4);
        row[2].store(p+8);
        row[3].store(p+12);
    }
    // Member function storing into array, aligned by 32
    // You may use store_a instead of store if you are certain that p points to an address
    // divisible by 32
    void store_a(double * p) const {
        row[0].store_a(p);
        row[1].store_a(p+4);
        row[2].store_a(p+8);
        row[3].store_a(p+12);
    }
    // Member function storing to aligned uncached memory (non-temporal store).
    // This may be more efficient than store_a when storing large blocks of memory if it 
    // is unlikely that the data will stay in the cache until it is read again.
    // Note: Will generate runtime error if p is not aligned by 32
    void store_nt(double * p) const {
        row[0].store_nt(p);
        row[1].store_nt(p+4);
        row[2].store_nt(p+8);
        row[3].store_nt(p+12);
    }
*/
};

/*****************************************************************************
*
*          Operators for Mat4d
*
*****************************************************************************/

// vector operator + : add element by element
static inline Mat4d operator + (Mat4d const a, Mat4d const b) {
    return Mat4d(a.row[0]+b.row[0], a.row[1]+b.row[1], a.row[2]+b.row[2], a.row[3]+b.row[3]);
}

// vector operator += : add
static inline Mat4d & operator += (Mat4d & a, Mat4d const b) {
    a = a + b;
    return a;
}

// vector operator - : subtract element by element
static inline Mat4d operator - (Mat4d const a, Mat4d const b) {
    return Mat4d(a.row[0]-b.row[0], a.row[1]-b.row[1], a.row[2]-b.row[2], a.row[3]-b.row[3]);
}

// vector operator - : unary minus
// Change sign bit, even for 0, INF and NAN
static inline Mat4d operator - (Mat4d const a) {
    return Mat4d(-a.row[0], -a.row[1], -a.row[2], -a.row[3]);
}

// vector operator -= : subtract
static inline Mat4d & operator -= (Mat4d & a, Mat4d const b) {
    a = a - b;
    return a;
}

static inline Mat4d transpose(Mat4d const m)
{
    return Mat4d(m.row[0][0], m.row[1][0], m.row[2][0], m.row[3][0],
                 m.row[0][1], m.row[1][1], m.row[2][1], m.row[3][1],
                 m.row[0][2], m.row[1][2], m.row[2][2], m.row[3][2],
                 m.row[0][3], m.row[1][3], m.row[2][3], m.row[3][3]);
}

// vector operator * : multiply element by element
static inline Mat4d operator * (Mat4d a, Mat4d const b) {

    a = transpose(a);
    return Mat4d(horizontal_add(a.row[0]*b.row[0]),
                 horizontal_add(a.row[0]*b.row[1]),
                 horizontal_add(a.row[0]*b.row[2]),
                 horizontal_add(a.row[0]*b.row[3]),

                 horizontal_add(a.row[1]*b.row[0]),
                 horizontal_add(a.row[1]*b.row[1]),
                 horizontal_add(a.row[1]*b.row[2]),
                 horizontal_add(a.row[1]*b.row[3]),

                 horizontal_add(a.row[2]*b.row[0]),
                 horizontal_add(a.row[2]*b.row[1]),
                 horizontal_add(a.row[2]*b.row[2]),
                 horizontal_add(a.row[2]*b.row[3]),

                 horizontal_add(a.row[3]*b.row[0]),
                 horizontal_add(a.row[3]*b.row[1]),
                 horizontal_add(a.row[3]*b.row[2]),
                 horizontal_add(a.row[3]*b.row[3]));
}

// vector operator * : multiply matrix with vector
static inline Mat4d operator * (Mat4d const a, Vec4d const b) {
    return Mat4d(a.row[0]*b, a.row[1]*b, a.row[2]*b, a.row[3]*b);
}

// vector operator * : multiply matrix with vector
static inline Mat4d operator * (double d, Mat4d const m) {
    return Mat4d(d*m.row[0], d*m.row[1], d*m.row[2], d*m.row[3]);
}


// vector operator * : multiply vector with matrix
static inline Vec4d operator * (Vec4d const a, Mat4d const b) {
    const Mat4d T(transpose(b));
    const Vec4d v[4] =
    {
        a[0]*T.row[0],
        a[1]*T.row[1],
        a[2]*T.row[2],
        a[3]*T.row[3]
    };

    return Vec4d(v[0][0]+v[1][0]+v[2][0]+v[3][0],
                 v[0][1]+v[1][1]+v[2][1]+v[3][1],
                 v[0][2]+v[1][2]+v[2][2]+v[3][2],
                 v[0][3]+v[1][3]+v[2][3]+v[3][3]);

    // FIMXE FIXME FIXME Maybe faster without extension???
}

static inline Vec4d operator *= (Vec4d &a, Mat4d const m) {
    a = a * m;
    return a;
}

// vector operator / : divide all elements by same integer
static inline Mat4d operator / (Mat4d const a, Mat4d const b) {
    return Mat4d(a.row[0]/b.row[0], a.row[1]/b.row[1], a.row[2]/b.row[2], a.row[3]/b.row[3]);
}

// vector operator / : divide vector and scalar
static inline Mat4d operator / (Mat4d const a, double b) {
    return Mat4d(a.row[0]/Vec4d(b), a.row[1]/Vec4d(b), a.row[2]/Vec4d(b), a.row[3]/Vec4d(b));
}
//static inline Mat4d operator / (double a, Mat4d const b) {
//    return Vec4d(a) / b;
//}

// vector operator /= : divide
static inline Mat4d & operator /= (Mat4d & a, Mat4d const b) {
    a = a / b;
    return a;
}

// ===============================================================

inline Vec4d cross_prod(const Vec4d &v, const Vec4d &w)
{
    return
        permute4<1,2,0,-1>(v)*permute4<2,0,1,-1>(w) -
        permute4<2,0,1,-1>(v)*permute4<1,2,0,-1>(w);
}

inline double scalar_prod(const Vec4d &v, const Vec4d &w)
{
    return horizontal_add(v*w);
}

inline double len(const V4ecd &v)
{
    return sqrt(horizontal_add(v*v));
}

inline Vec4d norm(const Vec4d &v)
{
    return horizontal_and(!v) ? Vec4d(0) : v / len(v);
}