WIP - Configure Episode Series Launch

Source/LuckyWorldV2/Private/Episode/EpisodeSubSystem.cpp

@@ -1,4 +1,10 @@
 #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()
@@ -18,9 +24,40 @@ void UEpisodeSubSystem::Deinitialize()
 void UEpisodeSubSystem::Tick(float DeltaTime)
 {
 	// if capture has started
-	if (!bIsCapturing) return;
+	if (!bIsCapturing || CapturedEpisodes >= EpisodesCount) return;
 
-	// Flow that we need to figure out
+	// 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
@@ -29,23 +66,62 @@ void UEpisodeSubSystem::Tick(float DeltaTime)
 	// How to reset the episode?
 }
 
-void UEpisodeSubSystem::StartNewEpisodesSeries(int32 EpisodesCount)
+void UEpisodeSubSystem::StartNewEpisodesSeries(const int32 EpisodesCountIn)
 {
-	// Noah
-	// Configure the DataTransfer -> Use CurrentRobot->Cameras
-	// Start the Capture
-	// Make specs for JB to add API on the robot data
-	
-	// JB
-	// Find the current Robot in Scene
-	// Store as CurrentRobot
-	// Order the robot to go fetch an object
-	
-	// Both of us once we finished our own tasks and synced
-	// - Spawn the shape at random locations -> might need another controller like ProceduralSceneSubSystem?
-	// - Add the timestamp debug component on the scene to check if the rendered frame and the data are in sync
-	
-	// TODO Which Tick is responsible of the scene capture?
-
+	// 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
 }