/**************************************************************************
**
** This file is part of Qt Simulator
**
** Copyright (c) 2011 Nokia Corporation and/or its subsidiary(-ies).
**
** Contact: Nokia Corporation (info@qt.nokia.com)
**
**
** GNU Lesser General Public License Usage
**
** This file may be used under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation and
** appearing in the file LICENSE.LGPL included in the packaging of this file.
** Please review the following information to ensure the GNU Lesser General
** Public License version 2.1 requirements will be met:
** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights. These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** Other Usage
**
** Alternatively, this file may be used in accordance with the terms and
** conditions contained in a signed written agreement between you and Nokia.
**
** If you have questions regarding the use of this file, please contact
** Nokia at info@qt.nokia.com.
**
**************************************************************************/

#include "accelerometercontrol.h"

#include <QtCore/QCoreApplication>
#include <QtCore/QTimer>
#include <QtCore/qmath.h>
#include <QtCore/QDir>
#include <QtGui/QMatrix4x4>
#include <QtGui/QMouseEvent>
#include <QtGui/QVector3D>

static QVector3D accelerometerX(-1, 0, 0);
static QVector3D accelerometerY(0, 1, 0);
static QVector3D accelerometerZ(0, 0, -1);
static QVector3D unrotatedGravity(0, 1, 0);

static const double kEarthGravity = 9.8;
static const double kEarthMagneticFieldStrength = 30;
static const double kZero = 1e-4;

AccelerometerControl::AccelerometerControl(QWidget *parent)
    : QGLWidget(parent), mGravity(kEarthGravity), mMagneticFieldStrength(kEarthMagneticFieldStrength)
{
    QMatrix4x4 qtMatrix;
    qtMatrix.rotate(90, QVector3D(1, 0, 0)); // angle down to point at the horizon
    mNorthVector = qtMatrix.map(unrotatedGravity); // give us a vector that represents where North is
}

void AccelerometerControl::setValue(const QVector3D &newValue)
{
    mGravity = newValue.length();
    QVector3D target = newValue.normalized();

    if ((newValue - value()).lengthSquared() < 0.0001)
        return;

    qreal dot = QVector3D::dotProduct(accelerometerY, target);
    if (dot > 0.999) {
        mRotation = QQuaternion();
    } else if(dot < -0.999) {
        mRotation = QQuaternion::fromAxisAndAngle(0, 0, 1, 180);
    } else if (target.z() == -1) {
        // special case for face down (mirror of face up, not with the top pointing down)
        mRotation = QQuaternion(0.0, 0.0, -0.707107, -0.707107);
    } else {
        // we want the quaternion that transforms target into up
        QVector3D cross = QVector3D::crossProduct(target, accelerometerY);
        mRotation = QQuaternion::fromAxisAndAngle(cross, qAcos(dot) * 180 / M_PI);
    }

    updateAzimuth();
    updateGL();
}

void AccelerometerControl::initializeGL()
{
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_TEXTURE_2D);

    QImage frontImg(":ui/textures/N8_3D_front.png");
    QImage backImg(":ui/textures/N8_3D_back.png");
    QImage rightImg(":ui/textures/N8_3D_right.png");
    QImage topImg(":ui/textures/N8_3D_top.png");
    QImage bottomImg(":ui/textures/N8_3D_bottom.png");
    mFrontPortraitTexture = bindTexture(frontImg);
    mBackPortraitTexture = bindTexture(backImg);
    mLeftPortraitTexture = bindTexture(rightImg);
    mRightPortraitTexture = bindTexture(rightImg);
    mTopPortraitTexture = bindTexture(topImg);
    mBottomPortraitTexture = bindTexture(bottomImg);
    frontImg = QImage(":ui/textures/N900_front.png");
    backImg = QImage(":ui/textures/N900_back.png");
    rightImg = QImage(":ui/textures/N900_left.png");
    topImg = QImage(":ui/textures/N900_top.png");
    bottomImg = QImage(":ui/textures/N900_top.png");
    mFrontLandscapeTexture = bindTexture(frontImg);
    mBackLandscapeTexture = bindTexture(backImg);
    mLeftLandscapeTexture = bindTexture(rightImg);
    mRightLandscapeTexture = bindTexture(rightImg);
    mTopLandscapeTexture = bindTexture(topImg);
    mBottomLandscapeTexture = bindTexture(topImg);
    QImage northImg(":ui/icons/north.png");
    mNorthTexture = bindTexture(northImg);
}

/* since glu is not available everywhere we need to implement needed functions ourself */

static void lookAt(float eyeX, float eyeY, float eyeZ, float centerX, float centerY, float centerZ, float upX, float upY, float upZ)
{
    QVector3D F(centerX - eyeX, centerY - eyeY, centerZ - eyeZ);
    F.normalize();

    QVector3D UP(upX, upY, upZ);
    UP.normalize();

    QVector3D s = QVector3D::crossProduct(F, UP);
    QVector3D u = QVector3D::crossProduct(s, F);

    GLfloat M[16];
    M[0] = s.x();
    M[4] = s.y();
    M[8] = s.z();
    M[12] = 0;

    M[1] = u.x();
    M[5] = u.y();
    M[9] = u.z();
    M[13] = 0;

    M[2] = -F.x();
    M[6] = -F.y();
    M[10] = -F.z();
    M[14] = 0;

    M[3] = 0;
    M[7] = 0;
    M[11] = 0;
    M[15] = 1;

    glMultMatrixf(M);
    glTranslatef(-eyeX, -eyeY, -eyeZ);
}

static void perspective(float fovY, float aspectRatio, float front, float back)
{
    const double DEG2RAD = 3.14159265 / 180;

    double tangent = tan(fovY/2 * DEG2RAD);   // tangent of half fovY
    double height = front * tangent;          // half height of near plane
    double width = height * aspectRatio;      // half width of near plane

    // params: left, right, bottom, top, near, far
    glFrustum(-width, width, -height, height, front, back);
}

void AccelerometerControl::resizeGL(int w, int h)
{
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glViewport(0, 0, w, h);
    qreal aspect = (qreal)w / h;
    perspective(45, aspect, 0.1, 10);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glScalef (-1., 1., 1.); // left-handed system, x right, y up, z into
    lookAt(0.7, 1, -2,
           0, 0, 0,
           0, 1, 0);
}

void AccelerometerControl::drawMobile()
{
    // actually, half
    qreal height;
    qreal width;
    qreal depth;
    if (!mDefaultPortrait) {
        height = 0.4;
        width = height * 1.84; // values for the N900, based on the textures
        depth = width / 5.21;
    } else {
        height = 0.8;
        width = height / 1.92; // values for the N8, based on the textures
        depth = height / 7.16;
    }

    // backside
    glBindTexture(GL_TEXTURE_2D, mBackTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(1, 0);
    glVertex3f(-width, -height, depth);
    glTexCoord2f(0, 0);
    glVertex3f(width, -height, depth);
    glTexCoord2f(0, 1);
    glVertex3f(width, height, depth);
    glTexCoord2f(1, 1);
    glVertex3f(-width, height, depth);
    glEnd();

    // top
    glBindTexture(GL_TEXTURE_2D, mTopTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(1, 0);
    glVertex3f(-width, height, -depth);
    glTexCoord2f(1, 1);
    glVertex3f(-width, height, depth);
    glTexCoord2f(0, 1);
    glVertex3f(width, height, depth);
    glTexCoord2f(0, 0);
    glVertex3f(width, height, -depth);
    glEnd();

    // bottom
    glBindTexture(GL_TEXTURE_2D, mBottomTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(1, 1);
    glVertex3f(-width, -height, -depth);
    glTexCoord2f(0, 1);
    glVertex3f(width, -height, -depth);
    glTexCoord2f(0, 0);
    glVertex3f(width, -height, depth);
    glTexCoord2f(1, 0);
    glVertex3f(-width, -height, depth);
    glEnd();

    // left side
    glBindTexture(GL_TEXTURE_2D, mLeftTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(0, 0);
    glVertex3f(-width, -height, -depth);
    glTexCoord2f(1, 0);
    glVertex3f(-width, -height, depth);
    glTexCoord2f(1, 1);
    glVertex3f(-width, height, depth);
    glTexCoord2f(0, 1);
    glVertex3f(-width, height, -depth);
    glEnd();

    // right side
    glBindTexture(GL_TEXTURE_2D, mRightTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(0, 0);
    glVertex3f(width, -height, -depth);
    glTexCoord2f(0, 1);
    glVertex3f(width, height, -depth);
    glTexCoord2f(1, 1);
    glVertex3f(width, height, depth);
    glTexCoord2f(1, 0);
    glVertex3f(width, -height, depth);
    glEnd();

    // front side
    glBindTexture(GL_TEXTURE_2D, mFrontTexture);
    glBegin(GL_QUADS);
    glColor3f(1, 1, 1);
    glTexCoord2f(0, 0);
    glVertex3f(-width, -height, -depth);
    glTexCoord2f(0, 1);
    glVertex3f(-width, height, -depth);
    glTexCoord2f(1, 1);
    glVertex3f(width, height, -depth);
    glTexCoord2f(1, 0);
    glVertex3f(width, -height, -depth);
    glEnd();
}

void AccelerometerControl::drawGround()
{
    glBindTexture(GL_TEXTURE_2D, mNorthTexture);
    glBegin(GL_QUADS);
    glColor3d(0.3, 0.3, 0.3);
    qreal extends = 1.5;
    glTexCoord2f(0, 0);
    glVertex3f(-extends, -1, -extends);
    glTexCoord2f(0, 1);
    glVertex3f(-extends, -1, extends);
    glTexCoord2f(1, 1);
    glVertex3f(extends, -1, extends);
    glTexCoord2f(1, 0);
    glVertex3f(extends, -1, -extends);
    glEnd();
}

static void multQuaternion(const QQuaternion &q)
{
    // get the matrix representation
    QMatrix4x4 qtMatrix;
    qtMatrix.rotate(q);

    if (sizeof(qreal) == sizeof(GLfloat))
        glMultMatrixf(reinterpret_cast<const GLfloat *>(qtMatrix.constData()));
    else
        glMultMatrixd(reinterpret_cast<const GLdouble *>(qtMatrix.constData()));
}

void AccelerometerControl::paintGL()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    drawGround();

    glPushMatrix();
    QQuaternion rot = mRotation;
    rot.setX(-rot.x());
    multQuaternion(rot);
    drawMobile();
    glPopMatrix();
}

void AccelerometerControl::mouseMoveEvent(QMouseEvent *event)
{
    QPoint dist = event->pos() - mOldMousePosition;
    if (event->buttons() & Qt::LeftButton) {
        mRotation = QQuaternion::fromAxisAndAngle(0, -1, 0, dist.x()) * mRotation;
        mRotation = QQuaternion::fromAxisAndAngle(1, 0, 0, dist.y()) * mRotation;
    }
    if (event->buttons() & Qt::RightButton) {
        mRotation = QQuaternion::fromAxisAndAngle(0, 0, -1, dist.x()) * mRotation;
    }
    mOldMousePosition = event->pos();

    emit valueChanged(value());
    updateAzimuth();
    updateGL();
}

void AccelerometerControl::mousePressEvent(QMouseEvent *event)
{
    mOldMousePosition = event->pos();
}

void AccelerometerControl::setDeviceOrientation(bool portrait)
{
    mDefaultPortrait = portrait;
    if (portrait) {
        mFrontTexture = mFrontPortraitTexture;
        mBackTexture = mBackPortraitTexture;
        mLeftTexture = mLeftPortraitTexture;
        mRightTexture = mRightPortraitTexture;
        mTopTexture = mTopPortraitTexture;
        mBottomTexture = mBottomPortraitTexture;
    } else {
        mFrontTexture = mFrontLandscapeTexture;
        mBackTexture = mBackLandscapeTexture;
        mLeftTexture = mLeftLandscapeTexture;
        mRightTexture = mRightLandscapeTexture;
        mTopTexture = mTopLandscapeTexture;
        mBottomTexture = mBottomLandscapeTexture;
    }
    updateGL();
}

QVector3D AccelerometerControl::value() const
{
    QVector3D newValue;
    newValue.setX(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerX), unrotatedGravity));
    newValue.setY(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerY), unrotatedGravity));
    newValue.setZ(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerZ), unrotatedGravity));
    // remove rounding errors
    if (qAbs(newValue.x()) < kZero) newValue.setX(0);
    if (qAbs(newValue.y()) < kZero) newValue.setY(0);
    if (qAbs(newValue.z()) < kZero) newValue.setZ(0);
    newValue *= mGravity;
    return newValue;
}

void AccelerometerControl::updateAzimuth()
{
    emit magneticFieldChanged(magneticField());
    emit azimuthChanged(azimuth());
}

qreal AccelerometerControl::azimuth() const
{
    // Generate an azimuth value based on the position
    QVector3D grav = value();
    QVector3D mag = magneticField();
    QVector3D dev = mRotation.vector().normalized();
    qreal heading;
    if (grav.z() > 0) {
        // Face up
        heading = (1 - qAbs(dev.x())) * 180;
        // correct for 360 degrees of rotation
        if (mag.x() < 0) heading = 360 - heading;
        // rotate in the correct direction!
        heading = 360 - heading;
    } else {
        // Face down
        heading = qAbs(dev.x()) * 180;
        // correct for 360 degrees of rotation
        if (mag.x() < 0) heading = 360 - heading;
    }
    // May be too big
    while (heading > 360) heading -= 360;
    // report 360 as 0
    if (heading == 360) heading = 0;
    // report tiny numbers as 0
    if (heading < kZero) heading = 0;
    return heading;
}

QVector3D AccelerometerControl::magneticField() const
{
    QVector3D newValue;
    newValue.setX(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerX), mNorthVector));
    newValue.setY(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerY), mNorthVector));
    newValue.setZ(QVector3D::dotProduct(mRotation.conjugate().rotatedVector(accelerometerZ), mNorthVector));
    // remove rounding errors
    if (qAbs(newValue.x()) < kZero) newValue.setX(0);
    if (qAbs(newValue.y()) < kZero) newValue.setY(0);
    if (qAbs(newValue.z()) < kZero) newValue.setZ(0);
    newValue *= mMagneticFieldStrength;
    return newValue;
}