//
//  ObjectMotionState.cpp
//  libraries/physcis/src
//
//  Created by Andrew Meadows 2014.11.05
//  Copyright 2014 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 <math.h>

#include "BulletUtil.h"
#include "ObjectMotionState.h"
#include "PhysicsEngine.h"
#include "PhysicsHelpers.h"
#include "PhysicsLogging.h"

// origin of physics simulation in world-frame
glm::vec3 _worldOffset(0.0f);

// static
void ObjectMotionState::setWorldOffset(const glm::vec3& offset) {
    _worldOffset = offset;
}

// static
const glm::vec3& ObjectMotionState::getWorldOffset() {
    return _worldOffset;
}

// static
uint32_t worldSimulationStep = 0;
void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
    assert(step > worldSimulationStep);
    worldSimulationStep = step;
}

uint32_t ObjectMotionState::getWorldSimulationStep() {
    return worldSimulationStep;
}

// static
ShapeManager* shapeManager = nullptr;
void ObjectMotionState::setShapeManager(ShapeManager* manager) {
    assert(manager);
    shapeManager = manager;
}

ShapeManager* ObjectMotionState::getShapeManager() {
    assert(shapeManager); // you must properly set shapeManager before calling getShapeManager()
    return shapeManager;
}

ObjectMotionState::ObjectMotionState(btCollisionShape* shape) :
    _motionType(MOTION_TYPE_STATIC),
    _shape(shape),
    _body(nullptr),
    _mass(0.0f),
    _lastKinematicStep(worldSimulationStep)
{
}

ObjectMotionState::~ObjectMotionState() {
    assert(!_body);
    assert(!_shape);
}

void ObjectMotionState::setBodyLinearVelocity(const glm::vec3& velocity) const {
    _body->setLinearVelocity(glmToBullet(velocity));
}

void ObjectMotionState::setBodyAngularVelocity(const glm::vec3& velocity) const {
    _body->setAngularVelocity(glmToBullet(velocity));
}

void ObjectMotionState::setBodyGravity(const glm::vec3& gravity) const {
    _body->setGravity(glmToBullet(gravity));
}

glm::vec3 ObjectMotionState::getBodyLinearVelocity() const {
    return bulletToGLM(_body->getLinearVelocity());
}

glm::vec3 ObjectMotionState::getBodyLinearVelocityGTSigma() const {
    // NOTE: the threshold to use here relates to the linear displacement threshold (dX) for sending updates
    // to objects that are tracked server-side (e.g. entities which use dX = 2mm).  Hence an object moving
    // just under this velocity threshold would trigger an update about V/dX times per second.
    const float MIN_LINEAR_SPEED_SQUARED = 0.0036f; // 6 mm/sec

    glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity());
    if (glm::length2(velocity) < MIN_LINEAR_SPEED_SQUARED) {
        velocity *= 0.0f;
    }
    return velocity;
}

glm::vec3 ObjectMotionState::getObjectLinearVelocityChange() const {
    return glm::vec3(0.0f);  // Subclasses override where meaningful.
}

glm::vec3 ObjectMotionState::getBodyAngularVelocity() const {
    return bulletToGLM(_body->getAngularVelocity());
}

void ObjectMotionState::releaseShape() {
    if (_shape) {
        shapeManager->releaseShape(_shape);
        _shape = nullptr;
    }
}

void ObjectMotionState::setMotionType(MotionType motionType) {
    _motionType = motionType;
}

void ObjectMotionState::setRigidBody(btRigidBody* body) {
    // give the body a (void*) back-pointer to this ObjectMotionState
    if (_body != body) {
        if (_body) {
            _body->setUserPointer(nullptr);
        }
        _body = body;
        if (_body) {
            _body->setUserPointer(this);
        }
    }
}

bool ObjectMotionState::handleEasyChanges(uint32_t flags, PhysicsEngine* engine) {
    if (flags & Simulation::DIRTY_POSITION) {
        btTransform worldTrans;
        if (flags & Simulation::DIRTY_ROTATION) {
            worldTrans.setRotation(glmToBullet(getObjectRotation()));
        } else {
            worldTrans = _body->getWorldTransform();
        }
        worldTrans.setOrigin(glmToBullet(getObjectPosition()));
        _body->setWorldTransform(worldTrans);
    } else if (flags & Simulation::DIRTY_ROTATION) {
        btTransform worldTrans = _body->getWorldTransform();
        worldTrans.setRotation(glmToBullet(getObjectRotation()));
        _body->setWorldTransform(worldTrans);
    }

    if (flags & Simulation::DIRTY_LINEAR_VELOCITY) {
        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
        _body->setGravity(glmToBullet(getObjectGravity()));
    }
    if (flags & Simulation::DIRTY_ANGULAR_VELOCITY) {
        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
    }

    if (flags & Simulation::DIRTY_MATERIAL) {
        updateBodyMaterialProperties();
    }

    if (flags & Simulation::DIRTY_MASS) {
        updateBodyMassProperties();
    }

    return true;
}

bool ObjectMotionState::handleHardAndEasyChanges(uint32_t flags, PhysicsEngine* engine) {
    if (flags & Simulation::DIRTY_SHAPE) {
        // make sure the new shape is valid
        if (!isReadyToComputeShape()) {
            return false;
        }
        btCollisionShape* newShape = computeNewShape();
        if (!newShape) {
            qCDebug(physics) << "Warning: failed to generate new shape!";
            // failed to generate new shape! --> keep old shape and remove shape-change flag
            flags &= ~Simulation::DIRTY_SHAPE;
            // TODO: force this object out of PhysicsEngine rather than just use the old shape
            if ((flags & HARD_DIRTY_PHYSICS_FLAGS) == 0) {
                // no HARD flags remain, so do any EASY changes
                if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
                    handleEasyChanges(flags, engine);
                }
                return true;
            }
        }
        getShapeManager()->releaseShape(_shape);
        if (_shape != newShape) {
            _shape = newShape;
            _body->setCollisionShape(_shape);
        } else {
            // huh... the shape didn't actually change, so we clear the DIRTY_SHAPE flag
            flags &= ~Simulation::DIRTY_SHAPE;
        }
    }
    if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
        handleEasyChanges(flags, engine);
    }
    // it is possible that there are no HARD flags at this point (if DIRTY_SHAPE was removed)
    // so we check again befoe we reinsert:
    if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
        engine->reinsertObject(this);
    }

    return true;
}

void ObjectMotionState::updateBodyMaterialProperties() {
    _body->setRestitution(getObjectRestitution());
    _body->setFriction(getObjectFriction());
    _body->setDamping(fabsf(btMin(getObjectLinearDamping(), 1.0f)), fabsf(btMin(getObjectAngularDamping(), 1.0f)));
}

void ObjectMotionState::updateBodyVelocities() {
    setBodyLinearVelocity(getObjectLinearVelocity());
    setBodyAngularVelocity(getObjectAngularVelocity());
    setBodyGravity(getObjectGravity());
    _body->setActivationState(ACTIVE_TAG);
}

void ObjectMotionState::updateBodyMassProperties() {
    float mass = getMass();
    btVector3 inertia(0.0f, 0.0f, 0.0f);
    _body->getCollisionShape()->calculateLocalInertia(mass, inertia);
    _body->setMassProps(mass, inertia);
    _body->updateInertiaTensor();
}