/* This file is part of MAUS: http://micewww.pp.rl.ac.uk:8080/projects/maus
 *
 * MAUS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * MAUS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MAUS.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "RotationMatrix.h"

#include <cmath>
#include <iomanip>
#include <cstdlib>


RotationMatrix::RotationMatrix() {
  for (unsigned row = 0; row < DIM; ++row)
    for (unsigned col = 0; col < DIM; ++col)
      mat[row][col] = 0;
}


RotationMatrix::RotationMatrix(double angle, eAxis axis) {
  double radAng = angle*PI/180;
  for (unsigned row = 0; row < DIM; ++row)
    for (unsigned col = 0; col < DIM; ++col)
      mat[row][col] = 0;

  switch (axis) {
    case X:
      mat[0][0] = 1;
      mat[1][1] = cos(radAng);
      mat[1][2] = -sin(radAng);
      mat[2][1] = sin(radAng);
      mat[2][2] = cos(radAng);
      break;
    case Y:
      mat[0][0] = cos(radAng);
      mat[0][2] = -sin(radAng);
      mat[1][1] = 1;
      mat[2][0] = sin(radAng);
      mat[2][2] = cos(radAng);
      break;
    case Z:
      mat[0][0] = cos(radAng);
      mat[0][1] = -sin(radAng);
      mat[1][0] = sin(radAng);
      mat[1][1] = cos(radAng);
      mat[2][2] = 1;
      break;
    default:
      std::cerr << "[ERROR]RotationMatrix: Unknown axis! Exiting!" << std::endl;
      exit(42);
  }
}

RotationMatrix::RotationMatrix(std::vector<double> elements) { // elements row by row
    if (elements.size() != 9) {
      std::cerr << "[ERROR]RotationMatrix: Wrong elements vectors size! Cannot initialize!";
      std::cerr << " Exiting!" << std::endl;
      exit(42);
    }

    mat[0][0] = elements[0];
    mat[0][1] = elements[1];
    mat[0][2] = elements[2];
    mat[1][0] = elements[3];
    mat[1][1] = elements[4];
    mat[1][2] = elements[5];
    mat[2][0] = elements[6];
    mat[2][1] = elements[7];
    mat[2][2] = elements[8];
}


void RotationMatrix::PrintMatrix() {
  std::cout << std::endl;
  for (unsigned row = 0; row < DIM; ++row) {
    for (unsigned col = 0; col < DIM; ++col)
      std::cout << std::setw(7) << std::setprecision(3) << mat[row][col] << " ";
    std::cout << std::endl;
  }
  std::cout << std::endl;
}


void RotationMatrix::Transpose() {
  matrix transposed;

  for (unsigned row = 0; row < DIM; ++row)
    for (unsigned col = 0; col < DIM; ++col)
      transposed[row][col] = mat[col][row];

  for (unsigned row = 0; row < DIM; ++row)
    for (unsigned col = 0; col < DIM; ++col)
      mat[row][col] = transposed[row][col];
}


RotationMatrix RotationMatrix::operator*(const RotationMatrix& otherMatrix) {
  RotationMatrix result;
  for (unsigned row = 0; row < DIM; ++row)
    for (unsigned col = 0; col < DIM; ++col)
      for (unsigned k = 0; k < DIM; ++k)
        result.mat[row][col] += this->mat[row][k] * otherMatrix.mat[k][col];

  return result;
}



std::vector<double> RotationMatrix::operator*(const std::vector<double>& v) {
  if (v.size() != 3) {
    std::cerr << "[ERROR]RotationMatrix: Wrong vector size: " << v.size();
    std::cerr << "! Exiting!" << std::endl;
    exit(42);
  }

  std::vector<double> result;
  for (unsigned row = 0; row < DIM; ++row) {
    double sum = 0;
    for (unsigned col = 0; col < DIM; ++col)
      sum += this->mat[row][col]*v[col];
    result.push_back(sum);
  }

  return result;
}