#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;
}