// X = Roll  = Bank
// Y = Pitch = Attitude
// Z = Yaw   = Heading

enum AxisType {
    X,Y,Z
};

static QVector3D toVector3D(AxisType a) {
    if (a == X) {
        return QVector3D(1,0,0);
    } else if (a == Y) {
        return QVector3D(0,1,0);
    } else {
        return QVector3D(0,0,1);
    }
}
static AxisType nextCircular(AxisType a) {
    if (a == X) {
        return Y;
    } else if (a == Y) {
        return Z;
    } else {
        return X;
    }
}
static qreal angle3D(const QVector3D &a, const QVector3D &b) {
    qreal angle = qAcos(QVector3D::dotProduct(a, b) / double(a.length() * b.length()));
    if (isnan(angle))
        return 0;
    return angle;
}

struct EulerOrder {
    AxisType axisOrder[3];
    EulerOrder(AxisType first, AxisType second, AxisType third) {
        axisOrder[0] = first;
        axisOrder[1] = second;
        axisOrder[2] = third;
    }
    QList<QVector3D> orderedAxis() const {
        return QList<QVector3D>() << toVector3D(axisOrder[0]) << toVector3D(axisOrder[1]) << toVector3D(axisOrder[2]);
    }
    AxisType getAxisType(int index) const {
        if ((index > 2) || (index < 0)) {
            qDebug() << "EulerOrder[index] called with an invalid index";
        }

        return axisOrder[index];
    }
    bool isCircular() const {
        return nextCircular(axisOrder[0]) == axisOrder[1];
    }
    bool isRepeating() const {
        return axisOrder[0] == axisOrder[2];
    }
};

class EulerAngleDecomposer {
    struct IndexData {
        // for all of these indices:
        // 0 = X
        // 1 = Y
        // 2 = Z

        AxisType m_i1; // zero based index of first euler rotation
        AxisType m_i1n; // next circular index following i1
        AxisType m_i1nn; // next circular index following i1n

        AxisType m_i2; // zero based index of second euler rotation
        AxisType m_i2n; // next circular index following i2
        AxisType m_i2nn; // next circular index following i2n

        AxisType m_i3; // zero based index of third euler rotation
        AxisType m_i3n; // next circular index following i3
        AxisType m_i3nn; // next circular index following i3n

        // m_unitAxis[0] = first euler rotation axis
        // m_unitAxis[1] = second euler rotation axis
        // m_unitAxis[2] = third euler rotation axis
        QList<QVector3D> m_unitAxis;

        // create from a EulerOrder
        IndexData(const EulerOrder &order) {
            m_i1 = order.getAxisType(0);
            m_i2 = order.getAxisType(1);
            m_i3 = order.getAxisType(2);

            // now populate m_ixn, ans ixnn's
            m_i1n = nextCircular(m_i1);
            m_i1nn = nextCircular(m_i1n);

            m_i2n = nextCircular(m_i2);
            m_i2nn = nextCircular(m_i2n);

            m_i3n = nextCircular(m_i3);
            m_i3nn = nextCircular(m_i3n);

            m_unitAxis = order.orderedAxis();
        }

        QVector3D V1() { return m_unitAxis[0]; } // first axis of rotation
        QVector3D V2() { return m_unitAxis[1]; } // second axis of rotation
        QVector3D V3() { return m_unitAxis[2]; } // third axis of rotation
        QVector3D V3n()  { return toVector3D(m_i3n); }
        // next axis after V3 (circular)
        // a little table:
        // V3()     -->     V3n()
        // X        -->     Y
        // Y        -->     Z
        // Z        -->     X

        AxisType i1()   { return m_i1; } // first rotation axis
        AxisType i1n()  { return m_i1n; } // next circular axis folowing i1(). not to be confused with the second axis of rotation (i2)
        AxisType i1nn() { return m_i1nn; } // next circular axis following i1n(). not to be confused with the third axis of rotation (i3)
    };
public:
    static QVector3D DecomposeFromQuaternion(const QQuaternion &q, const EulerOrder &order) {
        IndexData d(order);

        QVector3D v3Rot = q.rotatedVector(d.V3()).normalized(); // q->GetRotatedVector(d.V3()).unit();

        // recall:
        // i1;      // zero based index of first euler rotation
        // i1n;     // next circular index following i1
        // i1nn;    // next circular index following i1n

        qreal theta1 = 0;
        qreal theta2 = 0;
        qreal theta3 = 0;

        if (order.isRepeating()) {
            if (order.isCircular()) {
                // circular, repeating
                theta1 = qAtan2(v3Rot[d.i1n()], -v3Rot[d.i1nn()]);
                theta2 = -qAcos(v3Rot[d.i1()]);
            } else {
                // non circular, repeating
                theta1 = qAtan2(v3Rot[d.i1nn()], v3Rot[d.i1n()]);
                theta2 = qAcos(v3Rot[d.i1()]);
            }

            // By convention, repeating sequences restrict theta2 to 0-->180
            if (theta2 < 0) {
                // need to resolve the ambiguity restrict theta2 to 0 --> 180
                //theta2 = theta2.negate();
                theta2 = -theta2;
                //theta1 = theta1 - pi;
            }

            // special case where theta2 is zero, which is somewhat nonsense for a repeating sequence
            // in this case, put all the entire angle into theta3
            if (theta2 == 0 || theta2 == M_PI) {
                theta1 = 0;
            }
        } else {
            if (order.isCircular()) {
                // circular, non-repeating
                theta1 = qAtan2(-v3Rot[d.i1n()], v3Rot[d.i1nn()]);
                theta2 = -qAsin(-v3Rot[d.i1()]);
            } else {
                // non circular, non repeating
                theta1 = qAtan2(v3Rot[d.i1nn()], v3Rot[d.i1n()]);
                theta2 = -qAsin(v3Rot[d.i1()]);
            }
        }

        // Create the Q12 quaternion using the first two axis and angles
        QQuaternion Q1 = QQuaternion::fromAxisAndAngle(d.V1(), qRadiansToDegrees(theta1));
        QQuaternion Q2 = QQuaternion::fromAxisAndAngle(d.V2(), qRadiansToDegrees(theta2));

        QQuaternion Q12 = Q1 * Q2;

        // get the next circular vector after V3
        QVector3D V3n = d.V3n();

        // rotate V3n through Q12 and q
        QVector3D V3n12 = Q12.rotatedVector(V3n).normalized();
        QVector3D V3nG = q.rotatedVector(V3n).normalized();

        // get the angle between them - theta3
        theta3 = angle3D(V3n12, V3nG);

        // use a cross product to determine the direction of the angle
        QVector3D Vc = QVector3D::crossProduct(V3n12, V3nG).normalized();

        qreal sign = QVector3D::dotProduct(Vc, v3Rot) > 0 ? 1.0 : -1.0;

        theta3 = sign * qAbs(theta3);

        return QVector3D(qRadiansToDegrees(theta1), qRadiansToDegrees(theta2), qRadiansToDegrees(theta3));
    }
};

QQuaternion log(const QQuaternion &q) {
    qreal a = qAcos(q.scalar());
    qreal sina = qSin(a);
    QQuaternion ret;

    ret.setScalar(0);
    if (sina > 0) {
        ret.setX(a * q.x() / sina);
        ret.setY(a * q.y() / sina);
        ret.setZ(a * q.z() / sina);
    } else {
        ret.setX(0);
        ret.setY(0);
        ret.setZ(0);
    }
    return ret;
}

QQuaternion exp(const QQuaternion &q) {
    qreal a = q.vector().length();
    qreal sina = qSin(a);
    qreal cosa = qCos(a);
    QQuaternion ret;

    ret.setScalar(cosa);
    if (a > 0) {
        ret.setX(sina * q.x() / a);
        ret.setY(sina * q.y() / a);
        ret.setZ(sina * q.z() / a);
    } else {
        ret.setX(0);
        ret.setY(0);
        ret.setZ(0);
    }
    return ret;
}

bool closeEnough(const float& a, const float& b, const float& epsilon = std::numeric_limits<float>::epsilon()) {
    return (epsilon > std::abs(a - b));
}

qreal clampF(qreal f) {
    return qBound<qreal>(-1.0, f, 1.0);
}

QVector3D eulerAngles(const QMatrix3x3 &R, float prevX) {
    //check for gimbal lock
    if (closeEnough(clampF(R(0, 2)), -1.0f)) {
        float x = prevX; //gimbal lock, value of x doesn't matter
        float y = M_PI / 2;
        float z = x + qAtan2(clampF(R(1, 0)), clampF(R(2, 0)));
        return { x, y, z };
    } else if (closeEnough(clampF(R(0, 2)), 1.0f)) {
        float x = prevX;
        float y = -M_PI / 2;
        float z = -x + qAtan2(clampF(-R(1, 0)), clampF(-R(2, 0)));
        return { x, y, z };
    } else { //two solutions exist
        float x1 = -qAsin(clampF(R(0, 2)));
        float x2 = M_PI - x1;

        qDebug() << prevX << x1 << x2;
        if (qAbs(x1 - prevX) > qAbs(x2 - prevX)) {
            x1 = x2;
        }

        qreal cx1 = qCos(x1);
        qreal cx2 = qCos(x2);

        float y1 = qAtan2(clampF(R(1, 2)) / cx1, clampF(R(2, 2)) / cx1);
        float y2 = qAtan2(clampF(R(1, 2)) / cx2, clampF(R(2, 2)) / cx2);

        float z1 = qAtan2(clampF(R(0, 1)) / cx1, clampF(R(0, 0)) / cx1);
        float z2 = qAtan2(clampF(R(0, 1)) / cx2, clampF(R(0, 0)) / cx2);

        //return { qMin(x1, x2), qMin(y1, y2), qMin(z1, z2) };

        //choose one solution to return
        //for example the "shortest" rotation
        if ((std::abs(x1) + std::abs(y1) + std::abs(z1)) <= (std::abs(x2) + std::abs(y2) + std::abs(z2))) {
            return { x1, y1, z1 };
        } else {
            return { x2, y2, z2 };
        }
    }
}
QVector3D eulerAngles(const QMatrix3x3 &R) {
    // Assuming the angles are in radians.
    if (R(1, 0) > 0.998) { // singularity at north pole
        return { atan2(clampF(R(0, 2)), clampF(R(2, 2))),
                 M_PI/2,
                 0
        };
    }
    if (R(1, 0) < -0.998) { // singularity at south pole
        return { atan2(clampF(R(0, 2)), clampF(R(2, 2))),
                 -M_PI / 2,
                 0
        };
    }
    return { atan2(clampF(-R(2, 0)), clampF(R(0, 0))),
             atan2(clampF(-R(1, 2)), clampF(R(1, 1))),
             asin(clampF(R(1, 0))) };
}

QVector3D eulerAngles(const QMatrix3x3 &R) {
    float Psi, Theta, Phi;

    if (fabs(R(2, 2)) > .999999999) {
        Theta = (clampF(R(2, 2)) > 0) ? 0 : M_PI;
        Psi = 0;
        if (fabs(clampF(R(0, 0))) > .999999999)
        Phi = (clampF(R(0, 0)) > 0) ? 0 : M_PI;
        else Phi = (clampF(R(1, 0)) > 0) ? acos(clampF(R(0, 0))) : - acos(clampF(R(0, 0)));
    } else {
        Theta = acos(clampF(R(2, 2)));
        double st = sin(Theta);
        double si = clampF(R(0, 2)) / st;
        double co = - clampF(R(1, 2)) / st;
        if (fabs(co) > .999999999)
            Psi = (co > 0) ? 0 : M_PI;
        else
            Psi = (si > 0) ? acos(co) : - acos(co);
        si = clampF(R(2, 0)) / st;
        co = clampF(R(2, 1)) / st;
        if (fabs(co) > .999999999)
            Phi = (co > 0) ? 0 : M_PI;
        else
            Phi = (si > 0) ? acos(co) : - acos(co);
    }
    return { Theta, Phi, Psi };
}

QVector3D eulerAngles(const QMatrix3x3 &R) {
    QVector3D ret;
    float x(0), y(0), z(0);

    const int order = 3;
    switch (order) {
    case 1: { // XYZ
          y = qAsin(R(0, 2));
          if (y < M_PI_2) {
             if(y > -M_PI_2) {
                x = qAtan2(-R(1,2), R(2,2));
                z = qAtan2(-R(0,1), R(0,0));
             } else { // Non-unique (x - z constant)
                x = -qAtan2(R(1,0), R(1,1));
                z = 0;
             }
          } else {
             // not unique (x + z constant)
             x = qAtan2(R(1,0), R(1,1));
             z = 0;
          }
          ret = { x, y, z };
       } break;
    case 2: { // ZYX
          y = qAsin(-R(2, 0));
          if (y < M_PI_2) {
             if (y > -M_PI_2) {
                z = qAtan2(R(1, 0), R(0, 0));
                x = qAtan2(R(2, 1), R(2, 2));
             } else { // not unique (x + z constant)
                z = qAtan2(-R(0, 1),-R(0, 2));
                x = 0;
             }
          } else { // not unique (x - z constant)
             z = -qAtan2(R(0, 1), R(0, 2));
             x = 0;
          }
          ret = { z, y, x };
       } break;
    case 3: { // ZXY
          x = qAsin(R(2, 1));
          if (x < M_PI_2) {
             if (x > -M_PI_2) {
                z = qAtan2(-R(0, 1), R(1, 1));
                y = qAtan2(-R(2, 0), R(2, 2));
             } else { // not unique (y - z constant)
                z = -qAtan2(R(0, 2), R(0, 0));
                y = 0;
             }
          } else { // not unique (y + z constant)
             z = qAtan2(R(0, 2), R(0, 0));
             y = 0;
          }
          ret = { z, x, y };
       } break;
    }
    return ret;
}


int FindMinAbsIndex(float list[2]) {
   double x = qAbs(list[0]);
   int index = 0;
   for (int i = 1; i < 2; i++) {
       if (qAbs(list[i]) < x) {
           index = i;
           x = qAbs(list[i]);
       }
   }
   return index;
}

QVector3D GetEulerZXZ(const QMatrix3x3 &R) {
    // Options
    float a_list[2] = { qAtan2(-R(0, 2), R(1, 2)), qAtan2(R(0, 2), -R(1, 2)) };
    float c_list[2] = { qAtan2(R(2, 0), R(2, 1)), qAtan2(-R(2, 0), -R(2, 1)) };
    float b_list[2] = { M_PI/2.0-qAsin(R(2, 2)), -M_PI/2.0+qAsin(R(2, 2)) };

    int min_a_index = FindMinAbsIndex(a_list);
    int min_b_index = FindMinAbsIndex(b_list);
    int min_c_index = FindMinAbsIndex(c_list);

    return { a_list[min_a_index],
             b_list[min_b_index],
             c_list[min_c_index]
    };
}
QVector3D GetEulerZYX(const QMatrix3x3 &R) {
   float a_list[2] = { qAtan2(R(1, 0), R(0, 0)), qAtan2(-R(1, 0), -R(0, 0)) };
   float c_list[2] = { qAtan2(R(2, 1), R(2, 2)), qAtan2(-R(2, 1), -R(2, 2)) };
   float b_list[2] = { -qAsin(R(2, 0)), qAsin(R(2, 0)) - M_PI };

    int min_a_index = FindMinAbsIndex(a_list);
    int min_b_index = FindMinAbsIndex(b_list);
    int min_c_index = FindMinAbsIndex(c_list);

    return { a_list[min_a_index],
             b_list[min_b_index],
             c_list[min_c_index]
    };
}

double angle3D(const QVector3D &a, const QVector3D &b) {
    // Store some information about them for below
    double dot = QVector3D::dotProduct(a, b);
    double lengthA = a.length();
    double lengthB = b.length();

    // Now to find the angle
    return qAcos( dot / (lengthA * lengthB) ); // Theta = 3.06 radians or 175.87 degrees
}

double angleFromDirection(const QVector3D &upDirection, const QVector3D &lookDirection, QVector3D &Yproj) {
    Yproj = QVector3D(   -(lookDirection.y() * lookDirection.x()),
                      1 - (lookDirection.y() * lookDirection.y()),
                         -(lookDirection.y() * lookDirection.z()));
    Yproj.normalize();

    // get absolute angle between Y projected and Up
    double absAngle = angle3D(upDirection, Yproj);

    // magic formula
    QVector3D cross = QVector3D::crossProduct(upDirection, Yproj);
    double dot = QVector3D::dotProduct(lookDirection, cross);

    // set actual signed angle
    return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
}

double angleFromDirection2(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
    double absAngle = angle3D(a, b);

    QVector3D cross = QVector3D::crossProduct(a, b);
    double dot = QVector3D::dotProduct(cross, ref);

    return qRadiansToDegrees(((dot < 0) ? -absAngle : absAngle));
}

double angleFromDirection3(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
    double absAngle = angle3D(a, ref);

    QVector3D cross = QVector3D::crossProduct(a, ref);
    double dot = QVector3D::dotProduct(cross, b);

    return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
}
double angleFromDirection3a(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
    double absAngle = angle3D(a, ref);

    QVector3D cross = QVector3D::crossProduct(b, ref);
    double dot = QVector3D::dotProduct(cross, a);

    return qRadiansToDegrees(((dot >= 0) ? absAngle : -absAngle));
}

double angleFromDirection4(const QVector3D &a, const QVector3D &b) {
    QVector3D ref(b.x(), 0, b.z());
    ref.normalize();
    double absAngle = angle3D(a, b);

    QVector3D cross = QVector3D::crossProduct(a, b);
    double dot = QVector3D::dotProduct(cross, ref);

    return qRadiansToDegrees(((dot < 0) ? -absAngle : absAngle));
}


enum RotSeq { zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy, xzx };

void twoaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  res[0] = qRadiansToDegrees(qAtan2( clampF(r11), clampF(r12) ));
  res[1] = qRadiansToDegrees(qAcos ( clampF(r21 )));
  res[2] = qRadiansToDegrees(qAtan2( clampF(r31), clampF(r32) ));
}

void threeaxisrot(double r11, double r12, double r21, double r31, double r32, double res[]){
  res[0] = qRadiansToDegrees(qAtan2( clampF(r31), clampF(r32) ));
  res[1] = qRadiansToDegrees(qAsin ( clampF(r21) ));
  res[2] = qRadiansToDegrees(qAtan2( clampF(r11), clampF(r12) ));
}

void quaternion2Euler(const QQuaternion& q, double res[], RotSeq rotSeq) {
    switch(rotSeq) {
    case zyx:
      threeaxisrot( 2*(q.x()*q.y() + q.scalar()*q.z()),
                     q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                    -2*(q.x()*q.z() - q.scalar()*q.y()),
                     2*(q.y()*q.z() + q.scalar()*q.x()),
                     q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                     res);
      break;

    case zyz:
      twoaxisrot( 2*(q.y()*q.z() - q.scalar()*q.x()),
                   2*(q.x()*q.z() + q.scalar()*q.y()),
                   q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                   2*(q.y()*q.z() + q.scalar()*q.x()),
                  -2*(q.x()*q.z() - q.scalar()*q.y()),
                  res);
      break;

    case zxy:
      threeaxisrot( -2*(q.x()*q.y() - q.scalar()*q.z()),
                      q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                      2*(q.y()*q.z() + q.scalar()*q.x()),
                     -2*(q.x()*q.z() - q.scalar()*q.y()),
                      q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                      res);
      break;

    case zxz:
      twoaxisrot( 2*(q.x()*q.z() + q.scalar()*q.y()),
                  -2*(q.y()*q.z() - q.scalar()*q.x()),
                   q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                   2*(q.x()*q.z() - q.scalar()*q.y()),
                   2*(q.y()*q.z() + q.scalar()*q.x()),
                   res);
      break;

    case yxz:
      threeaxisrot( 2*(q.x()*q.z() + q.scalar()*q.y()),
                     q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                    -2*(q.y()*q.z() - q.scalar()*q.x()),
                     2*(q.x()*q.y() + q.scalar()*q.z()),
                     q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                     res);
      break;

    case yxy:
      twoaxisrot( 2*(q.x()*q.y() - q.scalar()*q.z()),
                   2*(q.y()*q.z() + q.scalar()*q.x()),
                   q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                   2*(q.x()*q.y() + q.scalar()*q.z()),
                  -2*(q.y()*q.z() - q.scalar()*q.x()),
                  res);
      break;

    case yzx:
      threeaxisrot( -2*(q.x()*q.z() - q.scalar()*q.y()),
                      q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                      2*(q.x()*q.y() + q.scalar()*q.z()),
                     -2*(q.y()*q.z() - q.scalar()*q.x()),
                      q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                      res);
      break;

    case yzy:
      twoaxisrot( 2*(q.y()*q.z() + q.scalar()*q.x()),
                  -2*(q.x()*q.y() - q.scalar()*q.z()),
                   q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                   2*(q.y()*q.z() - q.scalar()*q.x()),
                   2*(q.x()*q.y() + q.scalar()*q.z()),
                   res);
      break;

    case xyz:
      threeaxisrot( -2*(q.y()*q.z() - q.scalar()*q.x()),
                    q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z(),
                    2*(q.x()*q.z() + q.scalar()*q.y()),
                   -2*(q.x()*q.y() - q.scalar()*q.z()),
                    q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                    res);
      break;

    case xyx:
      twoaxisrot( 2*(q.x()*q.y() + q.scalar()*q.z()),
                  -2*(q.x()*q.z() - q.scalar()*q.y()),
                   q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                   2*(q.x()*q.y() - q.scalar()*q.z()),
                   2*(q.x()*q.z() + q.scalar()*q.y()),
                   res);
      break;

    case xzy:
      threeaxisrot( 2*(q.y()*q.z() + q.scalar()*q.x()),
                     q.scalar()*q.scalar() - q.x()*q.x() + q.y()*q.y() - q.z()*q.z(),
                    -2*(q.x()*q.y() - q.scalar()*q.z()),
                     2*(q.x()*q.z() + q.scalar()*q.y()),
                     q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                     res);
      break;

    case xzx:
      twoaxisrot( 2*(q.x()*q.z() - q.scalar()*q.y()),
                   2*(q.x()*q.y() + q.scalar()*q.z()),
                   q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z(),
                   2*(q.x()*q.z() + q.scalar()*q.y()),
                  -2*(q.x()*q.y() - q.scalar()*q.z()),
                  res);
      break;
    default:
      qDebug() << "Unknown rotation sequence";
      break;
   }
}


qreal trace(const QMatrix3x3 &m) {
    return m(0,0) + m(1, 1) + m(2,2);
}

QMatrix3x3 rotationMatrix(qreal roll, qreal pitch, qreal yaw) {
    /*
    Return the rotation matrix when given the three Euler angles: roll, pitch and yaw.

    roll: cw rotation of the body about the x axis of the fixed frame (phi)
    pitch: cw rotation of the body about the y axis of the fixed frame (theta)
    yaw: cw rotation of the body about the z axis of the fixed frame (psi)
    This rotation matrix can be used to map any vector in the moving reference
    frame to the corresponding vector in the fixed frame by pre-multiplying it.

    By convention, the fixed z axis points down toward the center of the earth,
    the x axis points North and the y axis points East.
    A positive pitch is upward, a positive roll is to the right, a positive yaw is a right turn.

    >>> T = rotationMatrix(0,0,0)
    >>> import sys
    >>> abs(T-eye(3)) < EPS
    array([[ True,  True,  True],
           [ True,  True,  True],
           [ True,  True,  True]], dtype=bool)

    >>> T = rotationMatrix(pi/2,pi/2,pi/2)
    >>> abs(T-array([[0.,0,1],[0,1,0],[-1,0,0]])) < EPS
    array([[ True,  True,  True],
           [ True,  True,  True],
           [ True,  True,  True]], dtype=bool)

    >>> T = rotationMatrix(pi/4,0,0)
    >>> T
    array([[ 1.        ,  0.        ,  0.        ],
           [ 0.        ,  0.70710678, -0.70710678],
           [-0.        ,  0.70710678,  0.70710678]])
    >>> abs(T-array([[1.,0,0],[0,sqrt(2)/2,-sqrt(2)/2],[0,sqrt(2)/2,sqrt(2)/2]])) < EPS
    array([[ True,  True,  True],
           [ True,  True,  True],
           [ True,  True,  True]], dtype=bool)

    */
    QMatrix3x3 T;
    T(0,0)=cos(pitch)*cos(yaw);
    T(1,0)=cos(pitch)*sin(yaw);
    T(2,0)=-sin(pitch);
    T(0,1)=-cos(roll)*sin(yaw) + sin(roll)*sin(pitch)*cos(yaw);
    T(1,1)=cos(roll)*cos(yaw) + sin(roll)*sin(pitch)*sin(yaw);
    T(2,1)=sin(roll)*cos(pitch);
    T(0,2)=sin(roll)*sin(yaw) + cos(roll)*sin(pitch)*cos(yaw);
    T(1,2)=-sin(roll)*cos(yaw) + cos(roll)*sin(pitch)*sin(yaw);
    T(2,2)=cos(roll)*cos(pitch);
    return T;
}

QVector4D eulerParameters(const QMatrix3x3 &T) {
    /*
    Given a rotation matrix, compute the four Euler parameters.

    These parameters represent a 4-D vector that defines an axis such that if the
    xyz axes of the moving frame are rotated about it by an angle phi they will
    line up with the XYZ of the fixed frame and vice-versa.

    See: http://www.u.arizona.edu/~pen/ame553/Notes/Lesson%2009.pdf

    >>> es = eulerParameters(eye(3))
    >>> abs(array(es)-array([1.,0,0,0])) < EPS
    array([ True,  True,  True,  True], dtype=bool)

    >>> T = rotationMatrix(pi/4,0,0)
    >>> es = eulerParameters(T)
    >>> abs(array(es)-array((0.92387953251128674, 0.38268343236508973, 0.0, 0.0))) < EPS
    array([ True,  True,  True,  True], dtype=bool)
    */
    qreal tra = trace(T);
    // take the positive square roots as a starting point
    qreal e0 = sqrt(tra + 1.)/2.;
    qreal e1 = sqrt(1+2*T(0,0)-tra)/2.;
    qreal e2 = sqrt(1+2*T(1,1)-tra)/2.;
    qreal e3 = sqrt(1+2*T(2,2)-tra)/2.;
    QList<qreal> es;
    es << e0;
    es << e1;
    es << e2;
    es << e3;
    int eimax = es.indexOf(qMax(qMax(qMax(e0, e1), e2), e3));

    // for best numerical stability, take the largest parameter as positive
    // and use it to re-compute the others with correct signs
    if (eimax == 0) {
        e1 = (T(2,1)-T(1,2))/4/e0;
        e2 = (T(0,2)-T(2,0))/4/e0;
        e3 = (T(1,0)-T(0,1))/4/e0;
    } else if (eimax == 1) {
        e0 = (T(2,1)-T(1,2))/4/e1;
        e2 = (T(1,0)+T(0,1))/4/e1;
        e3 = (T(0,2)+T(2,0))/4/e1;
    } else if (eimax == 2) {
        e0 = (T(0,2)-T(2,0))/4/e2;
        e1 = (T(1,0)+T(0,1))/4/e2;
        e3 = (T(2,1)+T(1,2))/4/e2;
    } else {
        e0 = (T(1,0)-T(0,1))/4/e3;
        e1 = (T(0,2)+T(2,0))/4/e3;
        e2 = (T(2,1)+T(1,2))/4/e3;
    }
    return QVector4D(e0,e1,e2,e3);
}

QVector3D eulerAngles1(const QVector4D &es) {
    /*
    Compute the roll, pitch and yaw from a tuple of the four Euler parameters.

    Note: this function is not intended to be used at the singular points
    where pitch = +/- pi/2.  In general, you should use getEulerAngles() instead.
    That function will handle the singular cases and call this function in the
    non-singular cases

    >>> angles = (pi/6,pi/2-2*EPS,-pi/4)
    >>> T = rotationMatrix(*angles)
    >>> es = eulerParameters(T)
    >>> newangles = eulerAngles(es)
    Traceback (most recent call last):
    ...
    AssertionError

    >>> angles = (pi/6,pi/2-5*EPS,-pi/4)
    >>> T = rotationMatrix(*angles)
    >>> es = eulerParameters(T)
    >>> newangles = eulerAngles(es)
    >>> err = abs(array(angles)-array(newangles))
    >>> err[0] < .03
    True
    >>> err[1] < 1.e-7
    True
    >>> err[2] < .06
    True
    */
    // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
    // note arc
    qreal e0 = es.x();
    qreal e1 = es.y();
    qreal e2 = es.z();
    qreal e3 = es.w();

    float EPS = std::numeric_limits<float>::epsilon();

    qreal dy_roll = 2.0*(e0*e1 + e2*e3);
    qreal dx_roll = (1.0 - 2.0*(e1*e1 + e2*e2));
    qreal dy_yaw = 2.0*(e0*e3 + e1*e2);
    qreal dx_yaw = (1.0 - 2.0*(e2*e2 + e3*e3));
    // if both the dx and dy are tiny, the arctan is undefined.  Force it to zero to avoid numerical issues.
    // This can happen when the pitch is +/- pi/2 and the roll and yaw are both even or odd multiples of pi.
    // But forcing
    Q_ASSERT ((qAbs(dx_roll) > EPS || qAbs(dy_roll) > EPS) && (qAbs(dx_yaw) > EPS || qAbs(dy_yaw) > EPS));

    qreal roll  = qAbs(dx_roll) > EPS || qAbs(dy_roll) > EPS? qAtan2(dy_roll, dx_roll) : 0.;
    qreal pitch = qAsin(2.0*(e0*e2 - e3*e1));
    qreal yaw   = qAbs(dx_yaw) > EPS || qAbs(dy_yaw) > EPS? qAtan2(dy_yaw, dx_yaw) : 0.;

    return QVector3D(roll, pitch, yaw);
}

QVector3D getEulerAngles(const QMatrix3x3 &T) {
    /*
    Compute the roll, pitch and yaw from a rotation matrix.

    That function handles the singular cases directly and calls eulerParameters()
    for non-singular cases

    >>> angles = (pi/6,pi/2-2*EPS,-pi/4)
    >>> T = rotationMatrix(*angles)
    >>> newangles = getEulerAngles(T)
    >>> abs(angles[0]-angles[2]-newangles[0]) < EPS
    True
    >>> abs(angles[1]-newangles[1]) < 5*EPS
    True

    >>> angles = (pi/6,pi/2-5*EPS,-pi/4)
    >>> T = rotationMatrix(*angles)
    >>> es = eulerParameters(T)
    >>> newangles = eulerAngles(es)
    >>> err = abs(array(angles)-array(newangles))
    >>> err[0] < .03
    True
    >>> err[1] < 1.e-7
    True
    >>> err[2] < .06
    True
    */
    float EPS = std::numeric_limits<float>::epsilon();

    if (qAbs(T(0,0)) < 4*EPS && qAbs(T(1,0)) < 4*EPS && qAbs(T(2,1)) < 4*EPS && qAbs(T(2,2)) < 4*EPS) {
        qreal p, y, r;
        if (T(2,0) < 0.) {
            p = M_PI/2;
            y = 0;
            r = qAtan2(T(0,1),T(0,2));
        } else {
            p = -M_PI/2;
            y = 0;
            r = qAtan2(-T(0,1),-T(0,2));
        }
        return QVector3D(r, p, y);
    } else {
        QVector4D ep = eulerParameters(T);
        return eulerAngles1(ep);
    }
}



QVector3D correctedEulerAngles(const QVector3D &angles, const QMatrix3x3 &Tc) {
    /*
    Compute the corrected Euler angles from measured values and the correction matrix

    Given a set of Euler angles (e.g. measurement data from the box)
    and a correction matrix Tc, which takes coordinates from the plane to the box,
    calculate a set of corrected Euler angles which represent the true roll, pitch
    and yaw of the airplane.

    >>> Tc = getCorrectionMatrix(.5, .5, .5)
    >>> ea = correctedEulerAngles(.9, -.2, .6, Tc)
    >>> abs(array(ea) - array((0.73182631479752158, -0.35738772904903454, -0.10266899093274005))) < EPS
    array([ True,  True,  True], dtype=bool)

    */
    // get the matrix which maps from the box to the world system (measured)
    QMatrix3x3 Tk = rotationMatrix(angles.x(), angles.y(), angles.z());

    // T0 maps from the plane to the box system so multiplying takes us from plane to world.
    QMatrix3x3 T1 = Tk * Tc;

    QVector4D es = eulerParameters(T1);
    QVector3D result = eulerAngles1(es);
    return result;
}



QVector3D xAxis(const QQuaternion &q)   {
    qreal fTx  = 2.0f*q.x();
    qreal fTy  = 2.0f*q.y();
    qreal fTz  = 2.0f*q.z();
    qreal fTwy = fTy*q.scalar();
    qreal fTwz = fTz*q.scalar();
    qreal fTxy = fTy*q.x();
    qreal fTxz = fTz*q.x();
    qreal fTyy = fTy*q.y();
    qreal fTzz = fTz*q.z();

    return QVector3D(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
}

QVector3D projectedX(const QVector3D &q)   {
    qreal fTxy = 2.0f*q.y()*q.x();
    qreal fTxz = 2.0f*q.z()*q.x();
    qreal fTyy = 2.0f*q.y()*q.y();
    qreal fTzz = 2.0f*q.z()*q.z();

    return QVector3D(1.0f-(fTyy+fTzz), fTxy, fTxz);
}
//-----------------------------------------------------------------------
QVector3D yAxis(const QQuaternion &q)   {
    qreal fTx  = 2.0f*q.x();
    qreal fTy  = 2.0f*q.y();
    qreal fTz  = 2.0f*q.z();
    qreal fTwx = fTx*q.scalar();
    qreal fTwz = fTz*q.scalar();
    qreal fTxx = fTx*q.x();
    qreal fTxy = fTy*q.x();
    qreal fTyz = fTz*q.y();
    qreal fTzz = fTz*q.z();

    return QVector3D(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
}
QVector3D projectedY(const QVector3D &q)   {
    qreal fTxx = 2.0f*q.x()*q.x();
    qreal fTxy = 2.0f*q.y()*q.x();
    qreal fTyz = 2.0f*q.z()*q.y();
    qreal fTzz = 2.0f*q.z()*q.z();

    return QVector3D(fTxy, 1.0f-(fTxx+fTzz), fTyz);
}
//-----------------------------------------------------------------------
QVector3D zAxis(const QQuaternion &q) {
    qreal fTx  = 2.0f*q.x();
    qreal fTy  = 2.0f*q.y();
    qreal fTz  = 2.0f*q.z();
    qreal fTwx = fTx*q.scalar();
    qreal fTwy = fTy*q.scalar();
    qreal fTxx = fTx*q.x();
    qreal fTxz = fTz*q.x();
    qreal fTyy = fTy*q.y();
    qreal fTyz = fTz*q.y();

    return QVector3D(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
}
QVector3D projectedZ(const QVector3D &q) {
    qreal fTxx = 2.0f*q.x()*q.x();
    qreal fTxz = 2.0f*q.z()*q.x();
    qreal fTyy = 2.0f*q.y()*q.y();
    qreal fTyz = 2.0f*q.z()*q.y();

    return QVector3D(fTxz, fTyz, 1.0f-(fTxx+fTyy));
}
qreal getRoll(const QQuaternion &q, bool reprojectAxis) {
    if (reprojectAxis)    {
        // roll = atan2(localx.y, localx.x)
        // pick parts of xAxis() implementation that we need
//          Real fTx  = 2.0*x;
        qreal fTy  = 2.0f*q.y();
        qreal fTz  = 2.0f*q.z();
        qreal fTwz = fTz*q.scalar();
        qreal fTxy = fTy*q.x();
        qreal fTyy = fTy*q.y();
        qreal fTzz = fTz*q.z();

        // Vector3(1.0-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);

        return qAtan2(fTxy+fTwz, 1.0f-(fTyy+fTzz));

    }    else    {
        return qAtan2(2*(q.x()*q.y() + q.scalar()*q.z()), q.scalar()*q.scalar() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
    }
}
//-----------------------------------------------------------------------
qreal getPitch(const QQuaternion &q, bool reprojectAxis) {
    if (reprojectAxis)    {
        // pitch = atan2(localy.z, localy.y)
        // pick parts of yAxis() implementation that we need
        qreal fTx  = 2.0f*q.x();
//          Real fTy  = 2.0f*y;
        qreal fTz  = 2.0f*q.z();
        qreal fTwx = fTx*q.scalar();
        qreal fTxx = fTx*q.x();
        qreal fTyz = fTz*q.y();
        qreal fTzz = fTz*q.z();

        // Vector3(fTxy-fTwz, 1.0-(fTxx+fTzz), fTyz+fTwx);
        return qAtan2(fTyz+fTwx, 1.0f-(fTxx+fTzz));
    }    else    {
        // internal version
        return qAtan2(2*(q.y()*q.z() + q.scalar()*q.x()), q.scalar()*q.scalar() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
    }
}
//-----------------------------------------------------------------------
qreal getYaw(const QQuaternion &q, bool reprojectAxis) {
    if (reprojectAxis)    {
        // yaw = atan2(localz.x, localz.z)
        // pick parts of zAxis() implementation that we need
        qreal fTx  = 2.0f*q.x();
        qreal fTy  = 2.0f*q.y();
        qreal fTz  = 2.0f*q.z();
        qreal fTwy = fTy*q.scalar();
        qreal fTxx = fTx*q.x();
        qreal fTxz = fTz*q.x();
        qreal fTyy = fTy*q.y();

        // Vector3(fTxz+fTwy, fTyz-fTwx, 1.0-(fTxx+fTyy));

        return qAtan2(fTxz+fTwy, 1.0f-(fTxx+fTyy));

    }    else    {
        // internal version
        return qAsin(-2*(q.x()*q.z() - q.scalar()*q.y()));
    }
}

double angleFromDirection8(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
    double absAngle = angle3D(a, b);

    QVector3D cross = QVector3D::crossProduct(a, b);
    double dot = QVector3D::dotProduct(cross, ref);

    return qRadiansToDegrees(((dot < 0) ? absAngle : absAngle));
}


// Decompose rotation by rotation of direction vector and twist, around direction vector composite = without_twist * twist == orientation
void dir_twist_decomposition(const xxquaternion& orientation, const vector3& default_dir, xxquaternion& dir_rotation, xxquaternion& twist_rotation) {
    vector3 rotated_dir(orientation.rotate(default_dir));
    dir_rotation.shortest_arc(default_dir, rotated_dir);
    twist_rotation = dir_rotation.inverted() * orientation;
}
void dir_twist_decomposition_2(const xxquaternion& orientation, const vector3& default_dir, xxquaternion& dir_rotation, xxquaternion& twist_rotation) {
    vector3 rotation_axis(orientation.x, orientation.y, orientation.z); // return projection v1 on to v2 (parallel component)
    // here can be optimized if default_dir is unit
    vector3 proj = projection(rotation_axis, default_dir);
    twist_rotation.set(proj.x, proj.y, proj.z, orientation.w);
    twist_rotation.normalize();
    dir_rotation = orientation * twist_rotation.inverted();
}*/
/*
   Decompose the rotation on to 2 parts.
   1. Twist - rotation around the "direction" vector
   2. Swing - rotation around axis that is perpendicular to "direction" vector
   The rotation can be composed back by
   rotation = swing * twist

   has singularity in case of swing_rotation close to 180 degrees rotation.
   if the input quaternion is of non-unit length, the outputs are non-unit as well
   otherwise, outputs are both unit
*/
void swing_twist_decomposition(const QQuaternion &rotation, const QVector3D &direction, QQuaternion &swing, QQuaternion &twist) {
    QVector3D ra(rotation.x(), rotation.y(), rotation.z()); // rotation axis
    QVector3D p = (QVector3D::dotProduct(ra, direction) * direction); //projection(ra, direction); // return projection v1 on to v2  (parallel component)

    twist = QQuaternion(rotation.scalar(), p.x(), p.y(), p.z()).normalized();
    swing = rotation * twist.conjugated();
}
void toTwistSwing(const QQuaternion &q, qreal &tw, qreal &sx, qreal &sy) {
    // First test if the swing is in the singularity:
    if (qFuzzyIsNull(q.z()) && qFuzzyIsNull(q.scalar())) { sx = sy = M_PI; tw = 0; return; }
    qreal qw, qx, qy, qz;

    if (q.scalar() < 0) {
        qw = -q.scalar();
        qx = -q.x();
        qy = -q.y();
        qz = -q.z();
    } else {
        qw = q.scalar();
        qx = q.x();
        qy = q.y();
        qz = q.z();
    }

    qreal t = atan2(qz, qw);
    qreal s = atan2(sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw));
    qreal x=0.0, y=0.0, sins=sin(s);

    if (!qFuzzyIsNull(sins)) {
        qreal sint = sin(t);
        qreal cost = cos(t);
        y = (-qx*sint + qy*cost)/sins;
        x = ( qx*cost + qy*sint)/sins;
    }

    tw = qRadiansToDegrees(2.0*t);
    sx = qRadiansToDegrees(2.0*x*s);
    sy = qRadiansToDegrees(2.0*y*s);
}

void toSwingTwist(const QQuaternion &q, qreal &sx, qreal &sy, qreal &tw) {
    if (qFuzzyIsNull(q.z()) && qFuzzyIsNull(q.scalar())) { sx = sy = M_PI; tw = 0; return; }
    qreal qw, qx, qy, qz;

    if (q.scalar() < 0) {
        qw = -q.scalar();
        qx = -q.x();
        qy = -q.y();
        qz = -q.z();
    } else {
        qw = q.scalar();
        qx = q.x();
        qy = q.y();
        qz = q.z();
    }

    // Get the twist t:
    qreal t = 2.0 * atan2(qz,qw);

    qreal bet = atan2(sqrt(qx*qx+qy*qy), sqrt(qz*qz+qw*qw));
    qreal gam = t/2.0;
    qreal sinc = qFuzzyIsNull(bet)? 1.0 : sin(bet) / bet;
    qreal singam = sin(gam);
    qreal cosgam = cos(gam);

    sx = qRadiansToDegrees((2.0/sinc) * (cosgam*qx - singam*qy));
    sy = qRadiansToDegrees((2.0/sinc) * (singam*qx + cosgam*qy));
    tw = qRadiansToDegrees(t);
}

qreal len2(qreal x, qreal y, qreal z, qreal w) {
    return x * x + y * y + z * z + w * w;
}
qreal getAngleAround(const QQuaternion &q, const QVector3D &axis) {
    qreal d = QVector3D::dotProduct(q.vector(), axis);
    qreal l2 = len2(axis.x() * d, axis.y() * d, axis.z() * d, q.scalar());
    return qRadiansToDegrees(qFuzzyIsNull(l2) ? 0 : (2.0 * qAcos(qBound<qreal>(-1, (q.scalar() / qSqrt(l2)), 1))));
}

static QQuaternion OrthoX = QQuaternion::fromAxisAndAngle(1, 0, 0, 90); // X
static QQuaternion OrthoY = QQuaternion::fromAxisAndAngle(0, 1, 0, 90); // Y

void FindOrthonormals(const QVector3D &normal, QVector3D &orthonormal1, QVector3D &orthonormal2) {
    QVector3D w = OrthoX.rotatedVector(normal);
    float dot = QVector3D::dotProduct(normal, w);
    if (qAbs(dot) > 0.6) {
        w = OrthoY.rotatedVector(normal);
    }
    w.normalize();

    orthonormal1 = QVector3D::crossProduct(normal, w).normalized();
    orthonormal2 = QVector3D::crossProduct(normal, orthonormal1).normalized();
}
qreal FindQuaternionTwist(const QQuaternion &q, QVector3D axis) {
    axis.normalize();
    //get the plane the axis is a normal of
    QVector3D orthonormal1, orthonormal2;
    FindOrthonormals(axis, orthonormal1, orthonormal2);

    QVector3D transformed = q.rotatedVector(orthonormal1);

    // project transformed vector onto plane
    QVector3D flattened = transformed - (QVector3D::dotProduct(transformed, axis) * axis);
    flattened.normalize();

    // get angle between original vector and projected transform to get angle around normal
    return qRadiansToDegrees(qAcos(QVector3D::dotProduct(orthonormal1, flattened)));
}

qreal angleFromDirection2(const QVector3D &a, const QVector3D &b, const QVector3D &ref) {
    qreal absAngle = angle3D(a, b);

    QVector3D cross = QVector3D::crossProduct(a, b);
    qreal dot = QVector3D::dotProduct(cross, ref);

    return (dot < 0) ? -absAngle : absAngle;
}


QMatrix4x4 la;
la.lookAt(m_rotationVectorZ2, m_rotationVectorZ, m_rotationVectorY);
m_rotationMatrix = QMatrix4x4(la.normalMatrix());


qreal pitch = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 0, 1).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized());
qreal roll  = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, 0, -1)).normalized());
qreal yaw   = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized());

QQuaternion quat = QQuaternion::fromRotationMatrix(mat);

QQuaternion diffQuater = quat * m_prevQuat.inverted();
QQuaternion conjBoxQuater = m_prevQuat.conjugated();
QQuaternion velQuater = 2.0 * exp(log(diffQuater) / d_time) * conjBoxQuater;
//QQuaternion velQuater = ((diffQuater * 2.0) / d_time) * conjBoxQuater;



float angleInDegrees;
QVector3D rotationAxis;
velQuater.getAxisAndAngle(&rotationAxis, &angleInDegrees);
qDebug() << angleInDegrees << velQuater.scalar();

QVector3D angularDisplacement = rotationAxis * qDegreesToRadians(angleInDegrees);
QVector3D angularSpeed = angularDisplacement / d_time;
QVector3D relAngularSpeed = rotMat * angularSpeed;

QVector3D angles = velQuater.vector();//diffQuater.toEulerAngles();*/

qreal roll = angles[0];
qreal pitch = angles[1];
qreal yaw = angles[2];


m_prevQuat = quat;
//bool bHoldAttitude = false;


// BODY ROTATION VELOCITY (PITCH, YAW)
qreal fCosQx = qCos(pkUserAC->m_kData.m_kBodyAttitude.x());
qreal fSinQx = qSin(pkUserAC->m_kData.m_kBodyAttitude.x());
qreal fCosQy = qCos(pkUserAC->m_kData.m_kBodyAttitude.y());
qreal fSinQy = qSin(pkUserAC->m_kData.m_kBodyAttitude.y());
QVector3D kLocalUserAcAttitudeVec = QVector3D(fCosQx * fSinQy, -fSinQx, fCosQx * fCosQy);

fCosQx = qCos(m_kData.m_kBodyAttitude.x());
fSinQx = qSin(m_kData.m_kBodyAttitude.x());
fCosQy = qCos(m_kData.m_kBodyAttitude.y());
fSinQy = qSin(m_kData.m_kBodyAttitude.y());
QVector3D kLocalAcAttitudeVec = QVector3D(fCosQx * fSinQy, -fSinQx, fCosQx * fCosQy);

QVector3D kLocalAcRotationVec = QVector3D::crossProduct(kLocalAcAttitudeVec, kLocalUserAcAttitudeVec);
QVector3D kLocalAcRotationNormalizedVec = kLocalAcRotationVec.normalized();
QVector3D kBodyAcRotationVec = LocalToBodyTransformMatrix.mapVector(kLocalAcRotationNormalizedVec);
qreal fAngularVelocity = 132.1 * (1.0 - QVector3D::dotProduct(kLocalUserAcAttitudeVec, kLocalAcAttitudeVec));
kBodyAcRotationVec = fAngularVelocity * kBodyAcRotationVec;

// BODY ROTATION VELOCITY (ROLL)
QVector3D kBodyUserAcBankVec = QVector3D(qCos(pkUserAC->m_kData.m_kBodyAttitude.z()), qSin(pkUserAC->m_kData.m_kBodyAttitude.z()), 0);
QVector3D kBodyAcBankVec = QVector3D(qCos(m_kData.m_kBodyAttitude.z()), qSin(m_kData.m_kBodyAttitude.z()), 0);
QVector3D kBodyAcRollVec = QVector3D::crossProduct(kBodyAcBankVec, kBodyUserAcBankVec);
QVector3D kBodyAcRollNormalizedVec = kBodyAcRollVec.normalized();
FLOAT fRollVelocity = 132.1 * (1.0 - QVector3D::dotProduct(kBodyUserAcBankVec, kBodyAcBankVec));
kBodyAcRollVec = fRollVelocity * kBodyAcRollNormalizedVec;

kSimWriteData.m_kBodyRotationVelocity = bHoldAttitude ? QVector3D(kBodyAcRotationVec.x(), kBodyAcRotationVec.y(), kBodyAcRollVec.z()) : QVector3D(0, 0, 0);
kSimWriteData.m_kBodyVelocity = pkUserAC->m_kData.m_kBodyVelocity + QVector3D(kBodyAcToFormVelocity.x(), kBodyAcToFormVelocity.y(), kBodyAcToFormVelocity.z());



qreal pitch = angle3D(m_rotationMatrix * QVector3D(0, 1, 0), QVector3D(0, 0, 1));
qreal roll  = angle3D(m_rotationMatrix * QVector3D(0, 1, 0), QVector3D(1, 0, 0));
qreal yaw   = angle3D(m_rotationMatrix * QVector3D(0, 0, 1), QVector3D(1, 0, 0));


QVector3D V0 = m_rotationVectorY;
QVector3D V1 = m_rotationVectorX;
QVector3D V2 = m_rotationVectorX2;
QVector3D Normal = QVector3D::crossProduct(V1 - V0, V2 - V0).normalized();
qreal a = Normal.x();
qreal b = Normal.y();
qreal c = Normal.z();
qreal d = -QVector3D::dotProduct(V0, Normal);

QVector3D finalVector = QVector3D(-(m_rotationVectorX.y() * m_rotationVectorY.x()),
                                1 -(m_rotationVectorX.y() * m_rotationVectorY.y()),
                                  -(m_rotationVectorX.y() * m_rotationVectorY.z()));

finalVector.normalize();
QVector3D test2;
float ang;
qq.getAxisAndAngle(&test2, &ang);
QVector3D finalVector = m_rotationVectorY + Normal * d;
QQuaternion test = (qq * QQuaternion::fromAxisAndAngle(finalVector, -ang));

m_rotationVectorZ2 = projectedZ(m_rotationVectorZ);
m_rotationVectorX2 = projectedX(m_rotationVectorX);
m_rotationVectorY2 = projectedY(m_rotationVectorY);


//pitch = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 0, -1).normalized(), QVector3D(0, 0, 0)) - 90;
//roll = -(angleFromDirection(QVector3D(-1, 0, 0).normalized() * QMatrix4x4(mat), QVector3D(0, 0, 0)) - 90);
//pitch = -(angleFromDirection2(v1.normalized() * m_rotationMatrix2, v2.normalized(), v3.normalized()) - 90);
//pitch = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 1, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized());
//pitch = -(angleFromDirection4(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), (QVector3D(0, 0, 1)).normalized()) - 90);
//roll  =   angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), QVector3D(0, 0, -1).normalized() ) - 90;
//yaw   =   angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), QVector3D(0, -1, 0).normalized() ) - 90;
//m_rotationVectorY = (projectedY((m_rotationVectorY)));

//m_rotationVectorY2 += QVector3D(m_rotationVectorY.x(), 0, 0);

qq = QQuaternion::fromRotationMatrix(mat.transposed());
QQuaternion testtt = (QQuaternion::fromDirection(m_rotationVectorX, QVector3D(0, 1, 0)) * qq);
m_rotationMatrix = QMatrix4x4(testtt.toRotationMatrix());
testtt.getEulerAngles(&pitch, &yaw, &roll);

testtt = (QQuaternion::fromDirection(m_rotationVectorZ, m_rotationVectorY) * qq);
testtt.getEulerAngles(&pitch, &yaw, &roll);*/

m_rotationVectorY = (QQuaternion::fromDirection(m_rotationVectorX, m_rotationVectorY) * qq).vector();

m_rotationVectorY = QVector3D(0,
                             m_rotationVectorY.y() - m_rotationVectorY.x(),
                             m_rotationVectorY.z());


pitch = angleFromDirection3(m_rotationVectorY, QVector3D(1, 0, 0), (m_rotationVectorY2)) - 90;
roll  = angleFromDirection3(m_rotationVectorX, (projectedZ(projectedX(m_rotationVectorX))), projectedY(m_rotationVectorY));
yaw   = angleFromDirection3(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized()) - 90;
pitch = (angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(0, 0, 1).normalized(), m_rotationMatrix.mapVector(QVector3D(-1, 0, -1)).normalized()) + 90);
roll  = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 1, 0)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, 0, -1)).normalized()) - 90;
yaw   = angleFromDirection2(m_rotationMatrix.mapVector(QVector3D(0, 0, 1)).normalized(), QVector3D(1, 0, 0).normalized(), m_rotationMatrix.mapVector(QVector3D(0, -1, 0)).normalized()) - 90;
pitch = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 1, 0).normalized(), QVector3D(-1, 0, -1).normalized() * m_rotationMatrix);

//mat = mat.transposed();
//------ to find angle of rotaton of nose
QVector3D world_ref(0, 1, 0); // world.up
QVector3D nose        = QVector3D(mat(2, 0), mat(2, 1), mat(2, 2)); // transform.forward;

QVector3D nose_projection = QVector3D(0, nose.y(), 0).normalized();
QVector3D nose_axis       = QVector3D::crossProduct(world_ref, nose_projection).normalized();
//------ nose rotate axis
QQuaternion q1 = QQuaternion::rotationTo(world_ref,nose_axis);
QQuaternion q2 = QQuaternion::rotationTo(nose,     nose_axis);
float ax = qRadiansToDegrees(QQuaternion::dotProduct(q1, q2));
ax = ax - 90;

//------ to find angle of rotaton of wing
QVector3D wing        = QVector3D(mat(0, 0), mat(0, 1), mat(0, 2)); // transform.right;
QVector3D aux_axis    = QVector3D::crossProduct(world_ref,  nose).normalized();
QVector3D wing_ref    = QVector3D::crossProduct(nose,       aux_axis).normalized();

QQuaternion q3 = QQuaternion::rotationTo(wing_ref, nose);
QQuaternion q4 = QQuaternion::rotationTo(wing,     nose);
float az = qRadiansToDegrees(QQuaternion::dotProduct(q3, q4));

if (QVector3D::dotProduct(wing, nose_axis) > 0) {
    az = 360 - az;
}
az = az-90;

pitch = -angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(0, 0, 1).normalized(), QVector3D(-1, 0, -1).normalized() * m_rotationMatrix);
roll  = angleFromDirection2(QVector3D(0, 1, 0).normalized() * m_rotationMatrix, QVector3D(1, 0, 0).normalized(), QVector3D(0, 0, -1).normalized() * m_rotationMatrix);
yaw   = angleFromDirection2(QVector3D(0, 0, 1).normalized() * m_rotationMatrix, QVector3D(1, 0, 0).normalized(), QVector3D(0, -1, 0).normalized() * m_rotationMatrix);

QVector3D a = v1.normalized() * QMatrix4x4(mat);
QVector3D b = v2.normalized();
qreal sina = QVector3D::crossProduct(a, b).length() / (a.length() * b.length());
qreal cosa = QVector3D::dotProduct(a, b) / (a.length() * b.length());*/

roll = angleFromDirection2(v1 * m_rotationMatrix.transposed(), v2);
pitch = -angleFromDirection(QVector3D(0, 1, 0) * m_rotationMatrix, QVector3D(0, 0, 0));
roll  =  angleFromDirection2(QVector3D(1, 0, 0) * m_rotationMatrix, QVector3D(0, 0, 0));



qq = QQuaternion::rotationTo(QVector3D(1, 0, 0), qq.vector());
qq.getEulerAngles(&pitch, &yaw, &roll);
float X = -cos(qDegreesToRadians(pitch))*cos(qDegreesToRadians(roll));
float Y = -sin(qDegreesToRadians(pitch))*cos(qDegreesToRadians(roll));
float Z = sin(qDegreesToRadians(pitch));

yaw  = qAtan2(2*qq.y()()*qq.scalar() - 2*qq.x()()*qq.z()(), 1 - 2*qq.y()()*qq.y()() - 2*qq.z()()*qq.z()());
pitch = qAtan2(2*qq.x()()*qq.scalar() - 2*qq.y()()*qq.z()(), 1 - 2*qq.x()()*qq.x()() - 2*qq.z()()*qq.z()());
roll   = qAsin(2*qq.x()()*qq.y()() + 2*qq.z()()*qq.scalar());



float *R = mat.data();
roll = qAtan2(R[1], R[4]);
pitch = qAsin(-R[7]);
yaw = qAtan2(-R[6], R[8]);


float epsilon=0.0001f;
if (mat(1,0) > (1.0f-epsilon)) { // singularity at north pole
    yaw = atan2(mat(0, 2),mat(2,2));
    pitch = M_PI / 2.0f;
} else if (mat(1,0) < -(1.0f-epsilon)) { // singularity at south pole
    yaw = atan2(mat(0,2),mat(2,2));
    pitch = M_PI / -2.0f;
} else {
    yaw = atan2(-mat(2,0),mat(0,0));
    pitch = asin(mat(1, 0));
    roll = atan2(-mat(1,2),mat(1,1));
}