/* 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 "VectorTransform.h"

#include <iostream>
#include <iomanip>

VectorTransform::VectorTransform(std::vector<double> vec) {
  if (vec.size() != 3) {
    std::cerr << "[ERROR]VectorTransform: Trying to initialize VectorTransform";
    std::cerr << " with wrong size vector! Exiting!" << std::endl;
    exit(1);
  } else {
    v[0] = vec[0];
    v[1] = vec[1];
    v[2] = vec[2];
  }
}

void VectorTransform::Translate(double xComp, double yComp, double zComp, std::string opt) {
  double x = v[0];
  double y = v[1];
  double z = v[2];

  std::fill(v.begin(), v.end(), 0.);

  if (opt == "reg") {
    v[0] = x + xComp;
    v[1] = y + yComp;
    v[2] = z + zComp;
  } else if (opt == "inv") {
    v[0] = x - xComp;
    v[1] = y - yComp;
    v[2] = z - zComp;
  } else {
    std::cerr << "[ERROR]VectorTransform: Unknown option in translate!";
    std::cerr << " Options are 'reg' for regular or 'inv' for inverse translation.";
    std::cerr << " Exiting!" << std::endl;
    exit(42);
  }
}

void VectorTransform::Rotate(double angle, RotationMatrix::eAxis axis, std::string opt) {
  std::vector<double> result = std::vector<double>(3);
  std::fill(result.begin(), result.end(), 0.);
  RotationMatrix mat(angle, axis);

  if (opt == "inv")
    mat.Transpose();
  else if (opt != "reg") {
    std::cerr << "[ERROR]VectorTransform: Unknown option in rotate!";
    std::cerr << " Options are 'reg' for regular or 'inv' for inverse rotation.";
    std::cerr << " Exiting!" << std::endl;
    exit(42);
  }

  result = mat*v;

  for (unsigned i = 0; i < v.size(); ++i)
      v[i] = result[i];
}

void VectorTransform::Rotate(std::vector<double> matrixElements, std::string opt) {
    std::vector<double> result = std::vector<double>(3);
    std::fill(result.begin(), result.end(), 0.);
    RotationMatrix mat(matrixElements);

    if (opt == "inv")
      mat.Transpose();
    else if (opt != "reg") {
      std::cerr << "Error! Unknown option in rotate!";
      std::cerr << " Options are 'reg' for regular or 'inv' for inverse rotation.";
      std::cerr << " Exiting!" << std::endl;
      exit(42);
    }

    result = mat*v;

    for (unsigned i = 0; i < v.size(); ++i)
        v[i] = result[i];
}

void VectorTransform::Print() {
  std::cout << std::endl << "Vector coordinates:" << std::endl;
  for (unsigned i = 0; i < v.size(); ++i)
    std::cout << std::setw(3) << std::setprecision(3) << v[i] << "   ";
  std::cout << std::endl;
}