#include "Robot/RobotPawn.h"

#include "LuckyDataTransferSubsystem.h"
#include "LuckySensorPawnBase.h"
#include "Kismet/GameplayStatics.h"
#include "Kismet/KismetMathLibrary.h"
#include "Kismet/KismetSystemLibrary.h"
#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h"
#include "Robot/PilotComponent/RobotPilotSO100Component.h"

ARobotPawn::ARobotPawn()
{
}

void ARobotPawn::BeginPlay()
{
	Super::BeginPlay();
	InitRobot(); // TODO Maybe move to GameInstance to control when we initialize the robot completely
}

void ARobotPawn::InitRobot()
{
	InitPilotComponent();
}

void ARobotPawn::InitPilotComponent()
{
	// Initialize pilot component based on robot type
	switch (RobotType)
	{
	case ERobotsName::None:
		break;
		
	case ERobotsName::SO100Robot:
		RobotPilotComponent = NewObject<URobotPilotSO100Component>(this);
		break;

	case ERobotsName::DJIDrone:
		RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(this);
		break;
	
	case ERobotsName::Luck_e:
		break; // TODO or remove from enum
	case ERobotsName::Stretch:
		break; // TODO or remove from enum
	case ERobotsName::LuckyDrone:
		break; // TODO or remove from enum
	case ERobotsName::ArmLucky:
		break; // TODO or remove from enum
	case ERobotsName::UnitreeG1:
		break; // TODO or remove from enum
	case ERobotsName::StretchRobotV1:
		break; // TODO or remove from enum
	case ERobotsName::PandaArmRobot:
		break; // TODO or remove from enum
	case ERobotsName::PuralinkRobot:
		break; // TODO or remove from enum
	case ERobotsName::UnitreeGo2:
		break; // TODO or remove from enum
	case ERobotsName::RevoluteRobot:
		break; // TODO or remove from enum
	case ERobotsName::BostonSpotRobot:
		break; // TODO or remove from enum
	}

	// Register if this Robot has a Pilot Component 
	if (RobotPilotComponent)
	{
		RobotPilotComponent->RegisterComponent();
	}
}

void ARobotPawn::EnableRemoteControl()
{
	// Get subsystem
	if (ULuckyDataTransferSubsystem* DataTransfer = GetWorld()->GetSubsystem<ULuckyDataTransferSubsystem>())
	{
		// Connect first if necessary
		if (!DataTransfer->Socket->IsConnected())
		{
			DataTransfer->ConnectToWebsocket("ws://127.0.0.1:3000", "");
		}

		// TODO Should we wait for connection to be successful before binding OnCommandReady?
		DataTransfer->OnCommandReady.AddDynamic(this->RobotPilotComponent, &URobotPilotComponent::ReceiveRemoteCommand);
	}
}