#include "HeadLocker.h"
#include "Engine/World.h"
#include "DrawDebugHelpers.h"
#include <smmintrin.h>

FHeadLocker& FHeadLocker::Get() {
    static FHeadLocker Singleton;
    return Singleton;
}

FHeadLocker::FHeadLocker() {
    PreDirs.SetNum(65536);
    for (int32 i = 0; i < 65536; ++i) {
        float Pitch = float((i >> 8) & 0xFF) * 180.f / 255.f - 90.f;
        float Yaw   = float(i & 0xFF) * 360.f / 255.f;
        PreDirs[i] = FRotator(Pitch, Yaw, 0).Vector();
    }
}

void FHeadLocker::UpdateCache(const FVector& CamLoc) {
    Cache.Reset();
    for (TActorIterator<APawn> It(GWorld); It; ++It) {
        if (It->IsEnemy()) {
            FVector Head = It->GetMesh()->GetSocketLocation("head");
            Cache.Add({Head, FVector::DistSquared(CamLoc, Head)});
        }
    }
}

int32 FHeadLocker::FastMinIndex() {
    int32 Num = Cache.Num();
    if (Num == 0) return INDEX_NONE;
    if (Num < 8) {
        int32 Best = 0;
        for (int32 i = 1; i < Num; ++i)
            if (Cache[i].DistSq < Cache[Best].DistSq) Best = i;
        return Best;
    }
    __m128 Min4 = _mm_set1_ps(Cache[0].DistSq);
    int32 Indices[8] = {0,1,2,3,4,5,6,7};
    for (int32 i = 8; i < Num; i += 8) {
        __m128 Cur = _mm_loadu_ps(&Cache[i].DistSq);
        __m128 Mask = _mm_cmplt_ps(Cur, Min4);
        Min4 = _mm_min_ps(Min4, Cur);
    }
    float Arr[4]; _mm_storeu_ps(Arr, Min4);
    return FMath::MinElement(Arr, 4) + (Indices[FMath::MinElement(Arr, 4)]);
}

bool FHeadLocker::TryLock(const FVector& CamLoc, const FRotator& CamRot, FVector& OutAim) {
    UpdateCache(CamLoc);
    int32 Idx = FastMinIndex();
    if (Idx == INDEX_NONE) return false;
    FVector Dir = (Cache[Idx].Loc - CamLoc).GetSafeNormal();
    OutAim = CamLoc + Dir * 9999.f;
    return true;
}

void FHeadLocker::Tick(float DeltaTime) {
}