//
//  ObjectActionSpring.cpp
//  libraries/physics/src
//
//  Created by Seth Alves 2015-6-5
//  Copyright 2015 High Fidelity, Inc.
//
//  Distributed under the Apache License, Version 2.0.
//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
//

#include "ObjectActionSpring.h"

ObjectActionSpring::ObjectActionSpring(EntityActionType type, QUuid id, EntityItemPointer ownerEntity) :
    ObjectAction(type, id, ownerEntity) {
    #if WANT_DEBUG
    qDebug() << "ObjectActionSpring::ObjectActionSpring";
    #endif
}

ObjectActionSpring::~ObjectActionSpring() {
    #if WANT_DEBUG
    qDebug() << "ObjectActionSpring::~ObjectActionSpring";
    #endif
}

void ObjectActionSpring::updateActionWorker(btScalar deltaTimeStep) {
    if (!tryLockForRead()) {
        // don't risk hanging the thread running the physics simulation
        qDebug() << "ObjectActionSpring::updateActionWorker lock failed";
        return;
    }

    void* physicsInfo = _ownerEntity->getPhysicsInfo();
    if (!physicsInfo) {
        unlock();
        return;
    }
    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
    btRigidBody* rigidBody = motionState->getRigidBody();
    if (!rigidBody) {
        unlock();
        qDebug() << "ObjectActionSpring::updateActionWorker no rigidBody";
        return;
    }

    // handle the linear part
    if (_positionalTargetSet) {
        glm::vec3 offset = _positionalTarget - bulletToGLM(rigidBody->getCenterOfMassPosition());
        float offsetLength = glm::length(offset);
        float speed = offsetLength / _linearTimeScale;

        if (offsetLength > IGNORE_POSITION_DELTA) {
            glm::vec3 newVelocity = glm::normalize(offset) * speed;
            rigidBody->setLinearVelocity(glmToBullet(newVelocity));
            rigidBody->activate();
        } else {
            rigidBody->setLinearVelocity(glmToBullet(glm::vec3(0.0f)));
        }
    }

    // handle rotation
    if (_rotationalTargetSet) {
        glm::quat bodyRotation = bulletToGLM(rigidBody->getOrientation());
        // if qZero and qOne are too close to each other, we can get NaN for angle.
        auto alignmentDot = glm::dot(bodyRotation, _rotationalTarget);
        const float almostOne = 0.99999f;
        if (glm::abs(alignmentDot) < almostOne) {
            glm::quat target = _rotationalTarget;
            if (alignmentDot < 0) {
                target = -target;
            }
            glm::quat qZeroInverse = glm::inverse(bodyRotation);
            glm::quat deltaQ = target * qZeroInverse;
            glm::vec3 axis = glm::axis(deltaQ);
            float angle = glm::angle(deltaQ);
            assert(!isNaN(angle));
            glm::vec3 newAngularVelocity = (angle / _angularTimeScale) * glm::normalize(axis);
            rigidBody->setAngularVelocity(glmToBullet(newAngularVelocity));
            rigidBody->activate();
        } else {
            rigidBody->setAngularVelocity(glmToBullet(glm::vec3(0.0f)));
        }
    }

    unlock();
}


bool ObjectActionSpring::updateArguments(QVariantMap arguments) {
    // targets are required, spring-constants are optional
    bool ptOk = true;
    glm::vec3 positionalTarget =
        EntityActionInterface::extractVec3Argument("spring action", arguments, "targetPosition", ptOk, false);
    bool pscOk = true;
    float linearTimeScale =
        EntityActionInterface::extractFloatArgument("spring action", arguments, "linearTimeScale", pscOk, false);
    if (ptOk && pscOk && linearTimeScale <= 0.0f) {
        qDebug() << "spring action -- linearTimeScale must be greater than zero.";
        return false;
    }

    bool rtOk = true;
    glm::quat rotationalTarget =
        EntityActionInterface::extractQuatArgument("spring action", arguments, "targetRotation", rtOk, false);
    bool rscOk = true;
    float angularTimeScale =
        EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", rscOk, false);

    if (!ptOk && !rtOk) {
        qDebug() << "spring action requires at least one of targetPosition or targetRotation argument";
        return false;
    }

    lockForWrite();

    _positionalTargetSet = _rotationalTargetSet = false;

    if (ptOk) {
        _positionalTarget = positionalTarget;
        _positionalTargetSet = true;

        if (pscOk) {
            _linearTimeScale = linearTimeScale;
        } else {
            _linearTimeScale = 0.1f;
        }
    }

    if (rtOk) {
        _rotationalTarget = rotationalTarget;
        _rotationalTargetSet = true;

        if (rscOk) {
            _angularTimeScale = angularTimeScale;
        } else {
            _angularTimeScale = 0.1f;
        }
    }

    _active = true;
    unlock();
    return true;
}

QByteArray ObjectActionSpring::serialize() {
    QByteArray ba;
    QDataStream dataStream(&ba, QIODevice::WriteOnly);

    dataStream << getType();
    dataStream << getID();

    dataStream << _positionalTarget;
    dataStream << _linearTimeScale;
    dataStream << _positionalTargetSet;

    dataStream << _rotationalTarget;
    dataStream << _angularTimeScale;
    dataStream << _rotationalTargetSet;

    return ba;
}

void ObjectActionSpring::deserialize(QByteArray serializedArguments) {
    QDataStream dataStream(serializedArguments);

    EntityActionType type;
    QUuid id;

    dataStream >> type;
    dataStream >> id;
    assert(type == getType());
    assert(id == getID());

    dataStream >> _positionalTarget;
    dataStream >> _linearTimeScale;
    dataStream >> _positionalTargetSet;

    dataStream >> _rotationalTarget;
    dataStream >> _angularTimeScale;
    dataStream >> _rotationalTargetSet;

    _active = true;
}