//  ************************************************************************************************
//
//  libheinz:  C++ base library of Heinz Maier-Leibnitz Zentrum
//
//! @file      heinz/Rotations3D.h
//! @brief     Defines and implements the rotation matrix in three-dimensional
//! space
//!
//! @homepage  https://jugit.fz-juelich.de/mlz/libheinz
//! @license   Public Domain (BSD Zero Clause License, see LICENSE)
//! @copyright Forschungszentrum Jülich GmbH 2021
//! @authors   Scientific Computing Group at MLZ
//
//  ************************************************************************************************

#ifndef LIBHEINZ_ROTATIONS3D_H
#define LIBHEINZ_ROTATIONS3D_H

#include <array>
#include <cmath>
#include <heinz/Vectors3D.h>
#include <optional>

//! Rotation matrix in three dimensions. Represents group SO(3).
//! Internal parameterization based on quaternions.

template <class T>
class Rotation3D {
public:
    //! Constructs unit transformation
    Rotation3D()
        : Rotation3D<T>(0, 0, 0, 1)
    {
    }

    //! Destructor
    ~Rotation3D() = default;

    //! Creates rotation around x-axis
    static inline Rotation3D<T> AroundX(double phi) { return {sin(phi / 2), 0, 0, cos(phi / 2)}; }

    //! Creates rotation around y-axis
    static inline Rotation3D<T> AroundY(double phi) { return {0, sin(phi / 2), 0, cos(phi / 2)}; }

    //! Creates rotation around z-axis
    static inline Rotation3D<T> AroundZ(double phi) { return {0, 0, sin(phi / 2), cos(phi / 2)}; }

    //! Creates rotation defined by Euler angles
    static inline Rotation3D<T> EulerZXZ(double alpha, double beta, double gamma)
    {
        const auto zrot = AroundZ(alpha);
        const auto xrot = AroundX(beta);
        const auto zrot2 = AroundZ(gamma);
        return zrot * xrot * zrot2;
    }

    //! Calculates the Euler angles corresponding to the rotation
    inline std::array<double, 3> zxzEulerAngles() const
    {
        double m00 = (-1 + 2 * x * x + 2 * s * s);
        double m02 = 2 * (x * z + y * s);
        double m10 = 2 * (y * x + z * s);
        double m12 = 2 * (y * z - x * s);
        double m20 = 2 * (z * x - y * s);
        double m21 = 2 * (z * y + x * s);
        double m22 = (-1 + 2 * z * z + 2 * s * s);

        const double beta = std::acos(m22);

        if (std::abs(m22) == 1.0) // second z angle is zero or pi
            return {std::atan2(m10, m00), beta, 0.};
        return {std::atan2(m02, -m12), beta, std::atan2(m20, m21)};
    }

    //! Returns the inverse transformation.
    inline Rotation3D<T> Inverse() const { return {-x, -y, -z, s}; }

    //! Returns transformed vector _v_.
    template <class U>
    inline U transformed(const U& v) const
    {
        auto xf = (-1 + 2 * x * x + 2 * s * s) * v.x() + 2 * (x * y - z * s) * v.y()
                  + 2 * (x * z + y * s) * v.z();
        auto yf = (-1 + 2 * y * y + 2 * s * s) * v.y() + 2 * (y * z - x * s) * v.z()
                  + 2 * (y * x + z * s) * v.x();
        auto zf = (-1 + 2 * z * z + 2 * s * s) * v.z() + 2 * (z * x - y * s) * v.x()
                  + 2 * (z * y + x * s) * v.y();

        return U(xf, yf, zf);
    }

    //! Composes two transformations
    Rotation3D<T> operator*(const Rotation3D<T>& o) const
    {
        return {s * o.x + x * o.s + y * o.z - z * o.y, s * o.y + y * o.s + z * o.x - x * o.z,
                s * o.z + z * o.s + x * o.y - y * o.x, s * o.s - x * o.x - y * o.y - z * o.z};
    }

    //! Provides equality operator
    bool operator==(const Rotation3D<T>& o) const
    {
        return x == o.x && y == o.y && z == o.z && s == o.s;
    }

    //! Determines if the transformation is trivial (identity)
    inline bool isIdentity() const { return x == 0 && y == 0 && z == 0; }

    inline bool isXRotation() const { return y == 0 && z == 0; }
    inline bool isYRotation() const { return x == 0 && z == 0; }
    inline bool isZRotation() const { return x == 0 && y == 0; }

    std::optional<double> angleAroundCoordAxis(int iAxis) const
    {
        if (iAxis == 0 && isXRotation())
            return 2 * atan2(x, s);
        if (iAxis == 1 && isYRotation())
            return 2 * atan2(y, s);
        if (iAxis == 2 && isZRotation())
            return 2 * atan2(z, s);
        return {};
    }

private:
    Rotation3D(double x_, double y_, double z_, double s_)
        : x(x_)
        , y(y_)
        , z(z_)
        , s(s_)
    {
    }

    double x, y, z, s;
};

#endif // LIBHEINZ_ROTATIONS3D_H
