/*
 * matrixN.h
 *
 *  Created on: 2020.06.19
 *      Author: JackyPan
 */

/*
 *  N dimensional matrix operations
 */

#pragma once

#include "math/math_inc.h"

#ifdef __cplusplus
template <typename T, uint8_t N>
class VectorN;


template <typename T, uint8_t N>
class MatrixN {

    friend class VectorN<T,N>;

public:
    // constructor from zeros
    MatrixN<T,N>(void) {
        memset(v, 0, sizeof(v));
    }

    // constructor from 4 diagonals
    MatrixN<T,N>(const float d[N]) {
        memset(v, 0, sizeof(v));
        for (uint8_t i = 0; i < N; i++) {
            v[i][i] = d[i];
        }
    }

    // multiply two vectors to give a matrix, in-place
    void mult(const VectorN<T,N> &A, const VectorN<T,N> &B);

    // subtract B from the matrix
    MatrixN<T,N> &operator -=(const MatrixN<T,N> &B);

    // add B to the matrix
    MatrixN<T,N> &operator +=(const MatrixN<T,N> &B);

    // Matrix symmetry routine
    void force_symmetry(void);

private:
    T v[N][N];
};

#endif/* __cplusplus */
