#ifndef MATRIX_H
#define MATRIX_H

#include "Quaternion.h"
#include "Vector3.h"

#include <cstring>  // for memcpy

namespace LinearAlgebra {

template <typename T>
class Matrix2Of;  // forward declaration

/// @brief A 1-dimensional matrix or vector of arbitrary size
template <typename T>
class Matrix1Of {
 public:
  /// @brief Create a zero-sized matrix
  Matrix1Of();
  /// @brief Create a 1 dimensional matrix
  /// @param size the length of the matrix
  Matrix1Of(int size);
  /// @brief Create a 1 dimensional matrix with the given data
  /// @param size the length of the matrix
  /// @param data the data used to fill the matrix
  Matrix1Of(int size, T* data);
  /// @brief Make a copy of a matrix (copy constructor)
  /// @param m the matrix to copy
  Matrix1Of(const Matrix1Of& m);
  /// @brief Move the data of matrix m to a new matrix (move constructor)
  /// @param m the matrix to move
  /// The data of the matrix m will be re-used for the new matrix.
  /// @note This operator will empty matrix m
  Matrix1Of(Matrix1Of&& m) noexcept;

  /// @brief matrix destructor
  ~Matrix1Of();

  /// @brief Copy the contents of a matrix
  /// @param m the matrix to copy
  /// @return The matrix with a copy of the contents of m
  Matrix1Of& operator=(const Matrix1Of& m);
  /// @brief Move the contents of a matrix
  /// @param m the matrix for which the contents should be moved
  /// @return A matrix with the contents of matrix m
  /// @note This operator will empty matrix m
  Matrix1Of& operator=(Matrix1Of&& m) noexcept;

  /// @brief Get the size of the matrix
  /// @return the size of the matrix
  unsigned int Size() const;
  /// @brief Reize the matrix
  /// @param size The resized matrix
  /// @note This will destruct the data contents of the matrix
  void Resize(int size);

  /// @brief Create a matrix initialized to all zeros
  /// @param size The length of the matrix
  /// @return the initialized matrix
  static Matrix1Of Zero(unsigned int size);
  /// @brief Overwrite all elements with a new value
  /// @param value the value to set for all elements
  void Fill(T value);
  /// @brief Overwrite a range of elements with a new value
  /// @param value the value to set
  /// @param start the first element to overwrite
  /// @param stop the first element to not overwrite
  /// @remark This will overwrite the elements [start..stop)
  /// So the element at index 'stop' will not be overwritten
  void Fill(T value, unsigned int start, unsigned int stop);

  /// @brief Convert from a 3d vector
  /// @param v the vector to convert
  /// @return a matrix with the 3 elements of the vector
  static Matrix1Of FromVector3(Vector3Of<T> v);
  /// @brief Convert to a 3d vector
  /// @return a vector with the first 3 elements of the matrix
  Vector3Of<T> ToVector3();

  /// @brief Convert from a quaternion
  /// @param q the quaternion to convert
  /// @return a matrix with the 4 elements of the quaternion
  static Matrix1Of<float> FromQuaternion(Quaternion q);
  /// @brief Convert to a quaternion
  /// @return a quaternion with the first 4 elements of the quaternion
  Quaternion ToQuaternion();

  /// @brief Get an element from the matrix
  /// @param ix the index of the element to retrieve
  /// @return the element at index ix
  const T& operator()(int ix) const;
  /// @brief Get the reference to an element of a matrix
  /// @param ix the index of the element
  /// @return a reference to the element of index ix
  T& operator()(int ix);

  /// @brief Get a subsection of a matrix
  /// @param start the first element to retrieve
  /// @param stop the first element to not retrieve
  /// @return a matrix with the requested elements
  /// @remark This will retrieve the elements [start..stop)
  /// So the element at index 'stop' will not be included
  Matrix1Of Slice(unsigned int start, unsigned int stop);
  /// @brief Overwrite a subsection of a matrix with the values from another matrix
  /// @param start the first element to overwrite
  /// @param stop the first element to not overwrite
  /// @param m a matrix with the value to write to the given range
  /// @remark This will overwrite the elements [start..stop)
  /// So the element at index 'stop' will not be overwritten
  void UpdateSlice(unsigned int start, unsigned int stop, const Matrix1Of& m) const;

 protected:
  /// @brief The data values
  T* data = nullptr;
  /// @brief The length of the matrix
  unsigned int size = 0;

 private:
  /// @brief if this is true, functions will not do memory management
  /// The allocation and deallocation of this->data should be handled outside this library
  bool externalData = true;

  friend class Matrix2Of<T>;
};

using Matrix1Int = Matrix1Of<int>;
using Matrix1Float = Matrix1Of<float>;
using Matrix1Double = Matrix1Of<double>;

using Matrix1 = Matrix1Float;

/*
class Matrix2;

class SubMatrix2 {
 public:
  const Matrix2& parent;
  int startRow;
  int startCol;
  int rowCount;
  int colCount;

  SubMatrix2(const Matrix2& m, int startRow, int startCol, int rowCount, int colCount);

  // Assign from another full Matrix (sizes must match)
  void operator=(const Matrix2& src);

  // Assign from another SubMatrix2 (handles aliasing)
  void operator=(const SubMatrix2& src);

  // Read element
  float operator()(int row, int col) const;

  // Write element
  const float& operator()(int row, int col);
};

/// @brief A 2-dimensional matrix of arbitrary size
class Matrix2 {
 public:
  Matrix2();
  Matrix2(int nRows, int nCols);
  Matrix2(int nRows, int nCols, float* data);
  Matrix2(const Matrix2& m);      // Copy constructor
  Matrix2(Matrix2&& m) noexcept;  // Move constructor

  ~Matrix2();

  Matrix2& operator=(const Matrix2& m);          // Copy assignment operator
  Matrix2& operator=(Matrix2&& other) noexcept;  // Move assignment operator

  unsigned int NRows() const;
  unsigned int NCols() const;

  float* GetData() { return this->data; }
  float* GetData() const { return this->data; }

  static Matrix2 Zero(int nRows, int nCols);
  void Fill(float value);
  void FillDiagonal(float f);

  static Matrix2 Identity(int size);
  static Matrix2 Identity(int nRows, int nCols);
  static Matrix2 Diagonal(float f, int size);

  static Matrix2 SkewMatrix(const Vector3Of<float>& v);

  Matrix2 Transpose() const;
  Matrix2 Inverse();

  const float& operator()(int row, int col) const;
  float& operator()(int row, int col);

  Matrix2 GetRows(int from, int to);
  Vector3 GetRow3(int rowIx);

  void SetRow(int rowIx, Matrix1 source);
  void SetRow3(int rowIx, Vector3 v);

  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop) const;

  void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;

  inline SubMatrix2 GetBlock(int startRow, int startCol, int rowCount, int colCount) const {
    return SubMatrix2(*this, startRow, startCol, rowCount, colCount);
  }
  inline Matrix2 CopyBlock(int startRow, int startCol, int rowCount, int colCount) const {
    return Slice(startRow, startRow + rowCount, startCol, startCol + colCount);
  }

  Matrix2 DeleteRows(int rowStart, int rowStop) const;
  Matrix2 DeleteColumns(int colStart, int colStop) const;

  Matrix2 operator-() const;

  /// @brief Add a matrix to this matrix
  /// @param m The matrix to add to this matrix
  /// @return The result of the addition
  Matrix2 operator+(const Matrix2& v) const;
  Matrix2& operator+=(const Matrix2& v);

  Matrix2 operator-(const Matrix2& v) const;

  Matrix2 operator*(const Matrix2& m) const;
  friend Matrix2 operator*(const Matrix2& m, float f) {
    Matrix2 r = Matrix2(m.nRows, m.nCols);
    for (int ix = 0; ix < r.size; ix++)
      r.data[ix] = m.data[ix] * f;
    return r;
  }
  friend Matrix2 operator*(float f, const Matrix2& m) {
    Matrix2 r = Matrix2(m.nRows, m.nCols);
    for (int ix = 0; ix < r.size; ix++)
      r.data[ix] = f * m.data[ix];
    return r;
  }

  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v);

  friend Matrix2 operator/(const Matrix2& m, float f) {
    Matrix2 r = Matrix2(m.nRows, m.nCols);
    for (int ix = 0; ix < r.size; ix++)
      r.data[ix] = m.data[ix] / f;
    return r;
  }
  friend Matrix2 operator/(float f, const Matrix2& m) {
    Matrix2 r = Matrix2(m.nRows, m.nCols);
    for (int ix = 0; ix < r.size; ix++)
      r.data[ix] = f / m.data[ix];
    return r;
  }

  template <typename T>
  static Matrix2 Omega(const Vector3Of<T>& v);

 protected:
  int nRows = 0;
  int nCols = 0;

 protected:
  int size = 0;
  float* data = nullptr;

 private:
  bool externalData = true;
};
*/

/// @brief A 2-dimensional matrix of arbitrary size
template <typename T>
class Matrix2Of {
 public:
  Matrix2Of();
  Matrix2Of(unsigned int rowCount, unsigned int colCount);
  Matrix2Of(unsigned int rowCount, unsigned int colCount, T* data);
  Matrix2Of(const Matrix2Of& m);      // Copy Constructor
  Matrix2Of(Matrix2Of&& m) noexcept;  // Move Contructor

  ~Matrix2Of();

  Matrix2Of& operator=(const Matrix2Of& m);      // Copy assignment operator
  Matrix2Of& operator=(Matrix2Of&& m) noexcept;  // Move assignment operator

  unsigned int RowCount() const;
  unsigned int ColCount() const;

  const T& operator()(unsigned int rowIx, unsigned int colIx) const;
  T& operator()(unsigned int rowIx, unsigned int colIx);

  T* GetData();
  T* GetData() const;

  static Matrix2Of Zero(unsigned int nRows, unsigned int nCols);
  void Fill(T value);
  void FillDiagonal(T f);

  static Matrix2Of Identity(unsigned int size);
  static Matrix2Of Identity(unsigned int nRows, unsigned int nCols);
  static Matrix2Of Diagonal(T f, unsigned int size);

  static Matrix2Of SkewMatrix(const Vector3Of<T>& v);

  Matrix1Of<T> GetRow(unsigned int rowIx);
  Vector3Of<T> GetRow3(unsigned int rowIx);
  Matrix2Of<T> GetRows(unsigned int fromRowIx, unsigned int toRowIx);

  void SetRow(unsigned int rowIx, Matrix1Of<T> source);
  void SetRow3(unsigned int rowIx, Vector3Of<T> source);

  Matrix2Of DeleteRows(unsigned int fromRowIx, unsigned int toRowIx) const;
  Matrix2Of DeleteColumns(unsigned int fromColIx, unsigned int colStop) const;

  Matrix2Of operator-() const;

  /// @brief Add a matrix to this matrix
  /// @param m The matrix to add to this matrix
  /// @return The result of the addition
  Matrix2Of operator+(const Matrix2Of& v) const;
  Matrix2Of& operator+=(const Matrix2Of& v);

  Matrix2Of operator-(const Matrix2Of& v) const;
  Matrix2Of& operator-=(const Matrix2Of& v);

  Matrix2Of operator*(const Matrix2Of& m) const;
  template <typename U>
  friend Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f);
  template <typename U>
  friend Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m);
  template <typename U>
  friend Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v);

  template <typename U>
  friend Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f);
  template <typename U>
  friend Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m);

  Matrix2Of Transposed() const;
  Matrix2Of Inverse();

  Matrix2Of Slice(int rawStart, int rowStop, int colStart, int colStop) const;

  void UpdateSlice(int rowStart, int rowStop, const Matrix2Of& m) const;
  void UpdateSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2Of& m) const;

  /// @brief Compute the Omega matrix of a 3D vector
  /// @param v The vector
  /// @return 4x4 Omega matrix
  // static Matrix2Of Omega(const Vector3Of<T>& v);

 protected:
  unsigned int nRows = 0;
  unsigned int nCols = 0;

 protected:
  unsigned int size = 0;
  T* data = nullptr;

 private:
  bool externalData = true;
};

#pragma region friend functions

// For a class template a friend function must also be a template and defined in the header (unless
// you restrict to a concrete instantiation)...
template <typename U>
Matrix2Of<U> operator*(const Matrix2Of<U>& m, U f) {
  Matrix2Of<U> r(m.nRows, m.nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = m.data[ix] * f;
  return r;
}
template <typename U>
Matrix2Of<U> operator*(U f, const Matrix2Of<U>& m) {
  Matrix2Of<U> r(m.nRows, m.nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = f * m.data[ix];
  return r;
}
template <typename U>
Matrix1Of<U> operator*(const Matrix2Of<U>& m, const Matrix1Of<U>& v) {
  Matrix1Of<U> r(m.nRows);
  for (unsigned int rowIx = 0; rowIx < m.nRows; rowIx++) {
    int mRowIx = rowIx * m.nCols;
    for (unsigned int colIx = 0; colIx < m.nCols; colIx++)
      r(rowIx) += m.data[mRowIx + colIx] * v(colIx);
  }
  return r;
}

template <typename U>
Matrix2Of<U> operator/(const Matrix2Of<U>& m, U f) {
  Matrix2Of<U> r(m.nRows, m.nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = m.data[ix] / f;
  return r;
}
template <typename U>
Matrix2Of<U> operator/(U f, const Matrix2Of<U>& m) {
  Matrix2Of r(m.nRows, m.nCols);
  for (unsigned int ix = 0; ix < r.size; ix++)
    r.data[ix] = f / m.data[ix];
  return r;
}

#pragma endregion friend functions

template class Matrix2Of<int>;
template class Matrix2Of<float>;
template class Matrix2Of<double>;

using Matrix2Int = Matrix2Of<int>;
using Matrix2Float = Matrix2Of<float>;
using Matrix2Double = Matrix2Of<double>;

//using Matrix2 = Matrix2Float;

}  // namespace LinearAlgebra
// using namespace LinearAlgebra;

#endif