#include "physics.hpp"
#include "filesystem.hpp"
#include "ia.hpp"
#include <nlohmann/json.hpp>

using namespace TSR;
using namespace nlohmann;

PxDefaultErrorCallback Physics::s_default_error_cb;
PxDefaultAllocator Physics::s_default_allocator_cb;

PxFoundation* Physics::s_foundation = nullptr;
PxPhysics* Physics::s_physics = nullptr;
PxCooking* Physics::s_cooking = nullptr;
PxPvd* Physics::s_pvd = nullptr;

void Physics::Init() {
    // PHYSX init
    static PxDefaultErrorCallback gDefaultErrorCallback;
    static PxDefaultAllocator gDefaultAllocatorCallback;

    s_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, gDefaultAllocatorCallback, gDefaultErrorCallback);
    if (!s_foundation)
        Throw("(PX) PxCreateFoundation failed!");

    bool recordMemoryAllocations = true;

    //auto mPvd = PxCreatePvd(*s_foundation);
    //PxPvdTransport* transport = PxDefaultPvdSocketTransportCreate("localhost", 5425, 10);
    //mPvd->connect(*transport, PxPvdInstrumentationFlag::eALL);

    PxTolerancesScale scale;

    s_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *s_foundation, scale, recordMemoryAllocations, nullptr);
    if (!s_physics)
        Throw("(PX) PxCreatePhysics failed!");

    s_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *s_foundation, PxCookingParams(scale));
    if (!s_cooking)
        Throw("(PX) PxCreateCooking failed!");

    PxCookingParams params(scale);
    s_cooking->setParams(params);
}

void Physics::Close() {
    if (s_cooking)
        s_cooking->release();
    
    if (s_physics)
        s_physics->release();
    
    if (s_foundation)
        s_foundation->release();
}

PhysicsScene::PhysicsScene() {
    PxSceneDesc sceneDesc(Physics::GetPhysics()->getTolerancesScale());
    sceneDesc.gravity = PxVec3(0.0f, -9.81f, 0.0f);
    // create CPU dispatcher which mNbThreads worker threads
    m_cpu_dispatcher = PxDefaultCpuDispatcherCreate(1);
    if (!m_cpu_dispatcher)
        Throw("(PX) PxDefaultCpuDispatcherCreate failed!");

    sceneDesc.cpuDispatcher = m_cpu_dispatcher;

    if (!sceneDesc.filterShader)
        sceneDesc.filterShader = &PxDefaultSimulationFilterShader;

    m_scene = Physics::GetPhysics()->createScene(sceneDesc);
    if (!m_scene)
        Throw("(PX) createScene failed!");

    m_cct_manager = PxCreateControllerManager(*m_scene);
    if (!m_cct_manager)
        Throw("(PX) PxCreateControllerManager failed!");

    m_scene->userData = this;
}

PhysicsScene::~PhysicsScene() {
    m_cct_manager->release();
    m_scene->release();
}

void PhysicsScene::StepSimulation(float time) {
    m_scene->simulate(time);
    m_scene->fetchResults(true);
}

void PhysicsScene::EnableDebug(bool enable) {
    m_scene->setVisualizationParameter(PxVisualizationParameter::eSCALE, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 2.0f : 0.0f);
    ////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
    ////m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eMBP_REGIONS, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eACTOR_AXES, enable ? 0.5f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_ANG_VELOCITY, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_AXES, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_LIN_VELOCITY, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eBODY_MASS_AXES, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AABBS, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_AXES, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_COMPOUNDS, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_DYNAMIC, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_EDGES, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_FNORMALS, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_SHAPES, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCOLLISION_STATIC, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_ERROR, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_FORCE, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_NORMAL, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eCONTACT_POINT, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eCULL_BOX, enable ? 1.0f : 0.0f);
    //m_scene->setVisualizationParameter(PxVisualizationParameter::eFORCE_DWORD, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LIMITS, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eJOINT_LOCAL_FRAMES, enable ? 1.0f : 0.0f);
    m_scene->setVisualizationParameter(PxVisualizationParameter::eWORLD_AXES, enable ? 1.0f : 0.0f);
    
}

static glm::vec4 U32ColorToVec4(uint32_t rgba_value) {
    float red = static_cast<float>((rgba_value >> 24) & 0xFF) / 255.0f;
    float green = static_cast<float>((rgba_value >> 16) & 0xFF) / 255.0f;
    float blue = static_cast<float>((rgba_value >> 8) & 0xFF) / 255.0f;
    float alpha = static_cast<float>(rgba_value & 0xFF) / 255.0f;

    return glm::vec4(red, green, blue, alpha);
}

void PhysicsScene::DrawDebug(Renderer& renderer) {
    const PxRenderBuffer& rb = m_scene->getRenderBuffer();
    for (PxU32 i = 0; i < rb.getNbLines(); i++) {
        const PxDebugLine& line = rb.getLines()[i];
        
        glm::vec3 pos1(line.pos0.x, line.pos0.y, line.pos0.z);
        glm::vec3 pos2(line.pos1.x, line.pos1.y, line.pos1.z);
        glm::vec3 color1 = U32ColorToVec4(line.color0);
        glm::vec3 color2 = U32ColorToVec4(line.color1);

        renderer.AddDebugLine(pos1, pos2, color1, color2);
    }
}

PhysicsMaterial::PhysicsMaterial(const std::string& path) {
    float static_friction, dynamic_friction, restitution;
    try {
        auto j_material = json::parse(Filesystem::Read(path));

        j_material.at("static_friction").get_to(static_friction);
        j_material.at("dynamic_friction").get_to(dynamic_friction);
        j_material.at("restitution").get_to(restitution);
    }
    catch (json::exception ex) {
        Throw(ex.what());
    }

    m_material = Physics::GetPhysics()->createMaterial(static_friction, dynamic_friction, restitution);
    m_material->userData = this;
}

PhysicsMaterial::~PhysicsMaterial() {
    m_material->release();
}

PhysicsTriangleMesh::PhysicsTriangleMesh(const std::string& path) {
    auto ia_str = Filesystem::Read(path);

    auto mia3 = ParseIA(ia_str);

    PxTriangleMeshDesc mesh_desc;
    mesh_desc.points.count = mia3.num_vertices;
    mesh_desc.points.stride = sizeof(PxVec3);
    mesh_desc.points.data = mia3.vertices_ptr;

    mesh_desc.triangles.count = mia3.num_indices / 3;
    mesh_desc.triangles.stride = 3 * sizeof(PxU32);
    mesh_desc.triangles.data = mia3.indices_ptr;

#ifdef _DEBUG
    // mesh should be validated before cooked without the mesh cleaning
    bool res = Physics::GetCooking()->validateTriangleMesh(mesh_desc);
    PX_ASSERT(res);
#endif

    m_mesh = Physics::GetCooking()->createTriangleMesh(mesh_desc, Physics::GetPhysics()->getPhysicsInsertionCallback());
}

PhysicsTriangleMesh::~PhysicsTriangleMesh() {
    m_mesh->release();
}