//
//  ObjectConstraintHinge.cpp
//  libraries/physics/src
//
//  Created by Seth Alves 2017-4-11
//  Copyright 2017 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 "QVariantGLM.h"

#include "EntityTree.h"
#include "ObjectConstraintHinge.h"
#include "PhysicsLogging.h"


const uint16_t ObjectConstraintHinge::constraintVersion = 1;


ObjectConstraintHinge::ObjectConstraintHinge(const QUuid& id, EntityItemPointer ownerEntity) :
    ObjectConstraint(DYNAMIC_TYPE_HINGE, id, ownerEntity),
    _pivotInA(glm::vec3(0.0f)),
    _axisInA(glm::vec3(0.0f))
{
    #if WANT_DEBUG
    qCDebug(physics) << "ObjectConstraintHinge::ObjectConstraintHinge";
    #endif
}

ObjectConstraintHinge::~ObjectConstraintHinge() {
    #if WANT_DEBUG
    qCDebug(physics) << "ObjectConstraintHinge::~ObjectConstraintHinge";
    #endif
}

QList<btRigidBody*> ObjectConstraintHinge::getRigidBodies() {
    QList<btRigidBody*> result;
    result += getRigidBody();
    QUuid otherEntityID;
    withReadLock([&]{
        otherEntityID = _otherEntityID;
    });
    if (!otherEntityID.isNull()) {
        result += getOtherRigidBody(otherEntityID);
    }
    return result;
}

void ObjectConstraintHinge::prepareForPhysicsSimulation() {
    // setting the motor velocity doesn't appear to work for anyone.  constantly adjusting the
    // target angle seems to work.
    // https://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=7020

    if (!isMine()) {
        // XXX
        // don't activate motor for someone else's action?
        // maybe don't if this interface isn't the sim owner?
        return;
    }
    uint64_t now = usecTimestampNow();
    withWriteLock([&]{
        btHingeConstraint* constraint = static_cast<btHingeConstraint*>(_constraint);
        if (!constraint) {
            return;
        }
        if (_previousMotorTime == 0) {
            _previousMotorTime = now;
            return;
        }
        if (_motorVelocity != 0.0f) {
            if (_startMotorTime == 0) {
                _startMotorTime = now;
            }
            float dt = (float)(now - _previousMotorTime) / (float)USECS_PER_SECOND;
            float t = (float)(now - _startMotorTime) / (float)USECS_PER_SECOND;
            float motorTarget = _motorVelocity * t;

            // // bring motorTarget into the range of [-PI, PI]
            // motorTarget += PI;
            // motorTarget = fmodf(motorTarget, 2.0f * PI);
            // motorTarget -= PI;

            if (!_motorEnabled) {
                constraint->enableMotor(true);
                _motorEnabled = true;
            }
            constraint->setMaxMotorImpulse(_maxImpulse);
            constraint->setMotorTarget(motorTarget, dt);

        } else if (_motorTargetTimeScale > 0.0f) {
            // XXX
        } else if (_motorEnabled) {
            constraint->enableMotor(false);
            _motorEnabled = false;
            _startMotorTime = 0;
        }
        _previousMotorTime = now;
    });
}

void ObjectConstraintHinge::updateHinge() {
    btHingeConstraint* constraint { nullptr };
    float low;
    float high;
    float softness;
    float biasFactor;
    float relaxationFactor;
    float motorVelocity;
    float motorTarget;
    float motorTargetTimeScale;
    float maxImpulse;

    withReadLock([&]{
        constraint = static_cast<btHingeConstraint*>(_constraint);
        low = _low;
        high = _high;
        maxImpulse = _maxImpulse;
        biasFactor = _biasFactor;
        relaxationFactor = _relaxationFactor;

        // under the hood, motorTarget sets a veloctiy and must be called repeatedly to maintain
        // a velocity.  _motorTargetTimeScale of 0.0f means the target isn't set.  motorVelocity is
        // only considered when motorTarget isn't set.
        motorVelocity = _motorVelocity;
        motorTarget = _motorTarget;
        motorTargetTimeScale = _motorTargetTimeScale;
        softness = _softness;
    });

    if (!constraint) {
        return;
    }
}


btTypedConstraint* ObjectConstraintHinge::getConstraint() {
    btHingeConstraint* constraint { nullptr };
    QUuid otherEntityID;
    glm::vec3 pivotInA;
    glm::vec3 axisInA;
    glm::vec3 pivotInB;
    glm::vec3 axisInB;

    withReadLock([&]{
        constraint = static_cast<btHingeConstraint*>(_constraint);
        pivotInA = _pivotInA;
        axisInA = _axisInA;
        otherEntityID = _otherEntityID;
        pivotInB = _pivotInB;
        axisInB = _axisInB;
    });
    if (constraint) {
        return constraint;
    }

    btRigidBody* rigidBodyA = getRigidBody();
    if (!rigidBodyA) {
        qCDebug(physics) << "ObjectConstraintHinge::getConstraint -- no rigidBodyA";
        return nullptr;
    }

    if (!otherEntityID.isNull()) {
        // This hinge is between two entities... find the other rigid body.
        btRigidBody* rigidBodyB = getOtherRigidBody(otherEntityID);
        if (!rigidBodyB) {
            return nullptr;
        }
        constraint = new btHingeConstraint(*rigidBodyA, *rigidBodyB,
                                           glmToBullet(pivotInA), glmToBullet(pivotInB),
                                           glmToBullet(axisInA), glmToBullet(axisInB),
                                           true); // useReferenceFrameA

    } else {
        // This hinge is between an entity and the world-frame.
        constraint = new btHingeConstraint(*rigidBodyA,
                                           glmToBullet(pivotInA), glmToBullet(axisInA),
                                           true); // useReferenceFrameA
    }

    withWriteLock([&]{
        _constraint = constraint;
    });

    // if we don't wake up rigidBodyA, we may not send the dynamicData property over the network
    forceBodyNonStatic();
    activateBody();

    updateHinge();

    return constraint;
}


bool ObjectConstraintHinge::updateArguments(QVariantMap arguments) {
    glm::vec3 pivotInA;
    glm::vec3 axisInA;
    QUuid otherEntityID;
    glm::vec3 pivotInB;
    glm::vec3 axisInB;
    float low;
    float high;
    float softness;
    float biasFactor;
    float relaxationFactor;
    float motorVelocity;
    float motorTarget;
    float motorTargetTimeScale;
    float maxImpulse;

    bool needUpdate = false;
    bool somethingChanged = ObjectDynamic::updateArguments(arguments);
    withReadLock([&]{
        bool ok = true;
        pivotInA = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "pivot", ok, false);
        if (!ok) {
            pivotInA = _pivotInA;
        }

        ok = true;
        axisInA = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "axis", ok, false);
        if (!ok) {
            axisInA = _axisInA;
        }

        ok = true;
        otherEntityID = QUuid(EntityDynamicInterface::extractStringArgument("hinge constraint",
                                                                            arguments, "otherEntityID", ok, false));
        if (!ok) {
            otherEntityID = _otherEntityID;
        }

        ok = true;
        pivotInB = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "otherPivot", ok, false);
        if (!ok) {
            pivotInB = _pivotInB;
        }

        ok = true;
        axisInB = EntityDynamicInterface::extractVec3Argument("hinge constraint", arguments, "otherAxis", ok, false);
        if (!ok) {
            axisInB = _axisInB;
        }

        ok = true;
        low = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "low", ok, false);
        if (!ok) {
            low = _low;
        }

        ok = true;
        high = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "high", ok, false);
        if (!ok) {
            high = _high;
        }

        ok = true;
        softness = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "softness", ok, false);
        if (!ok) {
            softness = _softness;
        }

        ok = true;
        biasFactor = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "biasFactor", ok, false);
        if (!ok) {
            biasFactor = _biasFactor;
        }

        ok = true;
        relaxationFactor = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments,
                                                                        "relaxationFactor", ok, false);
        if (!ok) {
            relaxationFactor = _relaxationFactor;
        }

        ok = true;
        motorVelocity = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments,
                                                                     "motorVelocity", ok, false);
        if (!ok) {
            motorVelocity = _motorVelocity;
        }

        ok = true;
        motorTarget = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments,
                                                                     "motorTarget", ok, false);
        if (!ok) {
            motorTarget = _motorTarget;
        }

        ok = true;
        motorTargetTimeScale = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments,
                                                                   "motorTargetTimeScale", ok, false);
        if (!ok) {
            motorTargetTimeScale = _motorTargetTimeScale;
        }

        ok = true;
        maxImpulse = EntityDynamicInterface::extractFloatArgument("hinge constraint", arguments, "maxImpulse", ok, false);
        if (!ok) {
            maxImpulse = _maxImpulse;
        }


        if (somethingChanged ||
            pivotInA != _pivotInA ||
            axisInA != _axisInA ||
            otherEntityID != _otherEntityID ||
            pivotInB != _pivotInB ||
            axisInB != _axisInB ||
            low != _low ||
            high != _high ||
            softness != _softness ||
            biasFactor != _biasFactor ||
            relaxationFactor != _relaxationFactor ||
            motorVelocity != _motorVelocity ||
            motorTarget != _motorTarget ||
            motorTargetTimeScale != _motorTargetTimeScale ||
            maxImpulse != _maxImpulse) {
            // something changed
            needUpdate = true;
        }
    });

    if (needUpdate) {
        withWriteLock([&] {
            _pivotInA = pivotInA;
            _axisInA = axisInA;
            _otherEntityID = otherEntityID;
            _pivotInB = pivotInB;
            _axisInB = axisInB;
            _low = low;
            _high = high;
            _softness = softness;
            _biasFactor = biasFactor;
            _relaxationFactor = relaxationFactor;
            _motorVelocity = motorVelocity;
            _motorTarget = motorTarget;
            _motorTargetTimeScale = motorTargetTimeScale;
            _maxImpulse = maxImpulse;

            _active = true;

            auto ownerEntity = _ownerEntity.lock();
            if (ownerEntity) {
                ownerEntity->setDynamicDataDirty(true);
                ownerEntity->setDynamicDataNeedsTransmit(true);
            }
        });

        updateHinge();
    }

    return true;
}

QVariantMap ObjectConstraintHinge::getArguments() {
    QVariantMap arguments = ObjectDynamic::getArguments();
    withReadLock([&] {
        if (_constraint) {
            arguments["pivot"] = glmToQMap(_pivotInA);
            arguments["axis"] = glmToQMap(_axisInA);
            arguments["otherEntityID"] = _otherEntityID;
            arguments["otherPivot"] = glmToQMap(_pivotInB);
            arguments["otherAxis"] = glmToQMap(_axisInB);
            arguments["low"] = _low;
            arguments["high"] = _high;
            arguments["softness"] = _softness;
            arguments["biasFactor"] = _biasFactor;
            arguments["relaxationFactor"] = _relaxationFactor;
            arguments["motorVelocity"] = _motorVelocity;
            arguments["motorTarget"] = _motorTarget;
            arguments["motorTargetTimeScale"] = _motorTargetTimeScale;
            arguments["maxImpulse"] = _maxImpulse;
            arguments["angle"] = static_cast<btHingeConstraint*>(_constraint)->getHingeAngle(); // [-PI,PI]
        }
    });
    return arguments;
}

QByteArray ObjectConstraintHinge::serialize() const {
    QByteArray serializedConstraintArguments;
    QDataStream dataStream(&serializedConstraintArguments, QIODevice::WriteOnly);

    dataStream << DYNAMIC_TYPE_HINGE;
    dataStream << getID();
    dataStream << ObjectConstraintHinge::constraintVersion;

    withReadLock([&] {
        dataStream << _pivotInA;
        dataStream << _axisInA;
        dataStream << _otherEntityID;
        dataStream << _pivotInB;
        dataStream << _axisInB;
        dataStream << _low;
        dataStream << _high;
        dataStream << _softness;
        dataStream << _biasFactor;
        dataStream << _relaxationFactor;

        dataStream << localTimeToServerTime(_expires);
        dataStream << _tag;

        dataStream << _motorVelocity;
        dataStream << _motorTarget;
        dataStream << _motorTargetTimeScale;
        dataStream << _maxImpulse;
    });

    return serializedConstraintArguments;
}

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

    EntityDynamicType type;
    dataStream >> type;
    assert(type == getType());

    QUuid id;
    dataStream >> id;
    assert(id == getID());

    uint16_t serializationVersion;
    dataStream >> serializationVersion;
    if (serializationVersion != ObjectConstraintHinge::constraintVersion) {
        assert(false);
        return;
    }

    withWriteLock([&] {
        dataStream >> _pivotInA;
        dataStream >> _axisInA;
        dataStream >> _otherEntityID;
        dataStream >> _pivotInB;
        dataStream >> _axisInB;
        dataStream >> _low;
        dataStream >> _high;
        dataStream >> _softness;
        dataStream >> _biasFactor;
        dataStream >> _relaxationFactor;

        quint64 serverExpires;
        dataStream >> serverExpires;
        _expires = serverTimeToLocalTime(serverExpires);

        dataStream >> _tag;

        dataStream >> _motorVelocity;
        dataStream >> _motorTarget;
        dataStream >> _motorTargetTimeScale;
        dataStream >> _maxImpulse;

        _active = true;
    });
}