#include "Matrix.h"

template <> void MatrixOf<float>::Transpose(MatrixOf<float> *r) {
  // 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
  int matrixSize = this->rows * this->cols;
  if (matrixSize != r->rows * r->cols)
    // Exception??? For now we don't do anything
    return;

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

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

template <>
void MatrixOf<float>::Multiply(MatrixOf<float> *m2, MatrixOf<float> *r) {
  Multiply(this, m2, r);
}

template <> Vector3 MatrixOf<float>::Multiply(MatrixOf<float> *m, Vector3 v) {
  float *vData = new float[3]{v.x, v.y, v.z};
  MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
  float *rData = new float[3]{};
  MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);

  Multiply(m, &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;
}