#include "workspace.h"
#include "datatypes/vector.h"
#include "objects/base/instance.h"
#include "objects/jointsservice.h"
#include "objects/joint/jointinstance.h"
#include "objects/datamodel.h"
#include "physics/util.h"
#include <memory>
#include <reactphysics3d/engine/PhysicsCommon.h>

rp::PhysicsCommon* Workspace::physicsCommon = new rp::PhysicsCommon;

Workspace::Workspace(): Service(&TYPE) {
}

Workspace::~Workspace() {
    if (physicsWorld && physicsCommon)
        physicsCommon->destroyPhysicsWorld(physicsWorld);
}

void Workspace::InitService() {
    if (initialized) return;
    initialized = true;

    physicsWorld = physicsCommon->createPhysicsWorld();

    physicsWorld->setGravity(rp::Vector3(0, -196.2, 0));
    // world->setContactsPositionCorrectionTechnique(rp3d::ContactsPositionCorrectionTechnique::BAUMGARTE_CONTACTS);
    physicsWorld->setNbIterationsPositionSolver(2000);
    physicsWorld->setNbIterationsVelocitySolver(2000);
    // physicsWorld->setSleepLinearVelocity(10);
    // physicsWorld->setSleepAngularVelocity(5);

    // physicsWorld->setEventListener(&eventListener);

    // Sync all parts
    for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
        InstanceRef obj = *it;
        if (!obj->IsA<Part>()) continue;
        std::shared_ptr<Part> part = obj->CastTo<Part>().expect();
        this->SyncPartPhysics(part);
        part->MakeJoints();
    }

    // Activate all joints
    for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
        InstanceRef obj = *it;
        if (!obj->IsA<JointInstance>()) continue;
        std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
        joint->UpdateProperty("Part0");
    }

    for (auto obj : dataModel().value()->GetService<JointsService>()->GetChildren()) {
        if (!obj->IsA<JointInstance>()) continue;
        std::shared_ptr<JointInstance> joint = obj->CastTo<JointInstance>().expect();
        joint->UpdateProperty("Part0");
    }
}

void Workspace::SyncPartPhysics(std::shared_ptr<Part> part) {
    if (!physicsWorld) return;

    glm::mat4 rotMat = glm::mat4(1.0f);

    rp::Transform transform = part->cframe;
    if (!part->rigidBody) {
        part->rigidBody = physicsWorld->createRigidBody(transform);
    } else {
        part->rigidBody->setTransform(transform);
    }

    rp::BoxShape* shape = physicsCommon->createBoxShape(glmToRp(part->size * glm::vec3(0.5f)));

    if (part->rigidBody->getNbColliders() > 0)
        part->rigidBody->removeCollider(part->rigidBody->getCollider(0));

    part->rigidBody->addCollider(shape, rp::Transform());
    part->rigidBody->setType(part->anchored ? rp::BodyType::STATIC : rp::BodyType::DYNAMIC);
    part->rigidBody->getCollider(0)->setCollisionCategoryBits(0b11);

    rp::Material& material = part->rigidBody->getCollider(0)->getMaterial();
    material.setFrictionCoefficient(0.35);
    material.setMassDensity(1.f);

    //https://github.com/DanielChappuis/reactphysics3d/issues/170#issuecomment-691514860
    part->rigidBody->updateMassFromColliders();
    part->rigidBody->updateLocalInertiaTensorFromColliders();

    part->rigidBody->setLinearVelocity(part->velocity);
    // part->rigidBody->setMass(density * part->size.x * part->size.y * part->size.z);

    part->rigidBody->setUserData(&*part);
}

void Workspace::PhysicsStep(float deltaTime) {
    // Step the simulation a few steps
    physicsWorld->update(std::min(deltaTime / 2, (1/60.f)));

    // Naive implementation. Parts are only considered so if they are just under Workspace
    // TODO: Add list of tracked parts in workspace based on their ancestry using inWorkspace property of Instance
    for (auto it = this->GetDescendantsStart(); it != this->GetDescendantsEnd(); it++) {
        InstanceRef obj = *it;
        if (obj->GetClass()->className != "Part") continue; // TODO: Replace this with a .IsA call instead of comparing the class name directly
        std::shared_ptr<Part> part = std::dynamic_pointer_cast<Part>(obj);
        const rp::Transform& transform = part->rigidBody->getTransform();
        part->cframe = CFrame(transform);
        part->velocity = part->rigidBody->getLinearVelocity();

        // part->rigidBody->enableGravity(true);
        for (auto& joint : part->secondaryJoints) {
            if (joint.expired() || !joint.lock()->IsA("RotateV")) continue;
                        
            std::shared_ptr<JointInstance> motor = joint.lock()->CastTo<JointInstance>().expect();
            float rate = motor->part0.lock()->GetSurfaceParamB(motor->c0.LookVector().Unit()) * 30;
            // part->rigidBody->enableGravity(false);
            part->rigidBody->setAngularVelocity(-(motor->part0.lock()->cframe * motor->c0).LookVector() * rate);
        }
    }
}


RaycastResult::RaycastResult(const rp::RaycastInfo& raycastInfo)
    : worldPoint(raycastInfo.worldPoint)
    , worldNormal(raycastInfo.worldNormal)
    , hitFraction(raycastInfo.hitFraction)
    , triangleIndex(raycastInfo.triangleIndex)
    , body(raycastInfo.body)
    , collider(raycastInfo.collider) {}

class NearestRayHit : public rp::RaycastCallback {
    rp::Vector3 startPos;
    std::optional<RaycastFilter> filter;

    std::optional<RaycastResult> nearestHit;
    float nearestHitDistance = -1;

    // Order is not guaranteed, so we have to figure out the nearest object using a more sophisticated algorith,
    rp::decimal notifyRaycastHit(const rp::RaycastInfo& raycastInfo) override {
        // If the detected object is further away than the nearest object, continue.
        int distance = (raycastInfo.worldPoint - startPos).length();
        if (nearestHitDistance != -1 && distance >= nearestHitDistance)
            return 1;

        if (!filter) {
            nearestHit = raycastInfo;
            nearestHitDistance = distance;
            return 1;
        }

        std::shared_ptr<Part> part = partFromBody(raycastInfo.body);
        FilterResult result = filter.value()(part);
        if (result == FilterResult::BLOCK) {
            nearestHit = std::nullopt;
            nearestHitDistance = distance;
            return 1;
        } else if (result == FilterResult::TARGET) {
            nearestHit = raycastInfo;
            nearestHitDistance = distance;
            return 1;
        }
        return 1;
    };

public:
    NearestRayHit(rp::Vector3 startPos, std::optional<RaycastFilter> filter = std::nullopt) : startPos(startPos), filter(filter) {}
    std::optional<const RaycastResult> getNearestHit() { return nearestHit; };
};

std::optional<const RaycastResult> Workspace::CastRayNearest(glm::vec3 point, glm::vec3 rotation, float maxLength, std::optional<RaycastFilter> filter, unsigned short categoryMaskBits) {
    rp::Ray ray(glmToRp(point), glmToRp(glm::normalize(rotation)) * maxLength);
    NearestRayHit rayHit(glmToRp(point), filter);
    physicsWorld->raycast(ray, &rayHit, categoryMaskBits);
    return rayHit.getNearestHit();
}

void Workspace::DestroyRigidBody(rp::RigidBody* rigidBody) {
    physicsWorld->destroyRigidBody(rigidBody);
}