//
//  ObjectActionOffset.cpp
//  libraries/physics/src
//
//  Created by Seth Alves 2015-6-17
//  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 "ObjectActionOffset.h"

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

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

void ObjectActionOffset::updateActionWorker(btScalar deltaTimeStep) {
    if (!tryLockForRead()) {
        // don't risk hanging the thread running the physics simulation
        return;
    }

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

    if (_positionalTargetSet) {
        glm::vec3 offset = _pointToOffsetFrom - bulletToGLM(rigidBody->getCenterOfMassPosition());
        float offsetLength = glm::length(offset);
        float offsetError = _linearDistance - offsetLength;

        // if (glm::abs(offsetError) > IGNORE_POSITION_DELTA) {
        if (glm::abs(offsetError) > 0.0f) {
            float offsetErrorAbs = glm::abs(offsetError);
            float offsetErrorDirection = - offsetError / offsetErrorAbs;
            glm::vec3 previousVelocity = bulletToGLM(rigidBody->getLinearVelocity());

            glm::vec3 velocityAdjustment = glm::normalize(offset) * offsetErrorDirection * offsetErrorAbs / _linearTimeScale;
            rigidBody->setLinearVelocity(glmToBullet(previousVelocity + velocityAdjustment));
            // rigidBody->setLinearVelocity(glmToBullet(velocityAdjustment));
            rigidBody->activate();
        }
    }

    unlock();
}


bool ObjectActionOffset::updateArguments(QVariantMap arguments) {
    bool pOk0 = true;
    glm::vec3 pointToOffsetFrom =
        EntityActionInterface::extractVec3Argument("offset action", arguments, "pointToOffsetFrom", pOk0, true);

    bool pOk1 = true;
    float linearTimeScale =
        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearTimeScale", pOk1, false);

    bool pOk2 = true;
    float linearDistance =
        EntityActionInterface::extractFloatArgument("offset action", arguments, "linearDistance", pOk2, false);

    if (!pOk0) {
        return false;
    }
    if (pOk1 && linearTimeScale <= 0.0f) {
        qDebug() << "offset action -- linearTimeScale must be greater than zero.";
        return false;
    }

    lockForWrite();

    _pointToOffsetFrom = pointToOffsetFrom;
    _positionalTargetSet = true;

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

    if (pOk2) {
        _linearDistance = linearDistance;
    } else {
        _linearDistance = 1.0f;
    }

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