// SPDX-FileCopyrightText: The Eigen Authors
// SPDX-License-Identifier: MPL-2.0

#include <unsupported/Eigen/EulerAngles>
#include <iostream>

using namespace Eigen;

int main() {
  // A common Euler system by many armies around the world,
  //  where the first one is the azimuth(the angle from the north -
  //   the same angle that is show in compass)
  //  and the second one is elevation(the angle from the horizon)
  //  and the third one is roll(the angle between the horizontal body
  //   direction and the plane ground surface)
  // Keep remembering we're using radian angles here!
  typedef EulerSystem<-EULER_Z, EULER_Y, EULER_X> MyArmySystem;
  typedef EulerAngles<double, MyArmySystem> MyArmyAngles;

  MyArmyAngles vehicleAngles(3.14 /*PI*/ / 2, /* heading to east, notice that this angle is counter-clockwise */
                             -0.3,            /* going down from a mountain */
                             0.1);            /* slightly rolled to the right */

  // Some Euler angles representation that our plane use.
  EulerAnglesZYZd planeAngles(0.78474, 0.5271, -0.513794);

  MyArmyAngles planeAnglesInMyArmyAngles(planeAngles);

  std::cout << "vehicle angles(MyArmy):     " << vehicleAngles << std::endl;
  std::cout << "plane angles(ZYZ):        " << planeAngles << std::endl;
  std::cout << "plane angles(MyArmy):     " << planeAnglesInMyArmyAngles << std::endl;

  // Now lets rotate the plane a little bit
  std::cout << "==========================================================\n";
  std::cout << "rotating plane now!\n";
  std::cout << "==========================================================\n";

  Quaterniond planeRotated = AngleAxisd(-0.342, Vector3d::UnitY()) * planeAngles;

  planeAngles = planeRotated;
  planeAnglesInMyArmyAngles = planeRotated;

  std::cout << "new plane angles(ZYZ):     " << planeAngles << std::endl;
  std::cout << "new plane angles(MyArmy): " << planeAnglesInMyArmyAngles << std::endl;

  return 0;
}