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

#include "CustomerCrowdAvoidanceComponent.h"

#include "Components/CapsuleComponent.h"
#include "Faye/Components/NpcPawnMovement.h"
#include "GameFramework/Actor.h"

TArray<TWeakObjectPtr<UCustomerCrowdAvoidanceComponent>> UCustomerCrowdAvoidanceComponent::AllAgents;

UCustomerCrowdAvoidanceComponent::UCustomerCrowdAvoidanceComponent()
{
	PrimaryComponentTick.bCanEverTick = true;
}

void UCustomerCrowdAvoidanceComponent::BeginPlay()
{
	Super::BeginPlay();
	
	PlayerPawn = GetWorld()->GetFirstPlayerController()->GetPawn();
	NpcPawnMovement = GetOwner()->FindComponentByClass<UNpcPawnMovement>();
	CapsuleRadius = GetOwner()->FindComponentByClass<UCapsuleComponent>()->GetScaledCapsuleRadius();
	AllAgents.Add(this);
}

void UCustomerCrowdAvoidanceComponent::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
	AllAgents.Remove(this);
	Super::EndPlay(EndPlayReason);
}

void UCustomerCrowdAvoidanceComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
	
	if (!NpcPawnMovement) return;
	
	FVector SeparationForce = ComputeSeparation();
	SeparationForce = SeparationForce.GetClampedToMaxSize(MaxSeparationForce);
	NpcPawnMovement->AddInputVector(SeparationForce, true);
}

FVector UCustomerCrowdAvoidanceComponent::ComputeSeparation() const
{
	// Cache current position and velocity
	const FVector MyLocation = GetOwner()->GetActorLocation();
	const FVector MyVelocity = NpcPawnMovement? NpcPawnMovement->Velocity : FVector::ZeroVector;
	
	//Accumulate all avoidance forces for this frame
	FVector Force = FVector::ZeroVector;
	
	for (int32 i = AllAgents.Num() - 1; i >= 0; --i)
	{
		if (!AllAgents[i].IsValid())
		{
			AllAgents.RemoveAtSwap(i);
			continue;
		}
		
		UCustomerCrowdAvoidanceComponent* OtherComp = AllAgents[i].Get();
		if (!OtherComp || OtherComp == this) continue;
		
		//Compute distance and direction to the other pawn
		const FVector OtherLocation = OtherComp->GetOwner()->GetActorLocation();
		const FVector Delta = MyLocation - OtherLocation;
		const float Distance = Delta.Size();
		
		//Ignore if outside of avoidance radius or too close to normalize safely
		if (Distance > AvoidanceRadius || Distance < 1.f) continue;
		
		FVector ToOther = Delta.GetSafeNormal();
		
		//Are we moving towards each other
		FVector OtherVelocity = OtherComp->NpcPawnMovement? OtherComp->NpcPawnMovement->Velocity : FVector::ZeroVector;
		FVector RelativeVelocity = MyVelocity - OtherVelocity;
		
		//Positive value = we are closing distance to the other pawn
		float ClosingSpeed = FVector::DotProduct(RelativeVelocity, ToOther);
		
		const float Normalized = 1.f - (Distance - AvoidanceRadius);
		const float Strength = FMath::Pow(Normalized, 3.5f);
		if (ClosingSpeed > 0.f)
		{
			FVector SideStep = FVector::CrossProduct(ToOther, FVector::UpVector);
			SideStep.Normalize();
			Force += SideStep * Strength;
			
			if (bDebug)
			{
				DrawDebugDirectionalArrow(GetWorld(), MyLocation, MyLocation + ToOther * 100.f, 30.f, FColor::Blue, false, 0.f, 0, 2.f);
				DrawDebugDirectionalArrow(GetWorld(), MyLocation, MyLocation + SideStep * 100.f, 30.f, FColor::Yellow, false, 0.f, 0, 2.f);
			}
		}
		else
		{
			Force += ToOther * Strength * 0.3f;
			
			if (bDebug)
			{
				DrawDebugDirectionalArrow(GetWorld(), MyLocation, MyLocation + ToOther * 100.f, 30.f, FColor::Cyan, false, 0.f, 0, 2.f);
			}
		}
	}
	
	const FVector PlayerLocation = PlayerPawn->GetActorLocation();
	FVector Delta = PlayerLocation - MyLocation;
	float Distance = Delta.Size();
	
	if (Distance > 1.f && Distance < AvoidanceRadius)
	{
		FVector ToPlayer = Delta.GetSafeNormal();
		FVector SideStep = FVector::CrossProduct(ToPlayer, FVector::UpVector);
		const float Normalized = 1.f - (Distance - AvoidanceRadius);
		const float Strength = FMath::Pow(Normalized, 4.5f);
		
		Force += SideStep * Strength * SeparationStrength;
	}
	
	if (bDebug)
	{
		// Avoidance radius
		DrawDebugSphere(GetWorld(), MyLocation, AvoidanceRadius,24,FColor::Green,false,0.f,0,1.5f);

		// Final applied force
		DrawDebugDirectionalArrow(GetWorld(),MyLocation,MyLocation + Force * 50.f,40.f, FColor::Red,false,0.f,0,3.f);
	}

	return Force * SeparationStrength;
}

