// by RedPixel Studio


#include "EnemyState_MovingAroundBlockade.h"

#include "Components/ShapeComponent.h"
#include "FiniteStateMachine/FiniteStateMachine.h"
//Seekinng
#include "EnemyGlobalMachineState.h"
#include "EnemyState_Seeking.h"
#include "REnemyAIController.h"
#include "Kismet/KismetMathLibrary.h"
#include "NativeGameplayTags.h"
#include "GameFramework/Character.h"
#include "Kismet/GameplayStatics.h"
#include "Kismet/KismetSystemLibrary.h"
#include "Navigation/PathFollowingComponent.h"
//#include "Physics/MTD_CollisionChannels.h"
//#include "Utilities/MTD_GameplayStatics.h"
//#include "Utilities/MTD_Log.h"

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

UEnemyState_MovingAroundBlockade::UEnemyState_MovingAroundBlockade()
{
    StateDataClass = UEnemyStateData_MovingAroundBlockade::StaticClass();

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


float getTime(const UObject* ContextObject)
{
    const UWorld* World = GEngine->GetWorldFromContextObject(ContextObject, EGetWorldErrorMode::LogAndReturnNull);
    const float Time = World ? World->GetTimeSeconds() : 0.0;
    return Time;
}

float timeSince(const UObject* ContextObject, float Time)
{
    const float TimeSince = getTime(ContextObject) - Time;
    return TimeSince;
}

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

    auto* StateData = StateMachineContext->GetStateDataChecked<UEnemyStateData_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);


    if (SpeedSq > MoveAroundSomethingMaximumSpeedSq)
    {
        return false;
    }
    // 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(UEnemyState_Seeking::StaticClass()) &&
        !StateMachineContext->IsInState(UEnemyState_MovingAroundBlockade::StaticClass()))
    {
        return false;
    }

    if (timeSince(StateMachineContext, StateData->LastBlockadeCheckTime) < 0.5f)
    {

        return false;
    }

    const auto* GlobalStateData = StateMachineContext->GetStateDataChecked<UEnemyGlobalMachineStateData>(
        UGlobalMachineState::StaticClass());

    if (GlobalStateData->Target == BlockadeActor)
    {

        return false;
    }

    if (BlockadeActor == StateData->LastMovingAroundBlockadeActor &&
        timeSince(StateMachineContext, StateData->LastMovingAroundBlockadeTime) < 2.6f)
    {
        return false;
    }

    StateData->LastBlockadeCheckTime = getTime(StateMachineContext);

    //guess based on its collision component
    if (BlockadeWidth == 0.f)
    {
        const auto* CollisionComponent = BlockadeActor->FindComponentByTag<UShapeComponent>(UEnemyStateData_MovingAroundBlockade::CollisionComponentTag);
        if (!IsValid(CollisionComponent))
        {
            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)
    {
        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). Switches Directions
    if (!bSkipBlockingCheck)
    {
        // if this blockade actually is in the way of our destination


        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;

        // In case of Nn 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);


            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

        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>(UEnemyStateData_MovingAroundBlockade::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.

    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
    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;

        Offset.Y *= -1.f;

        StateData->MovingAroundBlockadePoint = BlockadePosition + HitNormalRotator.RotateVector(Offset);


        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"));*/


        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...");


    StateData->LastMovingAroundBlockadeActor = BlockadeActor;

    // @todo    FindSpot(Pawn.CollisionComponent.Bounds.BoxExtent, MovingAroundBlockadePoint);

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

    // Ready to enter or restart the MovingAroundBlockade state and attempt to head to our 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 = getTime(StateMachineContext);
        StateData->StartMovingAroundBlockadeLocation = OurPosition;

        // StateMachineContext->GotoState(StaticClass());
        StateMachineContext->GotoState(StaticClass(), TAG_StateMachine_Label_MoveAround);
    }
    else
    {

        StateMachineContext->GotoState(StaticClass(), TAG_StateMachine_Label_MoveAround);
    }

    return true;
}

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

    const UShapeComponent* CollisionComponent = Avatar->FindComponentByTag<UShapeComponent>(
        UEnemyStateData_MovingAroundBlockade::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 UEnemyState_MovingAroundBlockade::Begin(TSubclassOf<UMachineState> PreviousState)
{
    Super::Begin(PreviousState);

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

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

void UEnemyState_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, 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 UEnemyState_MovingAroundBlockade::End(TSubclassOf<UMachineState> NewState)
{
    CancelMoveAround();
    Super::End(NewState);
}

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

void UEnemyState_MovingAroundBlockade::CancelMoveAround()
{
    FTimerManager& TimerManager = GetWorld()->GetTimerManager();
    TimerManager.ClearTimer(CancelMoveAroundTimer);
    TimerManager.ClearTimer(TryOtherBlockadeDirectionTimer);

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

void UEnemyState_MovingAroundBlockade::TryOtherBlockadeDirection()
{
    const FVector Velocity = Character->GetVelocity();
    const float SpeedSq = Velocity.SquaredLength();
    if (SpeedSq < 25.f * 25.f)
    {


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