#include "Episode/EpisodeSubSystem.h"
#include "Actors/MujocoStaticMeshActor.h"
#include "Actors/MujocoVolumeActor.h"
#include "Kismet/GameplayStatics.h"
#include "Robot/RobotPawn.h"
#include "Robot/PilotComponent/RobotPilotComponent.h"
#include "Robot/PilotComponent/RobotPilotSO100Component.h"


UEpisodeSubSystem::UEpisodeSubSystem()
{
}

void UEpisodeSubSystem::Initialize(FSubsystemCollectionBase& Collection)
{
	Super::Initialize(Collection);
}

void UEpisodeSubSystem::Deinitialize()
{
	Super::Deinitialize();
}

void UEpisodeSubSystem::Tick(float DeltaTime)
{
	// if capture has started
	if (!bIsCapturing || CapturedEpisodes >= EpisodesCount) return;

	// Here we are capturing the data, running an episode
	
	// Noah
	// Configure the DataTransfer -> Use CurrentRobot->Cameras
	// Start the Capture
	// Make specs for JB to add API on the robot data

	// JB
	// Check status of the episode
	// object near base location ?
	// Get Robot right zone
	// Get Robot left zone
	// Move Object at Transform
	// Robot->Grab object at Location + Deliver at Zone left/right
	// Start Episode new episode - increase counter
	
	// Check the distance of Object to BaseLocation 
	// EpisodeTargetObject->MainActorBody->GetComponentTransform();
	// Order the robot to go fetch an object
	
	// Both of us once we finished our own tasks and synced
	// Add the timestamp debug component on the scene to check if the rendered frame and the data are in sync

	// Tick
	// Get object location + compute velocity
	// if velocity == 0 -> set Time since velocity 0
	// if velocity == 0 for time > trigger restart time
	// check object location - is in drop zone?
	// if in drop zone -> set success
	
	// Check ObjectLocation
	// If Velocity 
	
	// ProceduralSceneController - is object spawned? -> If not spawn it
	// if object spawned && !robot.hasTarget -> Robot -> SetTarget
	// if object.IsSpawned && robot.hasTarget -> Capture and send data
	// ProceduralSceneController -> is object collected? 
	// How to reset the episode?
}

void UEpisodeSubSystem::StartNewEpisodesSeries(const int32 EpisodesCountIn)
{
	// Robot and Exercise
	FindEpisodeObjectFromScene();
	FindRobotPawnFromScene();
	EpisodesCount = EpisodesCountIn;
	bIsCapturing = true;
	CurrentObjectBaseLocation = CurrentRobot->RobotPilotComponent->GetReachableTransform();

	// Data
	ConfigureDataCapture();
}

void UEpisodeSubSystem::StartEpisode()
{
	
}

void UEpisodeSubSystem::FindEpisodeObjectFromScene()
{
	TArray<AActor*> MujocoObjects;
	UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), AMujocoStaticMeshActor::StaticClass(), MujocoObjects);
	if (MujocoObjects.IsValidIndex(0) && Cast<AMujocoStaticMeshActor>(MujocoObjects[0]))
	{
		EpisodeTargetObject = Cast<AMujocoStaticMeshActor>(MujocoObjects[0]);		
	}
}

void UEpisodeSubSystem::FindRobotPawnFromScene()
{
	TArray<AActor*> RobotPawns;
	UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), ARobotPawn::StaticClass(), RobotPawns);
	if (RobotPawns.IsValidIndex(0) && Cast<ARobotPawn>(RobotPawns[0]))
	{
		CurrentRobot = Cast<ARobotPawn>(RobotPawns[0]);		
	}
}

void UEpisodeSubSystem::ConfigureDataCapture()
{
	// Noah's space of artistic expression
}

void UEpisodeSubSystem::CreatePayload()
{
	// CurrentRobot->Cameras
	// CurrentRobot -> Tell JB what he should expose on the RobotPawn
	const auto TimeStamp = CurrentRobot->PhysicsSceneProxy->GetMujocoData().time;
	const auto So100PilotCmp = Cast<URobotPilotSO100Component>(CurrentRobot->RobotPilotComponent);
	const auto Joints = So100PilotCmp->GetCurrentControlsFromPhysicScene();

	// JB
	// Here I need this specific data
}

void UEpisodeSubSystem::SendEpisodeData()
{
	// Send the Data
}