/****************************************************************************
**
** Copyright (C) 1993-2009 NVIDIA Corporation.
** Copyright (C) 2019 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of Qt Quick 3D.
**
** $QT_BEGIN_LICENSE:GPL$
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see https://www.qt.io/terms-conditions. For further
** information use the contact form at https://www.qt.io/contact-us.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3 or (at your option) any later version
** approved by the KDE Free Qt Foundation. The licenses are as published by
** the Free Software Foundation and appearing in the file LICENSE.GPL3
** included in the packaging of this file. Please review the following
** information to ensure the GNU General Public License requirements will
** be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** $QT_END_LICENSE$
**
****************************************************************************/

#include "qssgrendereulerangles_p.h"
#include <QtQuick3DUtils/private/qssgutils_p.h>

QT_BEGIN_NAMESPACE

namespace {
// extractEulerOrder unpacks all useful information about order simultaneously.
inline void extractEulerOrder(EulerOrder eulerOrder, int &i, int &j, int &k, int &h, int &n, int &s, int &f)
{
    uint order = eulerOrder;
    f = order & 1;
    order = order >> 1;
    s = order & 1;
    order = order >> 1;
    n = order & 1;
    order = order >> 1;
    i = EulSafe[order & 3];
    j = EulNext[i + n];
    k = EulNext[i + 1 - n];
    h = s ? k : i;
}

enum QuatPart { X, Y, Z, W };
}

/**
 *	Constructor
 */
QSSGEulerAngleConverter::QSSGEulerAngleConverter() = default;

/**
 *	Destructor
 */
QSSGEulerAngleConverter::~QSSGEulerAngleConverter() = default;

/**
 *  Constructs a Euler angle & holds it in a EulerAngles struct
 *  @param theI         x rotation ( radians )
 *  @param theJ         y rotation ( radians )
 *  @param theH         z rotation ( radians )
 *  @param theOrder     the order this angle is in namely XYZ( static ), etc.
 *                      use the getEulerOrder() to generate this enum
 *  @return the euler angle
 */
EulerAngles QSSGEulerAngleConverter::euler(float theI, float theJ, float theH, EulerOrder theOrder)
{
    EulerAngles theEulerAngle;
    theEulerAngle.x = theI;
    theEulerAngle.y = theJ;
    theEulerAngle.z = theH;
    theEulerAngle.order = theOrder;
    return theEulerAngle;
}

/**
 *  Construct quaternion from Euler angles (in radians).
 *  @param theEulerAngle        incoming angle( radians )
 *  @return the Quaternion
 */
QSSGEulerAngleConverter::Quat QSSGEulerAngleConverter::eulerToQuat(EulerAngles theEulerAngle)
{
    Quat theQuaternion;
    double a[3], ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i, j, k, h, n, s, f;

    extractEulerOrder(theEulerAngle.order, i, j, k, h, n, s, f);

    if (f == EulFrmR)
        std::swap(theEulerAngle.x, theEulerAngle.z);

    if (n == EulParOdd)
        theEulerAngle.y *= -1;

    ti = theEulerAngle.x * 0.5;
    tj = theEulerAngle.y * 0.5;
    th = theEulerAngle.z * 0.5;

    ci = cos(ti);
    cj = cos(tj);
    ch = cos(th);

    si = sin(ti);
    sj = sin(tj);
    sh = sin(th);

    cc = ci * ch;
    cs = ci * sh;
    sc = si * ch;
    ss = si * sh;

    if (s == EulRepYes) {
        a[i] = cj * (cs + sc); /* Could speed up with */
        a[j] = sj * (cc + ss); /* trig identities. */
        a[k] = sj * (cs - sc);
        theQuaternion.w = float(cj * (cc - ss));
    } else {
        a[i] = cj * sc - sj * cs;
        a[j] = cj * ss + sj * cc;
        a[k] = cj * cs - sj * sc;
        theQuaternion.w = float(cj * cc + sj * ss);
    }
    if (n == EulParOdd)
        a[j] = -a[j];

    theQuaternion.x = float(a[X]);
    theQuaternion.y = float(a[Y]);
    theQuaternion.z = float(a[Z]);
    return theQuaternion;
}

/**
 *  Construct matrix from Euler angles (in radians).
 *  @param theEulerAngle        incoming angle
 *  @param theMatrix            outgoing matrix
 */
void QSSGEulerAngleConverter::eulerToHMatrix(EulerAngles theEulerAngle, HMatrix theMatrix)
{
    double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
    int i, j, k, h, n, s, f;

    extractEulerOrder(theEulerAngle.order, i, j, k, h, n, s, f);

    if (f == EulFrmR)
        std::swap(theEulerAngle.x, theEulerAngle.z);

    if (n == EulParOdd) {
        theEulerAngle.x *= -1;
        theEulerAngle.y *= -1;
        theEulerAngle.z *= -1;
    }

    ti = theEulerAngle.x;
    tj = theEulerAngle.y;
    th = theEulerAngle.z;

    ci = cos(ti);
    cj = cos(tj);
    ch = cos(th);

    si = sin(ti);
    sj = sin(tj);
    sh = sin(th);

    cc = ci * ch;
    cs = ci * sh;
    sc = si * ch;
    ss = si * sh;

    if (s == EulRepYes) {
        theMatrix[i][i] = float(cj);
        theMatrix[i][j] = float(sj * si);
        theMatrix[i][k] = float(sj * ci);
        theMatrix[j][i] = float(sj * sh);
        theMatrix[j][j] = float(-cj * ss + cc);
        theMatrix[j][k] = float(-cj * cs - sc);
        theMatrix[k][i] = float(-sj * ch);
        theMatrix[k][j] = float(cj * sc + cs);
        theMatrix[k][k] = float(cj * cc - ss);
    } else {
        theMatrix[i][i] = float(cj * ch);
        theMatrix[i][j] = float(sj * sc - cs);
        theMatrix[i][k] = float(sj * cc + ss);
        theMatrix[j][i] = float(cj * sh);
        theMatrix[j][j] = float(sj * ss + cc);
        theMatrix[j][k] = float(sj * cs - sc);
        theMatrix[k][i] = float(-sj);
        theMatrix[k][j] = float(cj * si);
        theMatrix[k][k] = float(cj * ci);
    }

    theMatrix[W][X] = 0.0;
    theMatrix[W][Y] = 0.0;
    theMatrix[W][Z] = 0.0;
    theMatrix[X][W] = 0.0;
    theMatrix[Y][W] = 0.0;
    theMatrix[Z][W] = 0.0;
    theMatrix[W][W] = 1.0;
}

/**
 *  Convert matrix to Euler angles (in radians).
 *  @param theMatrix            incoming matrix
 *  @param theOrder             use getEulerOrder() to generate this enum
 *  @return a set of angles in radians!!!!
 */
EulerAngles QSSGEulerAngleConverter::eulerFromHMatrix(HMatrix theMatrix, EulerOrder theOrder)
{
    EulerAngles theEulerAngle;
    int i, j, k, h, n, s, f;

    extractEulerOrder(theOrder, i, j, k, h, n, s, f);

    double ij = double(theMatrix[i][j]), ik = double(theMatrix[i][k]);
    double ii = double(theMatrix[i][i]), ji = double(theMatrix[j][i]);
    double kk = double(theMatrix[k][k]), jj = double(theMatrix[j][j]);
    double ki = double(theMatrix[k][i]), kj = double(theMatrix[k][j]);
    double jk = double(theMatrix[j][k]);
    if (s == EulRepYes) {
        double sy = sqrt(ij*ij + ik*ik);
        theEulerAngle.y = float(atan2(sy, ii));
        if (!qFuzzyIsNull(float(sy))) {
            theEulerAngle.x = float(atan2(ij, ik));
            theEulerAngle.z = float(atan2(ji, -ki));
        } else {
            theEulerAngle.x = float(atan2(-jk, jj));
            theEulerAngle.z = 0;
        }
    } else {
        double cy = sqrt(ii*ii + ji*ji);
        theEulerAngle.y = float(atan2(-ki, cy));
        if (!qFuzzyIsNull(float(cy))) {
            theEulerAngle.x = float(atan2(kj, kk));
            theEulerAngle.z = float(atan2(ji, ii));
        } else {
            theEulerAngle.x = float(atan2(-jk, jj));
            theEulerAngle.z = 0;
        }
    }

    if (n == EulParOdd) {
        theEulerAngle.x *= -1;
        theEulerAngle.y *= -1;
        theEulerAngle.z *= -1;
    }

    if (f == EulFrmR)
        std::swap(theEulerAngle.x, theEulerAngle.z);

    theEulerAngle.order = theOrder;
    return theEulerAngle;
}

/**
 *  Convert quaternion to Euler angles (in radians).
 *  @param theQuaternion        incoming quaternion
 *  @param theOrder             use getEulerOrder() to generate this enum
 *  @return the generated angles ( radians )
 */
EulerAngles QSSGEulerAngleConverter::eulerFromQuat(Quat theQuaternion, EulerOrder theOrder)
{
    HMatrix theMatrix;
    double Nq = theQuaternion.x * theQuaternion.x + theQuaternion.y * theQuaternion.y
            + theQuaternion.z * theQuaternion.z + theQuaternion.w * theQuaternion.w;
    double s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
    double xs = theQuaternion.x * s;
    double ys = theQuaternion.y * s;
    double zs = theQuaternion.z * s;
    double wx = theQuaternion.w * xs;
    double wy = theQuaternion.w * ys;
    double wz = theQuaternion.w * zs;
    double xx = theQuaternion.x * xs;
    double xy = theQuaternion.x * ys;
    double xz = theQuaternion.x * zs;
    double yy = theQuaternion.y * ys;
    double yz = theQuaternion.y * zs;
    double zz = theQuaternion.z * zs;

    theMatrix[X][X] = float(1.0 - (yy + zz));
    theMatrix[X][Y] = float(xy - wz);
    theMatrix[X][Z] = float(xz + wy);
    theMatrix[Y][X] = float(xy + wz);
    theMatrix[Y][Y] = float(1.0 - (xx + zz));
    theMatrix[Y][Z] = float(yz - wx);
    theMatrix[Z][X] = float(xz - wy);
    theMatrix[Z][Y] = float(yz + wx);
    theMatrix[Z][Z] = float(1.0 - (xx + yy));
    theMatrix[W][X] = 0.0;
    theMatrix[W][Y] = 0.0;
    theMatrix[W][Z] = 0.0;
    theMatrix[X][W] = 0.0;
    theMatrix[Y][W] = 0.0;
    theMatrix[Z][W] = 0.0;
    theMatrix[W][W] = 1.0;

    return eulerFromHMatrix(theMatrix, theOrder);
}

EulerAngles QSSGEulerAngleConverter::calculateEulerAngles(const QVector3D &rotation, EulerOrder order)
{
    EulerAngles retval;
    retval.order = order;
    const int X = 0;
    const int Y = 1;
    const int Z = 2;

    switch (order) {
    case EulerOrder::XYZs:
        retval.x = -rotation[X];
        retval.y = -rotation[Y];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::XYXs:
        retval.x = -rotation[X];
        retval.y = -rotation[Y];
        retval.z = -rotation[X];
        break;
    case EulerOrder::XZYs:
        retval.x = -rotation[X];
        retval.y = -rotation[Z];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::XZXs:
        retval.x = -rotation[X];
        retval.y = -rotation[Z];
        retval.z = -rotation[X];
        break;
    case EulerOrder::YZXs:
        retval.x = -rotation[Y];
        retval.y = -rotation[Z];
        retval.z = -rotation[X];
        break;
    case EulerOrder::YZYs:
        retval.x = -rotation[Y];
        retval.y = -rotation[Z];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::YXZs:
        retval.x = -rotation[Y];
        retval.y = -rotation[X];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::YXYs:
        retval.x = -rotation[Y];
        retval.y = -rotation[X];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::ZXYs:
        retval.x = -rotation[Z];
        retval.y = -rotation[X];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::ZXZs:
        retval.x = -rotation[Z];
        retval.y = -rotation[X];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::ZYXs:
        retval.x = -rotation[Z];
        retval.y = -rotation[Y];
        retval.z = -rotation[X];
        break;
    case EulerOrder::ZYZs:
        retval.x = -rotation[Z];
        retval.y = -rotation[Y];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::ZYXr:
        retval.x = -rotation[Z];
        retval.y = -rotation[Y];
        retval.z = -rotation[X];
        break;
    case EulerOrder::XYXr:
        retval.x = -rotation[X];
        retval.y = -rotation[Y];
        retval.z = -rotation[X];
        break;
    case EulerOrder::YZXr:
        retval.x = -rotation[Y];
        retval.y = -rotation[Z];
        retval.z = -rotation[X];
        break;
    case EulerOrder::XZXr:
        retval.x = -rotation[X];
        retval.y = -rotation[Z];
        retval.z = -rotation[X];
        break;
    case EulerOrder::XZYr:
        retval.x = -rotation[X];
        retval.y = -rotation[Z];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::YZYr:
        retval.x = -rotation[Y];
        retval.y = -rotation[Z];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::ZXYr:
        retval.x = -rotation[Z];
        retval.y = -rotation[X];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::YXYr:
        retval.x = -rotation[Y];
        retval.y = -rotation[X];
        retval.z = -rotation[Y];
        break;
    case EulerOrder::YXZr:
        retval.x = -rotation[Y];
        retval.y = -rotation[X];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::ZXZr:
        retval.x = -rotation[Z];
        retval.y = -rotation[X];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::XYZr:
        retval.x = -rotation[X];
        retval.y = -rotation[Y];
        retval.z = -rotation[Z];
        break;
    case EulerOrder::ZYZr:
        retval.x = -rotation[Z];
        retval.y = -rotation[Y];
        retval.z = -rotation[Z];
        break;
    default:
        Q_UNREACHABLE();
        retval.x = rotation[X];
        retval.y = rotation[Y];
        retval.z = rotation[Z];
        break;
    }
    return retval;
}

QVector3D QSSGEulerAngleConverter::calculateRotationVector(const EulerAngles &angles)
{
    QVector3D retval(0, 0, 0);
    const int X = 0;
    const int Y = 1;
    const int Z = 2;

    switch (angles.order) {
    case EulerOrder::XYZs:
        retval[X] = -angles.x;
        retval[Y] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::XYXs:
        retval[X] = -angles.x;
        retval[Y] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::XZYs:
        retval[X] = -angles.x;
        retval[Z] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::XZXs:
        retval[X] = -angles.x;
        retval[Z] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::YZXs:
        retval[Y] = -angles.x;
        retval[Z] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::YZYs:
        retval[Y] = -angles.x;
        retval[Z] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::YXZs:
        retval[Y] = -angles.x;
        retval[X] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::YXYs:
        retval[Y] = -angles.x;
        retval[X] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::ZXYs:
        retval[Z] = -angles.x;
        retval[X] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::ZXZs:
        retval[Z] = -angles.x;
        retval[X] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::ZYXs:
        retval[Z] = -angles.x;
        retval[Y] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::ZYZs:
        retval[Z] = -angles.x;
        retval[Y] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::ZYXr:
        retval[Z] = -angles.x;
        retval[Y] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::XYXr:
        retval[X] = -angles.x;
        retval[Y] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::YZXr:
        retval[Y] = -angles.x;
        retval[Z] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::XZXr:
        retval[X] = -angles.x;
        retval[Z] = -angles.y;
        retval[X] = -angles.z;
        break;
    case EulerOrder::XZYr:
        retval[X] = -angles.x;
        retval[Z] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::YZYr:
        retval[Y] = -angles.x;
        retval[Z] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::ZXYr:
        retval[Z] = -angles.x;
        retval[X] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::YXYr:
        retval[Y] = -angles.x;
        retval[X] = -angles.y;
        retval[Y] = -angles.z;
        break;
    case EulerOrder::YXZr:
        retval[Y] = -angles.x;
        retval[X] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::ZXZr:
        retval[Z] = -angles.x;
        retval[X] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::XYZr:
        retval[X] = -angles.x;
        retval[Y] = -angles.y;
        retval[Z] = -angles.z;
        break;
    case EulerOrder::ZYZr:
        retval[Z] = -angles.x;
        retval[Y] = -angles.y;
        retval[Z] = -angles.z;
        break;
    default:
        Q_UNREACHABLE();
        retval.setX(angles.x);
        retval.setY(angles.y);
        retval.setZ(angles.z);
        break;
    }

    return retval;
}

QMatrix4x4 QSSGEulerAngleConverter::createRotationMatrix(const QVector3D &rotationAsRadians, EulerOrder order)
{
    QMatrix4x4 matrix;
    const EulerAngles theAngles = QSSGEulerAngleConverter::calculateEulerAngles(rotationAsRadians, order);
    QSSGEulerAngleConverter::eulerToHMatrix(theAngles, *reinterpret_cast<HMatrix *>(&matrix));
    return matrix;
}

QVector3D QSSGEulerAngleConverter::calculateRotationVector(const QMatrix3x3 &rotationMatrix,
                                                           bool matrixIsLeftHanded,
                                                           EulerOrder order)
{
    QMatrix4x4 theConvertMatrix = { rotationMatrix(0, 0),
                                    rotationMatrix(0, 1),
                                    rotationMatrix(0, 2),
                                    0.0f,
                                    rotationMatrix(1, 0),
                                    rotationMatrix(1, 1),
                                    rotationMatrix(1, 2),
                                    0.0f,
                                    rotationMatrix(2, 0),
                                    rotationMatrix(2, 1),
                                    rotationMatrix(2, 2),
                                    0.0f,
                                    0.0f,
                                    0.0f,
                                    0.0f,
                                    1.0f };

    if (matrixIsLeftHanded)
        mat44::flip(theConvertMatrix);

    HMatrix *theHMatrix = reinterpret_cast<HMatrix *>(theConvertMatrix.data());
    EulerAngles theAngles = QSSGEulerAngleConverter::eulerFromHMatrix(*theHMatrix, order);
    return calculateRotationVector(theAngles);
}
QT_END_NAMESPACE