MTD_FoeState_MovingAroundBlockade.cpp

#include "Gameplay/Foes/AI/States/MTD_FoeState_MovingAroundBlockade.h"

#include "Character/MTD_CharacterWithAbilities.h"
#include "Components/ShapeComponent.h"
#include "FiniteStateMachine/FiniteStateMachine.h"
#include "Gameplay/Foes/AI/Components/MTD_PathFollowingComponent.h"
#include "Gameplay/Foes/AI/States/MTD_FoeState_Seeking.h"
#include "Gameplay/Foes/MTD_FoeController.h"
#include "Kismet/KismetMathLibrary.h"
#include "NativeGameplayTags.h"
#include "Physics/MTD_CollisionChannels.h"
#include "Utilities/MTD_GameplayStatics.h"
#include "Utilities/MTD_Log.h"

DEFINE_LOG_CATEGORY(MTD_LogFoe_MoveAroundBlockade);
UE_DEFINE_GAMEPLAY_TAG(TAG_StateMachine_Label_MoveAround, "StateMachine.Label.MoveAround");

#define DRAW_DEBUG
#define DRAW_TIME 2.f
#define DRAW_THICKNESS 1.f
#ifdef DRAW_DEBUG
    #define DRAW_DEBUG_TYPE EDrawDebugTrace::ForDuration
#elif
    #define DRAW_DEBUG_TYPE EDrawDebugTrace::None
#endif

UMTD_FoeState_MovingAroundBlockade::UMTD_FoeState_MovingAroundBlockade()
{
    StateDataClass = UMTD_FoeStateData_MovingAroundBlockade::StaticClass();

    InitialLabelsToRegister.Add(TAG_StateMachine_Label_MoveAround,
        FLabelSignature::CreateUObject(this, &ThisClass::MoveAroundBlockade));
}

bool UMTD_FoeState_MovingAroundBlockade::MoveAroundBlockade(UFiniteStateMachine* StateMachineContext,
    AActor* BlockadeActor, float BlockadeWidth, FVector HitNormal, bool bSkipBlockingCheck /*false*/)
{
    if (!IsValid(BlockadeActor))
    {
        return false;
    }

    auto* StateData = StateMachineContext->GetStateDataChecked<UMTD_FoeStateData_MovingAroundBlockade>(StaticClass());
    const AActor* Avatar = StateMachineContext->GetAvatar();
    if (!IsValid(Avatar))
    {
        return false;
    }

    const FVector Velocity = Avatar->GetVelocity();
    const float SpeedSq = Velocity.SquaredLength();
    const float MoveAroundSomethingMaximumSpeedSq = FMath::Square(StateData->MoveAroundSomethingMaximumSpeed);

    // Don't try to move around blockade if we're moving at a high enough speed still
    if (SpeedSq > MoveAroundSomethingMaximumSpeedSq)
    {
        return false;
    }

    // Cheap hack to prevent enemies from moving around each other again as they all try to move around each other
    if (StateData->bIgnoreMoveAroundBlockade)
    {
        return false;
    }

    // if (!bReachedInitialTarget)
    // {
    //  return false;
    // }

    // bInvicible?

    // Only move-around-blockade when seeking or moving around a blockade
    if (!StateMachineContext->IsInState(UMTD_FoeState_Seeking::StaticClass()) &&
        !StateMachineContext->IsInState(UMTD_FoeState_MovingAroundBlockade::StaticClass()))
    {
        return false;
    }

    if (UMTD_GameplayStatics::TimeSince(StateMachineContext, StateData->LastBlockadeCheckTime) < 0.5f)
    {
        MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Target [%s] is irrelevant as "
            "we've tried to move around very recently [%f].", *BlockadeActor->GetName(),
            UMTD_GameplayStatics::TimeSince(StateMachineContext, StateData->LastBlockadeCheckTime));
        return false;
    }

    const auto* GlobalStateData = StateMachineContext->GetStateDataChecked<UMTD_GlobalFoeStateData>(
        UMTD_FoeState_Global::StaticClass());

    if (GlobalStateData->Target == BlockadeActor)
    {
        MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Target [%s] is the same as the "
            "current target.", *BlockadeActor->GetName());
        return false;
    }

    if (BlockadeActor == StateData->LastMovingAroundBlockadeActor &&
        UMTD_GameplayStatics::TimeSince(StateMachineContext, StateData->LastMovingAroundBlockadeTime) < 2.6f)
    {
        MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Target [%s] is a very recent "
            "[%f] try.", *BlockadeActor->GetName(),
            UMTD_GameplayStatics::TimeSince(StateMachineContext, StateData->LastMovingAroundBlockadeTime));
        return false;
    }

    StateData->LastBlockadeCheckTime = UMTD_GameplayStatics::GetTime(StateMachineContext);

    // If the blockade itself doesn't specify how wide it is, guess based on its collision component
    if (BlockadeWidth == 0.f)
    {
        const auto* CollisionComponent = BlockadeActor->FindComponentByTag<UShapeComponent>(CollisionComponentTag);
        if (!IsValid(CollisionComponent))
        {
            MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Target [%s] does not have "
                "a shape component with tag [%s].", *BlockadeActor->GetName(), *CollisionComponentTag.ToString());
            return false;
        }

        BlockadeWidth = CollisionComponent->Bounds.SphereRadius;
    }

    const FRotator AvatarRotator = Avatar->GetActorRotation();
    const FVector AvatarRotationVector = AvatarRotator.Vector();
    const float Dot = FVector::DotProduct(HitNormal.GetSafeNormal2D(), AvatarRotationVector.GetSafeNormal2D());
    if (Dot > 0.f)
    {
        MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Target [%s] is behind us.",
            *BlockadeActor->GetName());
        return false;
    }

    const FVector OurPosition = Avatar->GetActorLocation();
    const float AngleBetween = 180.f - UKismetMathLibrary::DegAcos(Dot);
    const FVector RotatedHitNormal = HitNormal.RotateAngleAxis(AngleBetween, FVector::UpVector);
    const FRotator HitNormalRotator = RotatedHitNormal.Rotation();

    const UWorld* World = StateMachineContext->GetWorld();
    check(IsValid(World));

    // Only run the blockade-validity if specified to (do by default). If we don't want to do the checks, we probably
    // just want to switch directions
    if (!bSkipBlockingCheck)
    {
        // We are going to see if this blockade actually is in the way of our destination

        // Store our destination in a temporary variable for checking
        const FVector TargetDesinationPosition = FVector::ZeroVector; // @todo MoveToTarget state
        FVector Delta = FVector::ZeroVector;
        if (!TargetDesinationPosition.IsZero())
        {
            Delta = TargetDesinationPosition - OurPosition;
        }
        else if (GlobalStateData->Target.IsValid())
        {
            Delta = GlobalStateData->Target->GetActorLocation() - OurPosition;
        }

        FVector TraceThroughBlockadePosition = FVector::ZeroVector;
        if (!Delta.IsZero())
        {
            TraceThroughBlockadePosition = Delta.GetSafeNormal() * BlockadeWidth * 1.5f + OurPosition;
        }

        bool bFoundAnyTraceCheck = false;

        // Just in case we may not actually have a target destination
        if (!TraceThroughBlockadePosition.IsZero())
        {
#ifdef DRAW_DEBUG
            DrawDebugSphere(World, TraceThroughBlockadePosition, 30, 16, FColor::Red, false, DRAW_TIME, 0, DRAW_THICKNESS);
            DrawDebugSphere(World, OurPosition, 30, 16, FColor::Green, false, DRAW_TIME, 0, DRAW_THICKNESS);
            DrawDebugLine(World, OurPosition, TraceThroughBlockadePosition, FColor::White, false, DRAW_TIME, 0, DRAW_THICKNESS);
#endif

            const auto* CollisionSettings = GetDefault<UCollisionProfile>();
            const ETraceTypeQuery TraceChannel = CollisionSettings->ConvertToTraceType(ECC_Visibility);

            TArray<FHitResult> Hits;
            UKismetSystemLibrary::BoxTraceMulti(Avatar, TraceThroughBlockadePosition, OurPosition,
                FVector(BlockadeWidth, BlockadeWidth, 0) / 2.f * 0.9f, FRotator::ZeroRotator, TraceChannel, false, {},
                DRAW_DEBUG_TYPE, OUT Hits, true, FLinearColor::Red, FLinearColor::Green, DRAW_TIME);

            // World->LineTraceMultiByObjectType(Hits, TraceThroughBlockadePosition, OurPosition, ObjectQueryParams,
            //  CollisionQueryParams);

            // Check if the blockade is actually standing in the way of our target destination
            for (const FHitResult& Hit : Hits)
            {
                const AActor* HitActor = Hit.GetActor();
                MTDC_VVERBOSE(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Collided with blockade [%s] "
                    "Trace checked [%s].", *BlockadeActor->GetName(), *HitActor->GetName());

                if (HitActor == BlockadeActor)
                {
                    bFoundAnyTraceCheck = true;
                    break;
                }
            }
        }

        // If blockade does not appear to be in the way of our target destination, cancel moving around it
        // It probably was just a side-glancing collision and not point to navigate around that
        if (!bFoundAnyTraceCheck)
        {
            MTDC_LOG(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Ignoring... Collided with blockade [%s] but "
                "it wasn't in trace path towards our target.", *BlockadeActor->GetName());
            return false;
        }
    }

    const UShapeComponent* CollisionComponent = Avatar->FindComponentByTag<UShapeComponent>(CollisionComponentTag);
    if (!IsValid(CollisionComponent))
    {
        MTDC_WARN(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Cancelling... Our pawn [%s] lacks a collision "
            "component with tag [%s].", *Avatar->GetName(), *CollisionComponentTag.ToString());
        return false;
    }

    const float OurWidth = CollisionComponent->Bounds.SphereRadius;
    FVector Offset;

    // Start with a navigation offset that is sideways, so that we will be moving to the left or right of the blockade.
    // MovingAroundBlockadeSide stores our current side (left or right) so that all subsequent blockade movements
    // automatically keep going in the same direction
    Offset.Y = StateData->MovingAroundBlockadeSide * (BlockadeWidth + OurWidth);

    // Push the offset a little backwards so we tend to navigate around the blockade
    Offset.X = -50.f - OurWidth;

    const FVector BlockadePosition = BlockadeActor->GetActorLocation();
#ifdef DRAW_DEBUG
    DrawDebugLine(World, BlockadePosition, BlockadePosition + HitNormal * 150.f, FColor::Red, false, 2.f, 0, 1.f);
#endif

    // Calculate the final move-around point as the blockade's location, plus our offset transformed by the rotation
    // of the normal, so we effectively pick a side relative to our collision face's orientation
    StateData->MovingAroundBlockadePoint = BlockadePosition + HitNormalRotator.RotateVector(Offset);

    // Ensure the point we're attempting to navigate to is at our height
    StateData->MovingAroundBlockadePoint.Z = OurPosition.Z;

#ifdef DRAW_DEBUG
    DrawDebugSphere(World, StateData->MovingAroundBlockadePoint, 15.f, 6, FColor::Cyan, false, 2.f, 0, 1.f);
#endif

    // Check that the point we're going to try to navigate to around the blockade actually is a decent point --
    // that it has ground underneath it; if it's not a valid point, then switch moving side
    if (!CheckMoveAroundBlockadePoint(StateMachineContext, StateData->MovingAroundBlockadePoint))
    {
        // Store the new side so that subsequent blockade movements will also travel in it
        StateData->MovingAroundBlockadeSide *= -1.f;

        // Switch the side direction of the offset
        Offset.Y *= -1.f;

        // And transform that new point
        StateData->MovingAroundBlockadePoint = BlockadePosition + HitNormalRotator.RotateVector(Offset);

        // Ensure the point we're attempting to navigate to is at our height
        StateData->MovingAroundBlockadePoint.Z = OurPosition.Z;

        MTDC_LOG(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Failed going [%s] side, trying to go the other "
            "one...", StateData->MovingAroundBlockadeSide == 1.f ? TEXT("Right") : TEXT("Left"));

        // Check the new-direction position as well just to be safe -- if this fails, we're just not going to do anything
        // right now... hopefully an ally will bust through that blockade soon!
        if(!CheckMoveAroundBlockadePoint(StateMachineContext, StateData->MovingAroundBlockadePoint))
        {
            // If we're here, then there were no valid points around the blocakde, so...
            // This blockade is totally blocking us, so perhaps let's try to destroy it!
            // @todo AggroEntries.Remove(0,AggroEntries.Length);
            // @todo AddToAggro(DunDefTargetableInterface(blockadeActor),1.0);

            MTDC_LOG(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Failed final viability check.");
            return false;
        }
    }

    MTDC_LOG(MTD_LogFoe_MoveAroundBlockade, StateMachineContext, "Moving around blockade...");

    // Store a reference to the blockade actor we're moving around, so that we don't attempt to move around it again
    // while already moving around it
    StateData->LastMovingAroundBlockadeActor = BlockadeActor;

    // Find an unobstructed point somewhere near to our move-around point (sets it into the movearound point)
    // @todo    FindSpot(Pawn.CollisionComponent.Bounds.BoxExtent, MovingAroundBlockadePoint);

    StateData->LastBlockadeWidth = BlockadeWidth;
    StateData->LastBlockadeHitNormal = HitNormal;

    // Finally, we're ready to enter or restart the MovingAroundBlockade state and attempt to head to our
    // move-around point!
    if (!bSkipBlockingCheck)
    {
        // If we did a full check, then begin a completely new blockade movement, starting at the beginning of the
        // MovingAroundBlockade state code
        StateData->LastMovingAroundBlockadeTime = UMTD_GameplayStatics::GetTime(StateMachineContext);
        StateData->StartMovingAroundBlockadeLocation = OurPosition;

        // StateMachineContext->GotoState(StaticClass());
        StateMachineContext->GotoState(StaticClass(), TAG_StateMachine_Label_MoveAround);
    }
    else
    {
        // If we did not do a full check (and were just intending to switch directions due to a time-out),
        // then go to the MoveAround Label, which will not reset our time-out timers, so that we will still time-out
        // of the overall state if it takes too long
        StateMachineContext->GotoState(StaticClass(), TAG_StateMachine_Label_MoveAround);
    }

    return true;
}

bool UMTD_FoeState_MovingAroundBlockade::CheckMoveAroundBlockadePoint(const UFiniteStateMachine* StateMachineContext,
    const FVector& MoveToPoint)
{
    const AActor* Avatar = StateMachineContext->GetAvatar();
    check(IsValid(Avatar));

    const UShapeComponent* CollisionComponent = Avatar->FindComponentByTag<UShapeComponent>(
        CollisionComponentTag);
    check(IsValid(CollisionComponent));

    const FVector Offset = FVector::ZAxisVector * CollisionComponent->Bounds.SphereRadius * -2.f;

    FHitResult Hit;
    const bool bHasGround = Avatar->GetWorld()->LineTraceSingleByChannel(OUT Hit, MoveToPoint, MoveToPoint + Offset,
        ECC_Visibility);

    return bHasGround;
}

void UMTD_FoeState_MovingAroundBlockade::Begin(TSubclassOf<UMachineState> PreviousState)
{
    Super::Begin(PreviousState);

    if (!StateData.IsValid())
    {
        StateData = StateMachine->GetStateDataChecked<UMTD_FoeStateData_MovingAroundBlockade>(StaticClass());
    }

    auto* PathFolliwingComponent = Controller->GetComponentByClass<UMTD_PathFollowingComponent>();
    PathFolliwingComponent->AddObstacle(StateData->LastMovingAroundBlockadeActor.Get());
    PathFolliwingComponent->SetSteeringEnabled(true);

    FTimerManager& TimerManager = GetWorld()->GetTimerManager();
    TimerManager.SetTimer(CancelMoveAroundTimer, this, &ThisClass::CancelMoveAround, 4.f, false);
    TimerManager.SetTimer(TryOtherBlockadeDirectionTimer, this, &ThisClass::TryOtherBlockadeDirection, 0.6f, true);
}

void UMTD_FoeState_MovingAroundBlockade::Tick(float DeltaSeconds)
{
    Super::Tick(DeltaSeconds);

    // Check if we don't appear to be moving or have not moved 1.5 seconds after entering this state,
    // and if not, cancel the move around
    if (TimeSince(StateData->LastMovingAroundBlockadeTime) > 1.5f)
    {
        const FVector OurPosition = Character->GetActorLocation();
        const FVector DistanceVector = OurPosition - StateData->StartMovingAroundBlockadeLocation;
        const float DistanceSq = DistanceVector.SquaredLength();
        if (DistanceSq < 0.01f)
        {
            CancelMoveAround();
        }
        else
        {
            const FVector Velocity = Character->GetVelocity();
            const float SpeedSq = Velocity.SquaredLength();
            if (SpeedSq < 1.f)
            {
                CancelMoveAround();
            }
        }
    }
}

void UMTD_FoeState_MovingAroundBlockade::End(TSubclassOf<UMachineState> NewState)
{
    CancelMoveAround();

    auto* PathFolliwingComponent = Controller->GetComponentByClass<UMTD_PathFollowingComponent>();
    PathFolliwingComponent->RemoveObstacle(StateData->LastMovingAroundBlockadeActor.Get());
    PathFolliwingComponent->SetSteeringEnabled(false);

    Super::End(NewState);
}

void UMTD_FoeState_MovingAroundBlockade::MoveAroundBlockade(float DeltaSeconds)
{
    Controller->MoveToLocation(StateData->MovingAroundBlockadePoint, 50.f, false, false);
}

void UMTD_FoeState_MovingAroundBlockade::CancelMoveAround()
{
    MTD_VERBOSE(MTD_LogFoe_MoveAroundBlockade, "Cancel move around.");

    FTimerManager& TimerManager = GetWorld()->GetTimerManager();
    TimerManager.ClearTimer(CancelMoveAroundTimer);
    TimerManager.ClearTimer(TryOtherBlockadeDirectionTimer);

    StateMachine->GotoState(UMTD_FoeState_Seeking::StaticClass());
}

void UMTD_FoeState_MovingAroundBlockade::TryOtherBlockadeDirection()
{
    const FVector Velocity = Character->GetVelocity();
    const float SpeedSq = Velocity.SquaredLength();
    if (SpeedSq < 25.f * 25.f)
    {
        MTD_VERBOSE(MTD_LogFoe_MoveAroundBlockade, "Try other blockade direction.");

        StateData->MovingAroundBlockadeSide *= -1.f;
        AActor* BlockadeActor = StateData->LastMovingAroundBlockadeActor.Get();
        StateData->LastMovingAroundBlockadeActor.Reset();
        MoveAroundBlockade(StateMachine.Get(), BlockadeActor, StateData->LastBlockadeWidth,
            StateData->LastBlockadeHitNormal, true);
    }
}