#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 "../../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.push(entity_id);
        phys_comp.needs_update = true;
    }
}

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;
    
}

void HydraPhysicsSystem::DestroyComponent(HydraID entity_id)
{
}

void HydraPhysicsSystem::UpdateSystem(float delta_t_seconds)
{
    HydraTaskScheduler *task_scheduler = GetEngine()->TaskScheduler();
    while(m_bodies_to_set.size() >0)
    {
        HydraID entity_id = m_bodies_to_set.front();
        m_bodies_to_set.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);          
    }
       
}

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);
}