#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
  unsigned int matrixSize = this->cols * this->rows;
  if (matrixSize != r->rows * r->cols)
    // Exception??? For now we don't do anything
    return;

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

  for (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];
  }
}

template <>
void MatrixOf<float>::Multiply(MatrixOf<float>* m1,
                               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 (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 <>
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;
}

template <>
float MatrixOf<float>::Get(unsigned int rowIx, unsigned int colIx) {
  unsigned int dataIx = rowIx * this->cols + colIx;
  return this->data[dataIx];
}

template <>
unsigned int MatrixOf<float>::RowCount() {
  return rows;
}

template <>
unsigned int MatrixOf<float>::ColCount() {
  return cols;
}