// Fill out your copyright notice in the Description page of Project Settings.

#include "Actor/InverseWorldPawn.h"

#include "Actor/BaseMotionController.h"
#include "Engine/StaticMeshActor.h"
#include "Engine/World.h"
#include "EngineUtils.h"
#include "GameFramework/WorldSettings.h"
#include "HeadMountedDisplayFunctionLibrary.h"
#include "Kismet/GameplayStatics.h"
#include "RHIDefinitions.h"

#if !UE_BUILD_SHIPPING
#pragma optimize("", off)
#endif


 AInverseWorldPawn::AInverseWorldPawn()
{
	 PrimaryActorTick.bCanEverTick = true;

	 HMDScene = CreateDefaultSubobject<USceneComponent>(TEXT("VR HMD Scene"));
	 SetRootComponent(HMDScene);

	 VRCamera = CreateDefaultSubobject<UCameraComponent>(TEXT("VR Camera"));
	 VRCamera->SetupAttachment(HMDScene);
 }

// Called when the game starts or when spawned.
void AInverseWorldPawn::BeginPlay()
{
	Super::BeginPlay();

	// Fill scaled/moving actor array.
	UGameplayStatics::GetAllActorsWithTag(this->GetWorld(), FName("moving_actor"), MovingActors);
	for (AActor* Actor : MovingActors)
	{
		MovingActorDefaultTransforms.Add(Actor, Actor->GetTransform());
	}
	MovingActorDefaultTransforms.Add(this, GetActorTransform());

	TArray<AActor*> PivotActors;
	UGameplayStatics::GetAllActorsWithTag(this->GetWorld(), FName("pivot_actor"), PivotActors);
	for (AActor* Actor : PivotActors)
	{
		PivotActor = Actor;
		break;
	}

	FActorSpawnParameters SpawnParams;
	SpawnParams.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AlwaysSpawn;
	SpawnParams.Owner = this;

	TSubclassOf<ABaseMotionController> RightControllerClass = DefaultControllers.ControllerRightHand;
	TSubclassOf<ABaseMotionController> LeftControllerClass = DefaultControllers.ControllerLeftHand;

	if (RightControllerClass && LeftControllerClass)
	{
		RightController = GetWorld()->SpawnActor<ABaseMotionController>(RightControllerClass, SpawnParams);
		LeftController = GetWorld()->SpawnActor<ABaseMotionController>(LeftControllerClass, SpawnParams);

		FAttachmentTransformRules Rules(
			EAttachmentRule::KeepRelative, EAttachmentRule::KeepRelative, EAttachmentRule::KeepRelative, false);

		RightController->AttachToComponent(RootComponent, Rules);
		LeftController->AttachToComponent(RootComponent, Rules);
	}

	SetupControllerInputs();
	InputComponent->BindAction("Reset_Button", IE_Pressed, this, &AInverseWorldPawn::ResetTransform);
}

void AInverseWorldPawn::SetupControllerInputs()
{
	if (RightController)
	{
		RightController->SetupInput(InputComponent);
	}
	if (LeftController)
	{
		LeftController->SetupInput(InputComponent);
	}
}

// Called every frame.
void AInverseWorldPawn::Tick(float DeltaTime)
{
	Super::Tick(DeltaTime);
	HandleControllerTick(DeltaTime);
}

// Move actor closer to/farther from Pivot depending on scale change.
void AInverseWorldPawn::FixActorLocationAccordingToScaleChange(AActor* InOutActor, const float InScaleChangeRatio)
{
	const FTransform PivotTransform = PivotActor->GetTransform();;
	FTransform ActorTransform = InOutActor->GetTransform();

	const FVector PivotVector = PivotTransform.GetTranslation();
	const FVector ActorVector = ActorTransform.GetTranslation();

	FVector Diff = ActorVector - PivotVector;
	Diff /= InScaleChangeRatio;
	Diff += PivotVector;
	ActorTransform.SetTranslation(Diff);

	InOutActor->SetActorTransform(ActorTransform);
};

void AInverseWorldPawn::TransformActor(AActor* InOutActor, const FTransform& InTransformChange)
{
	// Move and rotate actor.
	InOutActor->AddActorWorldOffset(InTransformChange.GetTranslation());
	InOutActor->AddActorLocalRotation(InTransformChange.GetRotation());

	// Rotate actor around Pivot to compensate for orientation change.
	CompensateActorRotationAroundPivot(InOutActor, InTransformChange);
};

// Rotate actor around Pivot to compensate for orientation change.
void AInverseWorldPawn::CompensateActorRotationAroundPivot(AActor* InOutActor, const FTransform& InTransformChange)
{
	const FTransform PivotTransform = PivotActor->GetActorTransform();

	FTransform ActorTransform = InOutActor->GetTransform();

	const FVector PivotVector = PivotTransform.GetTranslation();
	const FVector ActorVector = ActorTransform.GetTranslation();
	FVector Diff = ActorVector - PivotVector;
	Diff = Diff.RotateAngleAxis(InTransformChange.GetRotation().Rotator().Yaw, FVector(0, 0, 1));
	Diff += PivotVector;

	ActorTransform.SetTranslation(Diff);
	InOutActor->SetActorTransform(ActorTransform);
};

void AInverseWorldPawn::ScaleWorld(FTransform& InOutTransformChange)
{
	// Because we invert the transform, we need to invert the ratio back.
	float ScaleChangeRatio = 1 / InOutTransformChange.GetScale3D().X;

	// Skip if almost no change, and avoid division by zero.
	if (FMath::IsNearlyEqual(ScaleChangeRatio, 1.f) || FMath::IsNearlyEqual(ScaleChangeRatio, 0.f))
	{
		return;
	}

	float DesiredScale = CurrentScale * ScaleChangeRatio;
	if (DesiredScale > MaxScale)
	{
		// We're trying to go above max scale -> clamp it!
		DesiredScale = MaxScale;
		ScaleChangeRatio = MaxScale / CurrentScale;
		InOutTransformChange.SetScale3D(FVector(1 / ScaleChangeRatio));
	}
	else if (DesiredScale < MinScale)
	{
		// We're trying to go below min scale -> clamp it!
		DesiredScale = MinScale;
		ScaleChangeRatio = MinScale / CurrentScale;
		InOutTransformChange.SetScale3D(FVector(1 / ScaleChangeRatio));
	}
	CurrentScale = DesiredScale;

	// Change world scale.
	GetWorldSettings()->WorldToMeters = GetWorld()->GetWorldSettings()->WorldToMeters / ScaleChangeRatio;

	// Scale actors that need it (like floor for example).
	for (AActor* Actor : MovingActors)
	{
		// Scale actor.
		Actor->SetActorRelativeScale3D(Actor->GetActorRelativeScale3D() / ScaleChangeRatio);

		// Move actor closer to/farther from pivot depending on scale change.
		FixActorLocationAccordingToScaleChange(Actor, ScaleChangeRatio);
	}

	// Scale controllers.
	LeftController->ScaleMesh(ScaleChangeRatio);
	RightController->ScaleMesh(ScaleChangeRatio);

	// Move player pawn closer to/farther from pivot depending on scale change.
	FixActorLocationAccordingToScaleChange(this, ScaleChangeRatio);
};

void AInverseWorldPawn::HandleControllerTick(float DeltaTime)
{

	// Return if there are no controllers.
	if (!LeftController || !RightController)
	{
		return;
	}

	// Return if neither grip is pressed;
	if (!LeftController->IsMovingWorld() && !RightController->IsMovingWorld())
	{
		// Save world scale for next frame.
		SaveWorldToMeters();

		// Save controller locations and rotations for next frame.
		SaveControllerTransforms();

		return;
	}

	// Calculate TransformChange from controller movements.
	FTransform TransformChange = CalculateTransformChange();
	FVector RotatedTranslation = GetActorRotation().RotateVector(TransformChange.GetTranslation());
	TransformChange.SetTranslation(RotatedTranslation);

	// Invert transform axes + rotation to create feeling of rotating the pivot.
	TransformChange = TransformChange.Inverse();

	// Save current world scale for next frame.
	SaveWorldToMeters();

	// Apply transform change.
	if (!TransformChange.Equals(FTransform::Identity))
	{
		// Scale world and objects that need scaling to appear stationary.
		if (TransformChange.GetScale3D() != FVector(1, 1, 1))
		{
			ScaleWorld(TransformChange);
			TransformChange.SetRotation(FQuat::Identity);
			TransformChange.SetTranslation(FVector(0, 0, 0));
		}
		else
		{
			TransformActor(this, TransformChange);

			// Rotate actors that need it (like floor for example).
			for (AActor* Actor : MovingActors)
			{
				TransformActor(Actor, TransformChange);
			}
		}
	}

	// Save controller locations and rotations for next frame.
	SaveControllerTransforms();
}

FTransform AInverseWorldPawn::CalculateTransformChange()
{
	if (!LeftController || !RightController)
	{
		return FTransform::Identity;
	}

	FTransform TransformChange = FTransform::Identity;

	// Get current controller locations.
	const FVector LeftControllerLocation = LeftController->GetControllerLocation();
	const FVector RightControllerLocation = RightController->GetControllerLocation();

	// Get current controller rotations.
	const FRotator LeftControllerRotation = LeftController->GetControllerRotation();
	const FRotator RightControllerRotation = RightController->GetControllerRotation();

	// Calculate TransformChange from controller movements.
	if (LeftController->IsMovingWorld() && RightController->IsMovingWorld())
	{
		// Both grips -> rotate or scale

		// Calculate differences between controllers for this and last frame.
		FVector PreviousDifference = LastLeftControllerLocation - LastRightControllerLocation;
		FVector CurrentDifference = LeftControllerLocation - RightControllerLocation;

		// Compensate for changing world scale.
		CurrentDifference *= LastWorldToMeters;
		PreviousDifference *= GetWorld()->GetWorldSettings()->WorldToMeters;

		// Get how much distance changed.
		const float DistanceMultiplier = CurrentDifference.Size() / PreviousDifference.Size();
		// Get a nice number to compare to rotation magnitude - absolute percentage length change
		const float AbsDistanceDifferencePercent = FMath::Abs(DistanceMultiplier - 1) * 100;

		// Get rotators from difference vectors.
		const FRotator PreviousRotator = PreviousDifference.ToOrientationRotator();
		const FRotator CurrentRotator = CurrentDifference.ToOrientationRotator();

		// Get rotation change from last tick.
		FRotator RotationDifference = CurrentRotator - PreviousRotator;

		// We only want to rotate along Z axis (Yaw)
		RotationDifference.Roll = 0;
		RotationDifference.Pitch = 0;

		// Compare if we want to scale more than rotate. Favor rotation.
		if (AbsDistanceDifferencePercent >= FMath::Abs(RotationDifference.Yaw * 2))
		{
			// More length difference -> scale
			TransformChange.SetScale3D(FVector(DistanceMultiplier));
		}
		else
		{
			// More rotation difference -> rotate
			TransformChange.SetRotation(RotationDifference.Quaternion());
		}
	}
	else if (LeftController->IsMovingWorld())
	{
		// Translate by LeftController location difference.
		const FVector Translation = (LeftControllerLocation - LastLeftControllerLocation) * TranslationSpeed;

		// Rotate by LeftController rotation difference.
		FRotator Rotation = LeftControllerRotation - LastLeftControllerRotation;
		Rotation.Roll = 0;
		Rotation.Pitch = 0;

		// Calculate scale compensation factor for translation.
		const float ScaleCompensationFactor = FMath::IsNearlyEqual(GetWorld()->GetWorldSettings()->WorldToMeters, 0.f)
												  ? 1.f
												  : (100 / GetWorld()->GetWorldSettings()->WorldToMeters);

		// Compare if we want to rotate more than rotate.
		if (Translation.Size() * ScaleCompensationFactor >= FMath::Abs(Rotation.Yaw))
		{
			TransformChange.SetTranslation(Translation);
		}
		else
		{
			TransformChange.SetRotation(Rotation.Quaternion());
		}
	}
	else
	{
		// Translate by RightController location difference.
		const FVector Translation = (RightControllerLocation - LastRightControllerLocation) * TranslationSpeed;

		// Rotate by RightController rotation difference.
		FRotator Rotation = RightControllerRotation - LastRightControllerRotation;
		Rotation.Roll = 0;
		Rotation.Pitch = 0;

		// Calculate scale compensation factor for translation.
		const float ScaleCompensationFactor = FMath::IsNearlyEqual(GetWorld()->GetWorldSettings()->WorldToMeters, 0.f)
												  ? 1.f
												  : (100 / GetWorld()->GetWorldSettings()->WorldToMeters);

		// Compare if we want to translate more than rotate.
		if (Translation.Size() * ScaleCompensationFactor >= FMath::Abs(Rotation.Yaw))
		{
			TransformChange.SetTranslation(Translation);
		}
		else
		{
			TransformChange.SetRotation(Rotation.Quaternion());
		}
	}

	return TransformChange;
}

void AInverseWorldPawn::SaveControllerTransforms()
{
	LastLeftControllerLocation = LeftController->GetControllerLocation();
	LastRightControllerLocation = RightController->GetControllerLocation();

	LastLeftControllerRotation = LeftController->GetControllerRotation();
	LastRightControllerRotation = RightController->GetControllerRotation();
}

void AInverseWorldPawn::SaveWorldToMeters()
{
	LastWorldToMeters = GetWorld()->GetWorldSettings()->WorldToMeters;
}

void AInverseWorldPawn::ResetTransform()
{
	// Get yaw of pawn before resetting.
	float PreviousYaw = GetActorRotation().Yaw;

	// Reset everything to default transforms
	for (TTuple<AActor*, FTransform> ActorTransformPair : MovingActorDefaultTransforms)
	{
		ActorTransformPair.Key->SetActorTransform(ActorTransformPair.Value);
	}
	// Compensate laser pointer for world scale change.
	// Reset controller mesh scales.
	LeftController->ResetMeshScale();
	RightController->ResetMeshScale();

	// And set world scale back to default
	GetWorldSettings()->WorldToMeters = 100.0f;
	CurrentScale = 1.0f;

	// Get current look rotation.
	FVector PlayerLocation;
	FRotator PlayerRotation;

	if (!GEngine)
	{
		return;
	}
	GEngine->GetFirstLocalPlayerController(GetWorld())->GetPlayerViewPoint(PlayerLocation, PlayerRotation);

	// Zero out non-yaw rotations.
	PlayerRotation.Roll = PlayerRotation.Pitch = 0;
	// And set new yaw to old yaw - current look yaw.
	PlayerRotation.Yaw = PreviousYaw - PlayerRotation.Yaw;
	// Set the actor rotation so we're facing straight towards the pivot.
	SetActorRotation(PlayerRotation);
	// Set translation so that the HMD camera is exactly above at the pawn spawn point.
	FVector PositionOffset = VRCamera->GetComponentLocation() - GetActorLocation();
	PositionOffset.Z = 0;
	AddActorWorldOffset(-PositionOffset);
}

#if !UE_BUILD_SHIPPING
#pragma optimize("", on)
#endif
