// Oleg Petruny proprietary.


#include "CollisionSceneComponent.h"

#include "CharacterMovementComponentAsync.h"
#include "GameFramework/CheatManager.h"

#define PERF_MOVECOMPONENT_STATS 0
#define LOCTEXT_NAMESPACE "PrimitiveComponent"

CSV_DEFINE_CATEGORY(CollisionSceneComponent, false);

namespace CollisionSceneComponentStatics
{
    static const FText MobilityWarnText = LOCTEXT("InvalidMove", "move");
}

namespace PrimitiveComponentCVars
{
    int32 bAllowCachedOverlapsCVar = 1;
    static FAutoConsoleVariableRef CVarAllowCachedOverlaps(
        TEXT("p.AllowCachedOverlaps"),
        bAllowCachedOverlapsCVar,
        TEXT("Primitive Component physics\n")
        TEXT("0: disable cached overlaps, 1: enable (default)"),
        ECVF_Default);

    float InitialOverlapToleranceCVar = 0.0f;
    static FAutoConsoleVariableRef CVarInitialOverlapTolerance(
        TEXT("p.InitialOverlapTolerance"),
        InitialOverlapToleranceCVar,
        TEXT("Tolerance for initial overlapping test in PrimitiveComponent movement.\n")
        TEXT("Normals within this tolerance are ignored if moving out of the object.\n")
        TEXT("Dot product of movement direction and surface normal."),
        ECVF_Default);

    float HitDistanceToleranceCVar = 0.0f;
    static FAutoConsoleVariableRef CVarHitDistanceTolerance(
        TEXT("p.HitDistanceTolerance"),
        HitDistanceToleranceCVar,
        TEXT("Tolerance for hit distance for overlap test in PrimitiveComponent movement.\n")
        TEXT("Hits that are less than this distance are ignored."),
        ECVF_Default);

    int32 bEnableFastOverlapCheck = 1;
    static FAutoConsoleVariableRef CVarEnableFastOverlapCheck(TEXT("p.EnableFastOverlapCheck"), bEnableFastOverlapCheck, TEXT("Enable fast overlap check against sweep hits, avoiding UpdateOverlaps (for the swept component)."));
}

template<class AllocatorType>
FORCEINLINE_DEBUGGABLE int32 IndexOfOverlapFast(const TArray<FOverlapInfo, AllocatorType>& OverlapArray, const FOverlapInfo& SearchItem)
{
    return OverlapArray.IndexOfByPredicate(FFastOverlapInfoCompare(SearchItem));
}

template<class AllocatorType>
FORCEINLINE_DEBUGGABLE void AddUniqueOverlapFast(TArray<FOverlapInfo, AllocatorType>& OverlapArray, FOverlapInfo&& NewOverlap)
{
    if(IndexOfOverlapFast(OverlapArray, NewOverlap) == INDEX_NONE)
    {
        OverlapArray.Add(NewOverlap);
    }
}

bool UCollisionSceneComponent::MoveComponentImpl(const FVector& Delta, const FQuat& NewRotationQuat, bool bSweep, FHitResult* OutHit, EMoveComponentFlags MoveFlags, ETeleportType Teleport)
{
    // SCOPE_CYCLE_COUNTER(STAT_MoveComponentTime); // LINKER ERROR
    CSV_SCOPED_TIMING_STAT(CollisionSceneComponent, MoveComponentTime);

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) && PERF_MOVECOMPONENT_STATS
    FScopedMoveCompTimer MoveTimer(this, Delta);
#endif // !(UE_BUILD_SHIPPING || UE_BUILD_TEST) && PERF_MOVECOMPONENT_STATS

#if defined(PERF_SHOW_MOVECOMPONENT_TAKING_LONG_TIME) || LOOKING_FOR_PERF_ISSUES
    uint32 MoveCompTakingLongTime = 0;
    CLOCK_CYCLES(MoveCompTakingLongTime);
#endif

    // static things can move before they are registered (e.g. immediately after streaming), but not after.
    if(!IsValid(this) || CheckStaticMobilityAndWarn(CollisionSceneComponentStatics::MobilityWarnText))
    {
        if(OutHit)
        {
            OutHit->Init();
        }
        return false;
    }

    ConditionalUpdateComponentToWorld();

    // Set up
    const FVector TraceStart = GetComponentLocation();
    const FVector TraceEnd = TraceStart + Delta;
    float DeltaSizeSq = (TraceEnd - TraceStart).SizeSquared();				// Recalc here to account for precision loss of float addition
    const FQuat InitialRotationQuat = GetComponentTransform().GetRotation();

    // ComponentSweepMulti does nothing if moving < KINDA_SMALL_NUMBER in distance, so it's important to not try to sweep distances smaller than that. 
    const float MinMovementDistSq = (bSweep ? FMath::Square(4.f * UE_KINDA_SMALL_NUMBER) : 0.f);
    if(DeltaSizeSq <= MinMovementDistSq)
    {
        // Skip if no vector or rotation.
        if(NewRotationQuat.Equals(InitialRotationQuat, SCENECOMPONENT_QUAT_TOLERANCE))
        {
            // copy to optional output param
            if(OutHit)
            {
                OutHit->Init(TraceStart, TraceEnd);
            }
            return true;
        }
        DeltaSizeSq = 0.f;
    }

    const bool bSkipPhysicsMove = ((MoveFlags & MOVECOMP_SkipPhysicsMove) != MOVECOMP_NoFlags);

    // WARNING: HitResult is only partially initialized in some paths. All data is valid only if bFilledHitResult is true.
    FHitResult BlockingHit(NoInit);
    BlockingHit.bBlockingHit = false;
    BlockingHit.Time = 1.f;
    bool bFilledHitResult = false;
    bool bMoved = false;
    bool bIncludesOverlapsAtEnd = false;
    bool bRotationOnly = false;
    TInlineOverlapInfoArray PendingOverlaps;
    AActor* const Actor = GetOwner();

    if(!bSweep)
    {
        // not sweeping, just go directly to the new transform
        bMoved = InternalSetWorldLocationAndRotation(TraceEnd, NewRotationQuat, bSkipPhysicsMove, Teleport);
        bRotationOnly = (DeltaSizeSq == 0);
        bIncludesOverlapsAtEnd = bRotationOnly && (AreSymmetricRotations(InitialRotationQuat, NewRotationQuat, GetComponentScale())) && IsQueryCollisionEnabled();
    }
    else
    {
        TArray<FHitResult> Hits;
        FVector NewLocation = TraceStart;

        // Perform movement collision checking if needed for this actor.
        const bool bCollisionEnabled = IsQueryCollisionEnabled();
        UWorld* const MyWorld = GetWorld();
        if(MyWorld && bCollisionEnabled && (DeltaSizeSq > 0.f))
        {
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
            if(!IsRegistered() && !MyWorld->bIsTearingDown)
            {
                if(Actor)
                {
                    ensureMsgf(IsRegistered(), TEXT("%s MovedComponent %s not registered during sweep (IsValid %d)"), *Actor->GetName(), *GetName(), IsValid(Actor));
                }
                else
                { //-V523
                    ensureMsgf(IsRegistered(), TEXT("Non-actor MovedComponent %s not registered during sweep"), *GetFullName());
                }
            }
#endif

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) && PERF_MOVECOMPONENT_STATS
            MoveTimer.bDidLineCheck = true;
#endif 
            static const FName TraceTagName = TEXT("MoveComponent");
            const bool bForceGatherOverlaps = !ShouldCheckOverlapFlagToQueueOverlaps(*this);
            FComponentQueryParams Params(SCENE_QUERY_STAT(MoveComponent), Actor);
            FCollisionResponseParams ResponseParam;
            InitSweepCollisionParams(Params, ResponseParam);
            Params.bIgnoreTouches |= !(GetGenerateOverlapEvents() || bForceGatherOverlaps);
            Params.TraceTag = TraceTagName;

            bool const bHadBlockingHit = MyWorld->ComponentSweepMulti(Hits, this, TraceStart, TraceEnd, InitialRotationQuat, Params);

            if(Hits.Num() > 0)
            {
                const float DeltaSize = FMath::Sqrt(DeltaSizeSq);
                for(int32 HitIdx = 0; HitIdx < Hits.Num(); HitIdx++)
                {
                    FUpdatedComponentAsyncInput::PullBackHit(Hits[HitIdx], TraceStart, TraceEnd, DeltaSize);
                }
            }

            // If we had a valid blocking hit, store it.
            // If we are looking for overlaps, store those as well.
            int32 FirstNonInitialOverlapIdx = INDEX_NONE;
            if(bHadBlockingHit || (GetGenerateOverlapEvents() || bForceGatherOverlaps))
            {
                int32 BlockingHitIndex = INDEX_NONE;
                float BlockingHitNormalDotDelta = UE_BIG_NUMBER;
                for(int32 HitIdx = 0; HitIdx < Hits.Num(); HitIdx++)
                {
                    const FHitResult& TestHit = Hits[HitIdx];

                    if(TestHit.bBlockingHit)
                    {
                        if(!FUpdatedComponentAsyncInput::ShouldIgnoreHitResult(MyWorld, TestHit, Delta, Actor, MoveFlags) && !ShouldComponentIgnoreHitResult(TestHit, MoveFlags))
                        {
                            if(TestHit.bStartPenetrating)
                            {
                                // We may have multiple initial hits, and want to choose the one with the normal most opposed to our movement.
                                const float NormalDotDelta = (TestHit.ImpactNormal | Delta);
                                if(NormalDotDelta < BlockingHitNormalDotDelta)
                                {
                                    BlockingHitNormalDotDelta = NormalDotDelta;
                                    BlockingHitIndex = HitIdx;
                                }
                            }
                            else if(BlockingHitIndex == INDEX_NONE)
                            {
                                // First non-overlapping blocking hit should be used, if an overlapping hit was not.
                                // This should be the only non-overlapping blocking hit, and last in the results.
                                BlockingHitIndex = HitIdx;
                                break;
                            }
                        }
                    }
                    else if(GetGenerateOverlapEvents() || bForceGatherOverlaps)
                    {
                        UPrimitiveComponent* OverlapComponent = TestHit.Component.Get();
                        if(OverlapComponent && (OverlapComponent->GetGenerateOverlapEvents() || bForceGatherOverlaps))
                        {
                            if(!FUpdatedComponentAsyncInput::ShouldIgnoreOverlapResult(MyWorld, Actor, *this, TestHit.HitObjectHandle.FetchActor(), *OverlapComponent))
                            {
                                // don't process touch events after initial blocking hits
                                if(BlockingHitIndex >= 0 && TestHit.Time > Hits[BlockingHitIndex].Time)
                                {
                                    break;
                                }

                                if(FirstNonInitialOverlapIdx == INDEX_NONE && TestHit.Time > 0.f)
                                {
                                    // We are about to add the first non-initial overlap.
                                    FirstNonInitialOverlapIdx = PendingOverlaps.Num();
                                }

                                // cache touches
                                AddUniqueOverlapFast(PendingOverlaps, FOverlapInfo(TestHit));
                            }
                        }
                    }
                }

                // Update blocking hit, if there was a valid one.
                if(BlockingHitIndex >= 0)
                {
                    BlockingHit = Hits[BlockingHitIndex];
                    bFilledHitResult = true;
                }
            }

            // Update NewLocation based on the hit result
            if(!BlockingHit.bBlockingHit)
            {
                NewLocation = TraceEnd;
            }
            else
            {
                check(bFilledHitResult);
                NewLocation = TraceStart + (BlockingHit.Time * (TraceEnd - TraceStart));

                // Sanity check
                const FVector ToNewLocation = (NewLocation - TraceStart);
                if(ToNewLocation.SizeSquared() <= MinMovementDistSq)
                {
                    // We don't want really small movements to put us on or inside a surface.
                    NewLocation = TraceStart;
                    BlockingHit.Time = 0.f;

                    // Remove any pending overlaps after this point, we are not going as far as we swept.
                    if(FirstNonInitialOverlapIdx != INDEX_NONE)
                    {
                        PendingOverlaps.SetNum(FirstNonInitialOverlapIdx, EAllowShrinking::No);
                    }
                }
            }

            bIncludesOverlapsAtEnd = AreSymmetricRotations(InitialRotationQuat, NewRotationQuat, GetComponentScale());

#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST)
            if(UCheatManager::IsDebugCapsuleSweepPawnEnabled() && BlockingHit.bBlockingHit && !IsZeroExtent())
            {
                // this is sole debug purpose to find how capsule trace information was when hit 
                // to resolve stuck or improve our movement system - To turn this on, use DebugCapsuleSweepPawn
                APawn const* const ActorPawn = (Actor ? Cast<APawn>(Actor) : NULL);
                if(ActorPawn && ActorPawn->Controller && ActorPawn->Controller->IsLocalPlayerController())
                {
                    APlayerController const* const PC = CastChecked<APlayerController>(ActorPawn->Controller);
                    if(PC->CheatManager)
                    {
                        FVector CylExtent = ActorPawn->GetSimpleCollisionCylinderExtent() * FVector(1.001f, 1.001f, 1.0f);
                        FCollisionShape CapsuleShape = FCollisionShape::MakeCapsule(CylExtent);
                        PC->CheatManager->AddCapsuleSweepDebugInfo(TraceStart, TraceEnd, BlockingHit.ImpactPoint, BlockingHit.Normal, BlockingHit.ImpactNormal, BlockingHit.Location, CapsuleShape.GetCapsuleHalfHeight(), CapsuleShape.GetCapsuleRadius(), true, (BlockingHit.bStartPenetrating && BlockingHit.bBlockingHit) ? true : false);
                    }
                }
            }
#endif
        }
        else if(DeltaSizeSq > 0.f)
        {
            // apply move delta even if components has collisions disabled
            NewLocation += Delta;
            bIncludesOverlapsAtEnd = false;
        }
        else if(DeltaSizeSq == 0.f && bCollisionEnabled)
        {
            bIncludesOverlapsAtEnd = AreSymmetricRotations(InitialRotationQuat, NewRotationQuat, GetComponentScale());
            bRotationOnly = true;
        }

        // Update the location.  This will teleport any child components as well (not sweep).
        bMoved = InternalSetWorldLocationAndRotation(NewLocation, NewRotationQuat, bSkipPhysicsMove, Teleport);
    }

    // Handle overlap notifications.
    if(bMoved)
    {
        if(IsDeferringMovementUpdates())
        {
            // Defer UpdateOverlaps until the scoped move ends.
            FScopedMovementUpdate* ScopedUpdate = GetCurrentScopedMovement();
            if(bRotationOnly && bIncludesOverlapsAtEnd)
            {
                ScopedUpdate->KeepCurrentOverlapsAfterRotation(bSweep);
            }
            else
            {
                ScopedUpdate->AppendOverlapsAfterMove(PendingOverlaps, bSweep, bIncludesOverlapsAtEnd);
            }
        }
        else
        {
            if(bIncludesOverlapsAtEnd)
            {
                TInlineOverlapInfoArray OverlapsAtEndLocation;
                bool bHasEndOverlaps = false;
                if(bRotationOnly)
                {
                    bHasEndOverlaps = ConvertRotationOverlapsToCurrentOverlaps(OverlapsAtEndLocation, OverlappingComponents);
                }
                else
                {
                    bHasEndOverlaps = ConvertSweptOverlapsToCurrentOverlaps(OverlapsAtEndLocation, PendingOverlaps, 0, GetComponentLocation(), GetComponentQuat());
                }
                TOverlapArrayView PendingOverlapsView(PendingOverlaps);
                TOverlapArrayView OverlapsAtEndView(OverlapsAtEndLocation);
                UpdateOverlaps(&PendingOverlapsView, true, bHasEndOverlaps ? &OverlapsAtEndView : nullptr);
            }
            else
            {
                TOverlapArrayView PendingOverlapsView(PendingOverlaps);
                UpdateOverlaps(&PendingOverlapsView, true, nullptr);
            }
        }
    }

    // Handle blocking hit notifications. Avoid if pending kill (which could happen after overlaps).
    const bool bAllowHitDispatch = !BlockingHit.bStartPenetrating || !(MoveFlags & MOVECOMP_DisableBlockingOverlapDispatch);
    if(BlockingHit.bBlockingHit && bAllowHitDispatch && IsValid(this))
    {
        check(bFilledHitResult);
        if(IsDeferringMovementUpdates())
        {
            FScopedMovementUpdate* ScopedUpdate = GetCurrentScopedMovement();
            ScopedUpdate->AppendBlockingHitAfterMove(BlockingHit);
        }
        else
        {
            DispatchBlockingHit(*Actor, BlockingHit);
        }
    }

#if defined(PERF_SHOW_MOVECOMPONENT_TAKING_LONG_TIME) || LOOKING_FOR_PERF_ISSUES
    UNCLOCK_CYCLES(MoveCompTakingLongTime);
    const float MSec = FPlatformTime::ToMilliseconds(MoveCompTakingLongTime);
    if(MSec > PERF_SHOW_MOVECOMPONENT_TAKING_LONG_TIME_AMOUNT)
    {
        if(GetOwner())
        {
            UE_LOG(LogPrimitiveComponent, Log, TEXT("%10f executing MoveComponent for %s owned by %s"), MSec, *GetName(), *GetOwner()->GetFullName());
        }
        else
        {
            UE_LOG(LogPrimitiveComponent, Log, TEXT("%10f executing MoveComponent for %s"), MSec, *GetFullName());
        }
    }
#endif

    // copy to optional output param
    if(OutHit)
    {
        if(bFilledHitResult)
        {
            *OutHit = BlockingHit;
        }
        else
        {
            OutHit->Init(TraceStart, TraceEnd);
        }
    }

    // Return whether we moved at all.
    return bMoved;
}

void UCollisionSceneComponent::BeginPlay()
{
    for(auto& child : GetAttachChildren())
        if(auto primitive = Cast<UPrimitiveComponent>(child))
            targetCollisions.Add(primitive);
}

#undef LOCTEXT_NAMESPACE
