#include "Matrix.h"

template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
  if (rows <= 0 || cols <= 0) {
    this->rows = 0;
    this->cols = 0;
    this->data = nullptr;
    return;
  }
  this->rows = rows;
  this->cols = cols;

  unsigned int matrixSize = this->cols * this->rows;
  this->data = new float[matrixSize]{0.0f};
}

template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
  Set(0, 0, v.Right());
  Set(1, 0, v.Up());
  Set(2, 0, v.Forward());
}

template <>
void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
  for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
    for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
      unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
      r->data[rDataIx] = 0.0F;
      for (unsigned int kIx = 0; kIx < m2->rows; kIx++) {
        unsigned int dataIx1 = rowIx1 * m1->cols + kIx;
        unsigned int dataIx2 = kIx * m2->cols + colIx2;
        r->data[rDataIx] += m1->data[dataIx1] * m2->data[dataIx2];
      }
    }
  }
}

template <>
Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
  MatrixOf<float> v_m = MatrixOf<float>(v);
  MatrixOf<float> r_m = MatrixOf<float>(3, 1);

  Multiply(m, &v_m, &r_m);

  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
  return r;
}

template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
  MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
  float *rData = new float[3]{};
  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);

  Multiply(this, &v_m, &r_m);

  Vector3 r = Vector3(r_m.data[0], r_m.data[1], r_m.data[2]);
  delete[] vData;
  delete[] rData;
  return r;
}