AHRS::AHRS()
{
	foundZero = false;
	zeroSamples = 0;
	zx = zy = zz = 0.0f;

	quat = QQuaternion(1.0f, 0.0f, 0.0f, 0.0f);
	err[0] = err[1] = err[2] = 0.0f;
}

void GLWidget::mouseReleaseEvent(QMouseEvent *e){
    if ( m_mouseClick){
        float x = 2.0 * (float(e->x()) / w) - 1.0;
        float y = 1.0 - 2.0 * (float(e->y()) / h);
        QPointF p(x, y);
        m_trackBalls.release( p, QQuaternion());
        m_mouseClick = 0;
    }
}

void OpenGLWidget::mouseDoubleClickEvent(QMouseEvent *)
{
    m_angularSpeed = 0;
    m_rotation = QQuaternion();

    resetCamera();

    updateGL();
}

QQuaternion M4toQuat(QMatrix4x4 mat) {
	double trace = mat(0,0) + mat(1,1) + mat(2,2) + 1;
	double s = 0.5 / sqrt(trace);
	double w = 0.25 / s;
	double x = (mat(2, 1) - mat(1, 2)) * s;
	double y = (mat(0, 2) - mat(2, 0)) * s;
	double z = (mat(1, 0) - mat(0, 1)) * s;
	return QQuaternion(w, x, y, z);
}

QVRRenderContext::QVRRenderContext() :
    _processIndex(-1),
    _windowIndex(-1),
    _windowGeometry(),
    _screenGeometry(),
    _navigationPosition(0.0f, 0.0f, 0.0f),
    _navigationOrientation(0.0f, 0.0f, 0.0f, 0.0f),
    _screenWall { QVector3D(), QVector3D(), QVector3D() },
    _outputMode(QVR_Output_Center),
    _viewPasses(0),
    _eye { QVR_Eye_Center, QVR_Eye_Center },
    _trackingPosition { QVector3D(0.0f, 0.0f, 0.0f), QVector3D(0.0f, 0.0f, 0.0f) },
    _trackingOrientation { QQuaternion(0.0f, 0.0f, 0.0f, 0.0f), QQuaternion(0.0f, 0.0f, 0.0f, 0.0f) },
    _frustum { { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f } },
    _viewMatrix { QMatrix4x4(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f),
                  QMatrix4x4(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f) }
{
}

void MainWidget::mouseReleaseEvent(QMouseEvent *event)
{

    if(event->button() == Qt::LeftButton)
    {
        this->trackball.release(pixelPosToViewPos(event->pos()),QQuaternion());
        event->accept();
    }

}

TrackBall::TrackBall(TrackMode mode)
    : m_angularVelocity(0)
    , m_paused(false)
    , m_pressed(false)
    , m_mode(mode)
{
    m_axis = QVector3D(0, 1, 0);
    m_rotation = QQuaternion();
    m_lastTime = QTime::currentTime();
}

TrackBall::TrackBall(float angularVelocity, const QVector3D& axis, TrackMode mode)
    : m_axis(axis)
    , m_angularVelocity(angularVelocity)
    , m_paused(false)
    , m_pressed(false)
    , m_mode(mode)
{
    m_rotation = QQuaternion();
    m_lastTime = QTime::currentTime();
}

Camera::Camera()
{
	quaternion = QQuaternion(1.0, 0.0, 0.0, 0.0);	
	target = QVector3D(0.0, 0.0, 0.0);
	xAxis = QVector3D(1.0, 0.0, 0.0);
	yAxis = QVector3D(0.0, 1.0, 0.0);
	zAxis = QVector3D(0.0, 0.0, 1.0);
	zoomOffset = 0.0;
	zoomSpeed = 0.005;
	rotationSpeed = 0.3;	
	viewMatrix.loadIdentity();		
}

/*!
    Creates a normalized quaternion that corresponds to rotating through
    \a angle degrees about the specified 3D \a axis.
*/
QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, qreal angle)
{
    // Algorithm from:
    // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
    // We normalize the result just in case the values are close
    // to zero, as suggested in the above FAQ.
    qreal a = (angle / 2.0f) * M_PI / 180.0f;
    qreal s = qSin(a);
    qreal c = qCos(a);
    QVector3D ax = axis.normalized();
    return QQuaternion(c, ax.x() * s, ax.y() * s, ax.z() * s).normalized();
}

// Test getting and setting quaternion properties via the metaobject system.
void tst_QQuaternion::properties()
{
    tst_QQuaternionProperties obj;

    obj.setQuaternion(QQuaternion(6.0f, 7.0f, 8.0f, 9.0f));

    QQuaternion q = qVariantValue<QQuaternion>(obj.property("quaternion"));
    QCOMPARE(q.scalar(), (qreal)6.0f);
    QCOMPARE(q.x(), (qreal)7.0f);
    QCOMPARE(q.y(), (qreal)8.0f);
    QCOMPARE(q.z(), (qreal)9.0f);

    obj.setProperty("quaternion",
                    qVariantFromValue(QQuaternion(-6.0f, -7.0f, -8.0f, -9.0f)));

    q = qVariantValue<QQuaternion>(obj.property("quaternion"));
    QCOMPARE(q.scalar(), (qreal)-6.0f);
    QCOMPARE(q.x(), (qreal)-7.0f);
    QCOMPARE(q.y(), (qreal)-8.0f);
    QCOMPARE(q.z(), (qreal)-9.0f);
}

QQuaternion GLC_Matrix4x4::quaternion() const
{
	QQuaternion subject;
	GLC_Matrix4x4 rotMat= rotationMatrix();
	if ((this->type() != GLC_Matrix4x4::Identity) && (rotMat != GLC_Matrix4x4()))
	{
		const double matrixTrace= rotMat.trace();
		double s, w, x, y, z;

		if (matrixTrace > 0.0)
		{
			s= 0.5 / sqrt(matrixTrace);
			w= 0.25 / s;
			x= (rotMat.m_Matrix[9] - rotMat.m_Matrix[6]) * s;
			y= (rotMat.m_Matrix[2] - rotMat.m_Matrix[8]) * s;
			z= (rotMat.m_Matrix[4] - rotMat.m_Matrix[1]) * s;
		}
		else
		{
			if ((abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[5])) &&  (abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[15])))
			{	// column 0 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[0] - rotMat.m_Matrix[5] - rotMat.m_Matrix[10]) * 2.0;

		        w= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9] ) / s;
		        x= 0.5 / s;
		        y= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4] ) / s;
		        z= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8] ) / s;
			}
			else if ((abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[0])) &&  (abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[15])))
			{	// column 1 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[5] - rotMat.m_Matrix[0] - rotMat.m_Matrix[10]) * 2.0;

		        w= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
		        x= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
		        y= 0.5 / s;
		        z= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
			}
			else
			{	// column 3 greater
		        s= sqrt(1.0 + rotMat.m_Matrix[10] - rotMat.m_Matrix[0] - rotMat.m_Matrix[5]) * 2.0;

		        w = (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
		        x = (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
		        y = (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
		        z = 0.5 / s;
			}
		}
		subject= QQuaternion(w, x, y, z);
	}

	return subject;
}

void OpenGLWindow::initialize()
{
    m_program = new QOpenGLShaderProgram(this);
    // see declaration of sources on top
    m_program->addShaderFromSourceCode(QOpenGLShader::Vertex, vertexShaderSource);
    m_program->addShaderFromSourceCode(QOpenGLShader::Fragment, fragmentShaderSource);
    m_program->link();
    m_posAttr = m_program->attributeLocation("posAttr");
    m_colAttr = m_program->attributeLocation("colAttr");
    m_matrixUniform = m_program->uniformLocation("matrix");
    qrotation = QQuaternion(1, 0, 0, 0);
    rotationMode = 0; // 0 = simple rotation, 1 = trackball
}

void EditCameraDialog::setCamRot(Dim axis){
    switch (axis) {
    case X:
        parent->setCameraRot(QQuaternion::fromAxisAndAngle(0, 1, 0, 90));
        break;
    case Y:
        parent->setCameraRot(QQuaternion::fromAxisAndAngle(1, 0, 0, 90));
        break;
    case Z:
        parent->setCameraRot(QQuaternion());
        break;
    }
}

void  OpenGLWindow::mouseMoveEvent(QMouseEvent* event) {
    /*Change Model matrix according to mouse movement when left mouse button is pressed*/
    if (event->buttons() & Qt::LeftButton) {
        // check if position values are valid
        if (oldMousePosition.x() != -1 && oldMousePosition.y() != -1) {
            // quaterion q is used to store additional rotation
            QQuaternion q;
            if (rotationMode == 0) // simple rotation
            {
                //difference of new and old mouse position
                float xDiff = oldMousePosition.x() - event->x();
                float yDiff = oldMousePosition.y() - event->y();

                q = q.fromEulerAngles(QVector3D(yDiff, -xDiff, 0));
                //q2 = q2.fromEulerAngles(QVector3D(0, 0, -yDiff));
            }
            else if (rotationMode == 1) //trackball
            {
                // define center of window
                float centerX = float(this->width() / 2);
                float centerY = float(this->height() / 2);

                //difference of new and old mouse position
                float oldX = (oldMousePosition.x() - centerX) / centerX;
                float oldY = (oldMousePosition.y() - centerY) / centerY;
                float newX = (event->x() - centerX) / centerX;
                float newY = (event->y() - centerY) / centerY;

                //model.rotate(2, -xDiff, 0, -yDiff);
                float rotation[4];
                // get rotation as quaterion from trackball function
                gfs_gl_trackball(rotation, newX, newY, oldX, oldY);
                q = QQuaternion(QVector4D(rotation[0], rotation[1], rotation[2], rotation[3]));
            }
            // combine new rotation and current rotation
            qrotation = q  * qrotation;
            QMatrix4x4 m;
            m.rotate(qrotation);
            model.setToIdentity();
            // translate to center for rotation
            model.translate(center);
            model = model * m;
            //move back to former position
            model.translate(-center);
        }
        // set current mouse position as old
        oldMousePosition.setX(event->x());
        oldMousePosition.setY(event->y());
    }
}

void GLWidget::mouseMoveEvent(QMouseEvent *event)
{
	if ((event->buttons() & Qt::RightButton) && dragging) {
		double xtrans = (event->pos().x() - lastPoint.x()) / 1000.0;
		double ytrans = -(event->pos().y() - lastPoint.y()) / 1000.0; //Qt y-coord is inverted
		QVector4D trans(xtrans, ytrans, 0, 1);
		QVector4D worldTrans = trans * camera->getCameraMatrix();
		if(cameraActive) {
            this->camera->setpointOfInterest(scene->getMainBoat()->getPosition());
			this->camera->translate(-worldTrans.x(), -worldTrans.y(), -worldTrans.z());

			updateGL();
		} else {
			emit translate(worldTrans.x(), worldTrans.y(), worldTrans.z());
		}
	}

//	if ((event->buttons() & Qt::LeftButton) && dragging) {
		//Here we implement the trackball. Sample two points on the sphere and
		//calculate their angle to use as the rotation.

		//normalize to intervals [-1,1]
		double lastx = clampUnit(lastPoint.x() / (this->size().width() / 2.0) - 1.0);
		double lasty = clampUnit(-(lastPoint.y() / (this->size().height() / 2.0) - 1.0));

		double newx = clampUnit(event->pos().x() / (this->size().width() / 2.0) - 1.0);
		double newy = clampUnit(-(event->pos().y() / (this->size().height() / 2.0) - 1.0));

		//Project the two points into the sphere (or the hyperbolic plane)
		QVector3D v1(lastx, lasty, z(lastx, lasty));
		v1.normalize();
		QVector3D v2(newx, newy, z(newx, newy));
		v2.normalize();

		//Determine the normal of the generated plane through the center of the sphere
		QVector3D normal = QVector3D::crossProduct(v1, v2);
		double theta = acos(QVector3D::dotProduct(v1, v2)) / 3.0;

		//angle/2.0, because the quats double cover SO(3)
		QQuaternion newRot = QQuaternion(cos(theta/2.0), sin(theta/2.0) * normal.normalized());
		QQuaternion cameraQuat = M4toQuat(camera->getCameraMatrix());
		QQuaternion worldQuat = cameraQuat.conjugate() * newRot * cameraQuat;
		if(cameraActive) {
			this->camera->rotate(newRot);
			updateGL();
		} else {
			emit rotate(&worldQuat);
		}
//	}
}

/*!
    Creates a normalized quaternion that corresponds to rotating through
    \a angle degrees about the 3D axis (\a x, \a y, \a z).
*/
QQuaternion QQuaternion::fromAxisAndAngle
        (qreal x, qreal y, qreal z, qreal angle)
{
    qreal length = qSqrt(x * x + y * y + z * z);
    if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) {
        x /= length;
        y /= length;
        z /= length;
    }
    qreal a = (angle / 2.0f) * M_PI / 180.0f;
    qreal s = qSin(a);
    qreal c = qCos(a);
    return QQuaternion(c, x * s, y * s, z * s).normalized();
}

/*!
    Creates a normalized quaternion that corresponds to rotating through
    \a angle degrees about the 3D axis (\a x, \a y, \a z).
*/
QQuaternion QQuaternion::fromAxisAndAngle
        (float x, float y, float z, float angle)
{
    float length = qSqrt(x * x + y * y + z * z);
    if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) {
        x /= length;
        y /= length;
        z /= length;
    }
    float a = (angle / 2.0f) * M_PI / 180.0f;
    float s = sinf(a);
    float c = cosf(a);
    return QQuaternion(c, x * s, y * s, z * s).normalized();
}

/*!
    Returns the normalized unit form of this quaternion.

    If this quaternion is null, then a null quaternion is returned.
    If the length of the quaternion is very close to 1, then the quaternion
    will be returned as-is.  Otherwise the normalized form of the
    quaternion of length 1 will be returned.

    \sa length(), normalize()
*/
QQuaternion QQuaternion::normalized() const
{
    // Need some extra precision if the length is very small.
    double len = double(xp) * double(xp) +
                 double(yp) * double(yp) +
                 double(zp) * double(zp) +
                 double(wp) * double(wp);
    if (qFuzzyIsNull(len - 1.0f))
        return *this;
    else if (!qFuzzyIsNull(len))
        return *this / qSqrt(len);
    else
        return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f);
}

void Loader::loadAnimation(const aiNode *pNode,int index,CMC_AnimateData * animate)
{
    std::string NodeName(pNode->mName.data);
    if(!m_pScene->HasAnimations ())
    {
        m_model->m_hasAnimation = false;
        return ;
    }
    const aiAnimation* pAnimation = m_pScene->mAnimations[index];
    const aiNodeAnim * pNodeAnim = findNodeAnim(pAnimation, NodeName);
    if(pNodeAnim)//that node is a animation bone.
    {
        auto animateBone = new CMC_AnimateBone();
        animateBone->m_boneName = NodeName;
        //load position key.
        for(int i =0;i<pNodeAnim->mNumPositionKeys;i++)
        {
            auto v = pNodeAnim->mPositionKeys[i];
            CMC_TranslateKey key;
            key.time = v.mTime;
            key.trans = QVector3D (v.mValue.x,v.mValue.y,v.mValue.z);
            animateBone->addTranslate (key);
        }
        //load scale key
        for(int i =0;i<pNodeAnim->mNumScalingKeys;i++)
        {
            auto v = pNodeAnim->mScalingKeys[i];
            CMC_ScaleKey key;
            key.time = v.mTime;
            key.scale = QVector3D (v.mValue.x,v.mValue.y,v.mValue.z);
            animateBone->addScale (key);
        }
        //load rotation key
        for(int i =0;i<pNodeAnim->mNumPositionKeys;i++)
        {
            auto v = pNodeAnim->mRotationKeys[i];
            CMC_RotateKey key;
            key.time = v.mTime;
            key.rotate = QQuaternion(v.mValue.w,v.mValue.x,v.mValue.y,v.mValue.z);
            animateBone->addRotate (key);
        }

        animate->addAnimateBone (animateBone);
        animate->m_ticksPerSecond = pAnimation->mTicksPerSecond;
        animate->m_duration = pAnimation->mDuration;
    }
    for (uint i = 0 ; i < pNode->mNumChildren ; i++) {
        loadAnimation( pNode->mChildren[i],index,animate);
    }
}