#include "HydraPhysicsSystem.h"
#include "../../engine/HydraEngine.h"
#include "../../scheduler/HydraTaskScheduler.h"
#include "../../task/physics/HydraInitialisePhysicsWorldTask.hpp"
#include "../../task/physics/HydraDestroyPhysicsWorldTask.hpp"
#include "../../task/physics/HydraCreatePhysicsObjectTask.hpp"
#include "../../task/physics/HydraSetPhysicsTransform.hpp"
#include "../../task/physics/HydraGetPhysicsUpdatesTask.hpp"
#include "../../task/physics/HydraStepPhysicsTask.h"
#include "../../task/physics/HydraSetPhysicsLinearVelocity.hpp"
#include "../../task/physics/HydraDeletePhysicsObjectTask.h"
#include "../../ecs/HydraECS.h"
#include "../../ecs/components/HydraPhysicsComponent.h"
#include "../../ecs/components/HydraPositionComponent.h"


void HydraPhysicsSystem::Initialise()
{
    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();

    HydraID task_id = task_scheduler->CreateTask<HydraInitialisePhysicsWorldTask>(HydraThreadAffinity::Physics);
    HydraInitialisePhysicsWorldTask *task = static_cast<HydraInitialisePhysicsWorldTask *>(task_scheduler->GetTask(task_id));
    task->Initialise(&m_world_id);
    task_scheduler->WaitForTask(task_id);  
}

void HydraPhysicsSystem::Shutdown()
{
    //TODO destroy all objects here then

    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();

    HydraID task_id = task_scheduler->CreateTask<HydraDestroyPhysicsWorldTask>(HydraThreadAffinity::Physics);
    HydraDestroyPhysicsWorldTask *task = static_cast<HydraDestroyPhysicsWorldTask *>(task_scheduler->GetTask(task_id));
    task->Initialise(m_world_id);
    task_scheduler->WaitForTask(task_id);  

}

void HydraPhysicsSystem::SetTransform(HydraID entity_id)
{
    HydraPhysicsComponent &phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);
    if(!phys_comp.needs_update)
    {
        m_bodies_to_set_transform.push(entity_id);
        phys_comp.needs_update = true;
    }
    // HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();
    // HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);
    // HydraPositionComponent& pos_comp = GetEngine()->ECS()->GetComponent<HydraPositionComponent>(entity_id);

    // HydraID task_id = task_scheduler->CreateTask<HydraSetPhysicsTransform>(HydraThreadAffinity::Physics);
    // HydraSetPhysicsTransform *set_task = static_cast<HydraSetPhysicsTransform *>(task_scheduler->GetTask(task_id));
    // set_task->Initialise(phys_comp.body_id, pos_comp.position, glm::quat(pos_comp.orientation));
    // task_scheduler->WaitForTask(task_id);       
}

void HydraPhysicsSystem::SetLinearVelocity(HydraID entity_id, glm::vec3 velocity)
{
    PhysicsVelocity vel = {entity_id, velocity};
    m_bodies_to_set_velocity.push(vel);
    // HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);
    // HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();
    // HydraID task_id = task_scheduler->CreateTask<HydraSetPhysicsLinearVelocity>(HydraThreadAffinity::Physics);
    // HydraSetPhysicsLinearVelocity *set_task = static_cast<HydraSetPhysicsLinearVelocity *>(task_scheduler->GetTask(task_id));
    // set_task->Initialise(phys_comp.body_id, velocity);
    // task_scheduler->WaitForTask(task_id); 
}

void HydraPhysicsSystem::InitialiseComponent(HydraID entity_id)
{
    b3BodyId body_id = {};

    HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);
    HydraPositionComponent& pos_comp = GetEngine()->ECS()->GetComponent<HydraPositionComponent>(entity_id);

    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();

    HydraID task_id = task_scheduler->CreateTask<HydraCreatePhysicsObjectTask>(HydraThreadAffinity::Physics);
    HydraCreatePhysicsObjectTask *task = static_cast<HydraCreatePhysicsObjectTask *>(task_scheduler->GetTask(task_id));
    task->Initialise(m_world_id, phys_comp, &body_id, entity_id);
    task_scheduler->WaitForTask(task_id);  
    phys_comp.body_id = body_id;

    task_id = task_scheduler->CreateTask<HydraSetPhysicsTransform>(HydraThreadAffinity::Physics);
    HydraSetPhysicsTransform *set_task = static_cast<HydraSetPhysicsTransform *>(task_scheduler->GetTask(task_id));
    set_task->Initialise(body_id, pos_comp.position, glm::quat(pos_comp.orientation));
    task_scheduler->WaitForTask(task_id);  
    phys_comp.initialised = true;
    DebugLog("PHYSICS CREATED:" + std::to_string(entity_id) + "\n");
    
}

void HydraPhysicsSystem::DestroyComponent(HydraID entity_id)
{
    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();
    HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);

    if(phys_comp.initialised)
    {
        HydraID task_id = task_scheduler->CreateTask<HydraDeletePhysicsObjectTask>(HydraThreadAffinity::Physics);
        HydraDeletePhysicsObjectTask *task = static_cast<HydraDeletePhysicsObjectTask *>(task_scheduler->GetTask(task_id));
        task->Initialise(phys_comp.body_id);
        task_scheduler->WaitForTask(task_id);  
    }
    else
    {
        std::cout << "here";
    }
    DebugLog("PHYSICS DELETED:" + std::to_string(entity_id) + "\n");
}

void HydraPhysicsSystem::UpdateSystem(float delta_t_seconds)
{
    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();
    while(m_bodies_to_set_transform.size() >0)
    {
        HydraID entity_id = m_bodies_to_set_transform.front();
        m_bodies_to_set_transform.pop();

        HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(entity_id);
        HydraPositionComponent& pos_comp = GetEngine()->ECS()->GetComponent<HydraPositionComponent>(entity_id);

        HydraID task_id = task_scheduler->CreateTask<HydraSetPhysicsTransform>(HydraThreadAffinity::Physics);
        HydraSetPhysicsTransform *set_task = static_cast<HydraSetPhysicsTransform *>(task_scheduler->GetTask(task_id));
        set_task->Initialise(phys_comp.body_id, pos_comp.position, glm::quat(pos_comp.orientation));
        task_scheduler->WaitForTask(task_id);          
    }
    while(m_bodies_to_set_velocity.size() >0)
    {
        PhysicsVelocity vel = m_bodies_to_set_velocity.front();
        m_bodies_to_set_velocity.pop();
        HydraPhysicsComponent& phys_comp = GetEngine()->ECS()->GetComponent<HydraPhysicsComponent>(vel.entity_id);

        HydraID task_id = task_scheduler->CreateTask<HydraSetPhysicsLinearVelocity>(HydraThreadAffinity::Physics);
        HydraSetPhysicsLinearVelocity *set_task = static_cast<HydraSetPhysicsLinearVelocity *>(task_scheduler->GetTask(task_id));
        set_task->Initialise(phys_comp.body_id, vel.velocity);
        task_scheduler->WaitForTask(task_id);          
    }
}

void HydraPhysicsSystem::StepPhysics(float delta_t)
{
    HydraID physics_step_task_id = GetEngine()->TaskScheduler()->CreateTask<HydraStepPhysicsTask>(HydraThreadAffinity::Physics);
    HydraStepPhysicsTask* physics_task = static_cast<HydraStepPhysicsTask*>(GetEngine()->TaskScheduler()->GetTask(physics_step_task_id));
    physics_task->Initialise(m_world_id, delta_t);
    GetEngine()->TaskScheduler()->WaitForTask(physics_step_task_id);
}