//
// Created by ivan on 28.03.2022.
//

#include <iostream>
#include <cmath>
#include <Eigen/Dense>

void euler2Rot(Eigen::Matrix<double, 3, 3>& Rot, double const rx, double const ry, double const rz){
  Eigen::Matrix<double, 3, 3> Rx = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 3, 3> Ry = Eigen::Matrix<double, 3, 3>::Zero();
  Eigen::Matrix<double, 3, 3> Rz = Eigen::Matrix<double, 3, 3>::Zero();

  Rx(0, 0) = 1;
  Rx(1, 1) = cos(rx);
  Rx(2, 1) = sin(rx);
  Rx(1, 2) = -sin(rx);
  Rx(2, 2) = cos(rx);

  std::cout << Rx << std::endl;
  std::cout << " " << std::endl;

  Ry(0, 0) = cos(ry);
  Ry(0, 2) = sin(ry);
  Ry(1, 1) = 1;
  Ry(2, 0) = -sin(ry);
  Ry(2, 2) = cos(ry);

  std::cout << Ry << std::endl;
  std::cout << " " << std::endl;

  Rz(0, 0) = cos(rz);
  Rz(0, 1) = -sin(rz);
  Rz(1, 0) = sin(rz);
  Rz(1, 1) = cos(rz);
  Rz(2, 2) = 1;

  std::cout << Rz << std::endl;
  std::cout << " " << std::endl;

  auto Rxy = Rx * Ry;
  Rot = Rxy * Rz;

}

int main(int argc, char** argv){
  double rx;
  double ry;
  double rz;
  rx = 1.1;
  ry = 1.2;
  rz = 1.1;
  Eigen::Matrix<double, 3, 3> Rot;

  euler2Rot(Rot, rx, ry, rz);
  std::cout << Rot << std::endl;

  std::cout << "" << std::endl;
  std::cout << "The eigen matrix multiplication" << std::endl;
  Eigen::Matrix<int, 3, 3> M1;
  Eigen::Matrix<int, 3, 3> M2;
  M1 << 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

  M2 << 1, 2, 3,
        4, 5, 6,
        7, 8, 9;

  std::cout << M1 * M2 << std::endl;
}