#ifndef MATRIX_H
#define MATRIX_H

#include "Vector3.h"

namespace Passer {
namespace LinearAlgebra {

/// @brief Single precision float matrix
template <typename T> class MatrixOf {
public:
  MatrixOf(unsigned int rows, unsigned int cols);
  MatrixOf(unsigned int rows, unsigned int cols, const T *source)
      : MatrixOf(rows, cols) {
    Set(source);
  }
  MatrixOf(Vector3 v); // creates a 3,1 matrix

  ~MatrixOf() {
    if (this->data == nullptr)
      return;

    delete[] this->data;
  }

  /// @brief Transpose with result in matrix m
  /// @param r The matrix in which the transposed matrix is stored
  void Transpose(MatrixOf<T> *r) const {
    // Check dimensions first
    // We dont care about the rows and cols (we overwrite them)
    // but the data size should be equal to avoid problems
    // We cannot check the data size directly, but the row*col should be equal
    unsigned int matrixSize = this->cols * this->rows;
    unsigned int resultSize = r->rows * r->cols;
    if (matrixSize != resultSize) {
      // Return a null matrix;
      // We dont set data to nullptr because it is allocated memory
      // Instead we write all zeros
      for (unsigned int dataIx = 0; dataIx < resultSize; dataIx++)
        r->data[dataIx] = 0.0f;
      r->rows = 0;
      r->cols = 0;
      return;
    }

    r->cols = this->rows;
    r->rows = this->cols;

    for (unsigned int rDataIx = 0; rDataIx < matrixSize; rDataIx++) {
      unsigned int rowIx = rDataIx / this->rows;
      unsigned int colIx = rDataIx % this->rows;
      unsigned int mDataIx = this->cols * colIx + rowIx;
      r->data[rDataIx] = this->data[mDataIx];
    }
  }

  static void Multiply(const MatrixOf<T> *m1, const MatrixOf<T> *m2,
                       MatrixOf<T> *r);
  void Multiply(const MatrixOf<T> *m, MatrixOf<T> *r) const {
    Multiply(this, m, r);
  }

  static Vector3 Multiply(const MatrixOf<T> *m, Vector3 v);
  Vector3 operator*(const Vector3 v) const;

  T Get(unsigned int rowIx, unsigned int colIx) const {
    unsigned int dataIx = rowIx * this->cols + colIx;
    return this->data[dataIx];
  }

  void Set(unsigned int rowIx, unsigned int colIx, T value) {
    unsigned int dataIx = rowIx * this->cols + colIx;
    this->data[dataIx] = value;
  }

  // This function does not check on source size!
  void Set(const T *source) {
    unsigned int matrixSize = this->cols * this->rows;
    for (unsigned int dataIx = 0; dataIx < matrixSize; dataIx++)
      this->data[dataIx] = source[dataIx];
  }

  // This function does not check on source size!
  void SetRow(unsigned int rowIx, const T *source) {
    unsigned int dataIx = rowIx * this->cols;
    for (unsigned int sourceIx = 0; sourceIx < this->cols; dataIx++, sourceIx++)
      this->data[dataIx] = source[sourceIx];
  }

  // This function does not check on source size!
  void SetCol(unsigned int colIx, const T *source) {
    unsigned int dataIx = colIx;
    for (unsigned int sourceIx = 0; sourceIx < this->cols;
         dataIx += this->cols, sourceIx++)
      this->data[dataIx] = source[sourceIx];
  }

  void CopyFrom(const MatrixOf<T> *m) {
    unsigned int thisMatrixSize = this->cols * this->rows;
    unsigned int mMatrixSize = m->cols * m->rows;
    if (mMatrixSize != thisMatrixSize)
      return;

    for (unsigned int dataIx = 0; dataIx < thisMatrixSize; dataIx++)
      this->data[dataIx] = m->data[dataIx];
  }

  unsigned int RowCount() const { return rows; }
  unsigned int ColCount() const { return cols; }

private:
  unsigned int rows;
  unsigned int cols;
  T *data;
};

} // namespace LinearAlgebra
} // namespace Passer
using namespace Passer::LinearAlgebra;

#endif