#include "SingularitySharedBody.h"

#include "Components/SceneComponent.h"
#include "Components/StaticMeshComponent.h"
#include "Engine/World.h"
#include "Engine/StaticMesh.h"
#include "Materials/MaterialInterface.h"
#include "PhysicsEngine/PhysicsConstraintComponent.h"

namespace
{
const FName RoleNames[] = {
    TEXT("EyesHead"), TEXT("LeftArm"), TEXT("RightArm"),
    TEXT("TorsoBack"), TEXT("LeftLeg"), TEXT("RightLeg")};
}

ASingularitySharedBody::ASingularitySharedBody()
{
    PrimaryActorTick.bCanEverTick = true;

    BodyRoot = CreateDefaultSubobject<USceneComponent>(TEXT("BodyRoot"));
    RootComponent = BodyRoot;

    for (int32 Index = 0; Index < UE_ARRAY_COUNT(RoleNames); ++Index)
    {
        UStaticMeshComponent* Part = CreateDefaultSubobject<UStaticMeshComponent>(RoleNames[Index]);
        Part->SetupAttachment(BodyRoot);
        // Ranked presentation follows server snapshots, never local Chaos forces.
        Part->SetSimulatePhysics(false);
        const FVector Rest[] = {FVector(0,0,300), FVector(0,-100,200),
            FVector(0,100,200), FVector(0,0,200), FVector(0,-45,35), FVector(0,45,35)};
        Part->SetRelativeLocation(Rest[Index]);
        Part->SetEnableGravity(true);
        Part->SetCollisionProfileName(TEXT("PhysicsActor"));
        RoleBodies.Add(Part);
        CurrentInputs.Add(FSingularityRoleInput());
        InputAge.Add(1.0f);
        PreviousAction.Add(false);
    }

    const FName ConstraintNames[] = {
        TEXT("ConstraintTorsoHead"), TEXT("ConstraintTorsoLeftArm"),
        TEXT("ConstraintTorsoRightArm"), TEXT("ConstraintTorsoLeftLeg"),
        TEXT("ConstraintTorsoRightLeg"), TEXT("ConstraintLegs"),
        TEXT("ConstraintHeadLeftArm"), TEXT("ConstraintHeadRightArm")};
    for (const FName& Name : ConstraintNames)
    {
        UPhysicsConstraintComponent* Constraint = CreateDefaultSubobject<UPhysicsConstraintComponent>(Name);
        Constraint->SetupAttachment(BodyRoot);
        Constraint->SetDisableCollision(true);
        Constraints.Add(Constraint);
    }
}

void ASingularitySharedBody::BeginPlay()
{
    Super::BeginPlay();
    LoadImportedArt();
    ConfigureConstraints();
}

void ASingularitySharedBody::LoadImportedArt()
{
    for (int32 Index = 0; Index < RoleBodies.Num(); ++Index)
    {
        if (RoleMeshes.IsValidIndex(Index))
        {
            if (UStaticMesh* Mesh = RoleMeshes[Index].LoadSynchronous())
            {
                RoleBodies[Index]->SetStaticMesh(Mesh);
            }
        }
        if (UMaterialInterface* Material = TeamMaterial.LoadSynchronous())
        {
            for (int32 Slot = 0; Slot < RoleBodies[Index]->GetNumMaterials(); ++Slot)
            {
                const UMaterialInterface* Existing = RoleBodies[Index]->GetMaterial(Slot);
                if (Existing && (Existing->GetName().Contains(TEXT("TeamCoral")) ||
                    Existing->GetName().Contains(TEXT("TeamMint"))))
                    RoleBodies[Index]->SetMaterial(Slot, Material);
            }
        }
    }
}

void ASingularitySharedBody::ConfigureConstraints()
{
    if (RoleBodies.Num() != 6 || Constraints.Num() != 8)
    {
        return;
    }

    const int32 Pairs[][2] = {{3, 0}, {3, 1}, {3, 2}, {3, 4},
                              {3, 5}, {4, 5}, {0, 1}, {0, 2}};
    for (int32 Index = 0; Index < Constraints.Num(); ++Index)
    {
        const FVector A = RoleBodies[Pairs[Index][0]]->GetComponentLocation();
        const FVector B = RoleBodies[Pairs[Index][1]]->GetComponentLocation();
        Constraints[Index]->SetWorldLocation((A + B) * 0.5);
        Constraints[Index]->SetLinearXLimit(LCM_Locked, 0);
        Constraints[Index]->SetLinearYLimit(LCM_Locked, 0);
        Constraints[Index]->SetLinearZLimit(LCM_Locked, 0);
        Constraints[Index]->SetAngularSwing1Limit(ACM_Limited, 35);
        Constraints[Index]->SetAngularSwing2Limit(ACM_Limited, 35);
        Constraints[Index]->SetAngularTwistLimit(ACM_Limited, 25);
        Constraints[Index]->SetConstrainedComponents(
            RoleBodies[Pairs[Index][0]], NAME_None,
            RoleBodies[Pairs[Index][1]], NAME_None);
    }
}

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

    for (int32 Index = 0; Index < RoleBodies.Num(); ++Index)
    {
        const FSingularityRoleInput& Input = CurrentInputs[Index];
        const ESingularityBodyRole Role = static_cast<ESingularityBodyRole>(Index);
        InputAge[Index] += DeltaSeconds;
        if (!RoleBodies[Index]->IsSimulatingPhysics()) continue;
        if (InputAge[Index] > 0.5f) { PreviousAction[Index] = false; continue; }
        const FVector Force(Input.Z * RoleForce, Input.X * RoleForce, 0.0f);
        RoleBodies[Index]->AddForce(Force);
        RoleBodies[Index]->AddTorqueInRadians(FVector(0.0f, Input.X * RoleForce * 0.08f, 0.0f));

        FHitResult Ground;
        const FVector Foot = RoleBodies[Index]->GetComponentLocation();
        FCollisionQueryParams Query;
        Query.AddIgnoredActor(this);
        const bool bGrounded = GetWorld()->LineTraceSingleByChannel(
            Ground, Foot, Foot - FVector(0,0,55), ECC_Visibility, Query);
        if (Input.bAction && !PreviousAction[Index] && IsLeg(Role) && bGrounded)
        {
            RoleBodies[Index]->AddImpulse(FVector(0.0f, 0.0f, LegHopImpulse));
        }

        PreviousAction[Index] = Input.bAction;
    }
}

void ASingularitySharedBody::ApplyRoleInput(ESingularityBodyRole Role, const FSingularityRoleInput& Input)
{
    const int32 Index = RoleIndex(Role);
    if (CurrentInputs.IsValidIndex(Index))
    {
        if (!FMath::IsFinite(Input.X) || !FMath::IsFinite(Input.Z)) return;
        InputAge[Index] = 0.0f;
        CurrentInputs[Index] = Input;
        CurrentInputs[Index].X = FMath::Clamp(CurrentInputs[Index].X, -1.0f, 1.0f);
        CurrentInputs[Index].Z = FMath::Clamp(CurrentInputs[Index].Z, -1.0f, 1.0f);
    }
}

void ASingularitySharedBody::ApplyRoleInputPacket(const FSingularityRoleInput& Input)
{
    ApplyRoleInput(Input.Role, Input);
}

void ASingularitySharedBody::ApplyAuthoritativeSnapshot(const FSingularityTeamSnapshot& Snapshot)
{
    if (Snapshot.Nodes.Num() != 6) return;
    for (const FVector& Node : Snapshot.Nodes) if (Node.ContainsNaN()) return;
    // SpacetimeDB's shared solver stores torso/head before the role list used
    // by the Unreal actor. Keep this translation at the transport boundary.
    const int32 NodeToRole[] = {3, 0, 1, 2, 4, 5};
    for (int32 NodeIndex = 0; NodeIndex < Snapshot.Nodes.Num() && NodeIndex < UE_ARRAY_COUNT(NodeToRole); ++NodeIndex)
    {
        const int32 Role = NodeToRole[NodeIndex];
        const FVector Node = Snapshot.Nodes[NodeIndex];
        RoleBodies[Role]->SetSimulatePhysics(false);
        RoleBodies[Role]->SetWorldLocation(GetActorTransform().TransformPosition(
            FVector(Node.Z, Node.X, Node.Y) * 100.0));
    }
}

UStaticMeshComponent* ASingularitySharedBody::GetRoleComponent(ESingularityBodyRole Role) const
{
    const int32 Index = RoleIndex(Role);
    return RoleBodies.IsValidIndex(Index) ? RoleBodies[Index] : nullptr;
}

int32 ASingularitySharedBody::RoleIndex(ESingularityBodyRole Role)
{
    return static_cast<int32>(Role);
}

bool ASingularitySharedBody::IsLeg(ESingularityBodyRole Role)
{
    return Role == ESingularityBodyRole::LeftLeg || Role == ESingularityBodyRole::RightLeg;
}