// Project Sword & Gun Copyright © 2021 Kevin Poretti


#include "Components/SNGStateMachineComponent.h"

static int32 DebugStateMachine = 0;
FAutoConsoleVariableRef CVARDebugStateMachine(TEXT("SNG.DebugStateMachine"),
                        DebugStateMachine, 
                        TEXT("Display debug text for state machine components"), 
                        ECVF_Cheat);

// Sets default values for this component's properties
USNGStateMachineComponent::USNGStateMachineComponent()
{
    // Set this component to be initialized when the game starts, and to be ticked every frame.  You can turn these features
    // off to improve performance if you don't need them.
    PrimaryComponentTick.bCanEverTick = true;

    // ...
}

bool USNGStateMachineComponent::PushEvent(FSNGEvent Event)
{
    EventQueue.Enqueue(Event);
    return true;
}

FName USNGStateMachineComponent::GetCurrentStateName()
{
    return CurrentState ? CurrentState->GetName() : NAME_None; 
}

FName USNGStateMachineComponent::GetPreviousStateName()
{
    return PreviousState ? PreviousState->GetName() : NAME_None;
}

USNGState* USNGStateMachineComponent::CreateAndInitializeState(TSubclassOf<USNGState> StateClass, USNGState* Parent)
{
    USNGState* State = NewObject<USNGState>(this, StateClass); //, NAME_None, RF_MarkAsRootSet);
    State->SetParent(Parent);
    State->SetOwner(GetOwner());
    AddStateToMap(State);
    UE_LOG(LogTemp, Log, TEXT("Adding state: %s"), *State->GetName().ToString());
    for (auto SubStateClass : State->GetSubStateClasses())
    {
        USNGState* CurrSubState = CreateAndInitializeState(SubStateClass, State);
        State->AddSubState(CurrSubState);
        UE_LOG(LogTemp, Log, TEXT("Adding substate %s of state %s"), *CurrSubState->GetName().ToString(), *State->GetName().ToString());
    }
    State->CreateState();
    return State;
}

void USNGStateMachineComponent::AddStateToMap(USNGState* State)
{
    if(!States.Contains(State->GetName()))
    {
        States.Add(State->GetName(), State);
    }
    else
    {
        UE_LOG(LogTemp, Warning, TEXT("SNGStateMachineComponent :: Trying to add a state to states map that already exists"));
    }
}

void USNGStateMachineComponent::TransitionToState(FName NextStateName, FSNGEvent& Event)
{
    if(States.Contains(NextStateName))
    {
        UE_LOG(LogTemp, Log, TEXT("Transitioning to state %s"), *NextStateName.ToString());
        USNGState* NextState = States[NextStateName];
        
        // find least common ancestor of current state and the next state
        // this is currently the naive approach where in the worst case we need to traverse
        // from two leaf nodes to the root node
        // there are some optimizations that can be made if this proves to be too slow
        // https://medium.com/@sahilawasthi9560460170/lowest-common-ancestor-of-n-ary-tree-107fa772a939
        uint32 CurrStateDepth = CurrentState->GetDepth();
        uint32 NextStateDepth = NextState->GetDepth();

        // let A be the deeper of the two states
        USNGState* A = CurrStateDepth > NextStateDepth ? CurrentState : NextState;
        USNGState* B = CurrStateDepth > NextStateDepth ? NextState : CurrentState;
        // traverse deeper state's parents until you find a state that is the same depth as the shallower state
        // if A and B are the same depth we don't need to do this
        if(A->GetDepth() != B->GetDepth())
        {
            USNGState* TempState = A->GetParent();
            while(TempState->GetDepth() != B->GetDepth())
            {
                TempState = TempState->GetParent();
            }
            A = TempState;
        }

        // continue to traverse A and B's parents until they are the same
        while(A->GetParent() != B->GetParent())
        {
            A = A->GetParent();
            B = B->GetParent();
        }
        USNGState* LCA = A->GetParent();

        // call exit state on current state and it's parents until you get to the LCA
        USNGState* CurrExitingState = CurrentState; 
        while(CurrExitingState != LCA)
        {
            CurrExitingState->ExitState();
            CurrExitingState = CurrExitingState->GetParent();
        }
        PreviousState = CurrentState;
        
        // then call enter state on each state going from LCA to node we are transitioning into
        // first we need to get all the states that are between the next state and the LCA
        TArray<USNGState*> StatesBetweenNextAndLCA;
        USNGState* Temp = NextState;
        StatesBetweenNextAndLCA.Add(NextState);
        while(Temp->GetParent() != LCA)
        {
            StatesBetweenNextAndLCA.Add(Temp->GetParent());
            Temp = Temp->GetParent();
        }
        // then in reverse order call EnterState
        for(int i = StatesBetweenNextAndLCA.Num() - 1; i >= 0; i--)
        {
            CurrentState = StatesBetweenNextAndLCA[i]->EnterState(Event);
        }
        //CurrentState = NextState;
        //PreviousState->ExitState();
        //CurrentState = NextState->EnterState(Event);
    }
    else
    {
        UE_LOG(LogTemp, Log,
            TEXT("Tried to transition to state %s from state %s but the state does not exist in the map"),
            *NextStateName.ToString(), *CurrentState->GetName().ToString());
    }
}

// Called when the game starts
void USNGStateMachineComponent::BeginPlay()
{
    Super::BeginPlay();

    if(RootStateClass)
    {
        USNGState* RootState = CreateAndInitializeState(RootStateClass, nullptr);
        FSNGEvent Event;
        Event.EventType = ESNGEventType::None;
        CurrentState = RootState->EnterState(Event);
    }

#ifdef UE_BUILD_DEBUG
    UE_LOG(LogTemp, Log, TEXT("List of states in state machine:"));
    for (auto Pair : States)
    {
        UE_LOG(LogTemp, Log, TEXT("%s"), *Pair.Key.ToString());
    }
#endif
}

// Called every frame
void USNGStateMachineComponent::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
{
    Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
    
    if(CurrentState)
    {
        if(DebugStateMachine)
        {
            FString DebugMsg = FString::Printf(TEXT("Current State: %s"), *CurrentState->GetName().ToString());
            GEngine->AddOnScreenDebugMessage(-1, 0.0f, FColor::Cyan, DebugMsg);
            DebugMsg = FString::Printf(TEXT("Time In State: %s"), *FString::SanitizeFloat(CurrentState->GetTimeInState()));
            GEngine->AddOnScreenDebugMessage(-1, 0.0f, FColor::Magenta, DebugMsg);
        }
        
        CurrentState->UpdateState(DeltaTime);

        FName NextStateName = NAME_None;
        FSNGEvent CurrEvent;
        CurrEvent.EventType = ESNGEventType::None;
        while(EventQueue.Dequeue(CurrEvent))
        {
            NextStateName = CurrentState->GetNextStateFromEvent(CurrEvent);
        }

        if(NextStateName != NAME_None &&
            (NextStateName != CurrentState->GetName() || CurrentState->CanTransitionToItself()))
        {
            TransitionToState(NextStateName, CurrEvent);
            return;
        }
        
        NextStateName = CurrentState->GetNextState();
        if(NextStateName != NAME_None &&
            (NextStateName != CurrentState->GetName() || CurrentState->CanTransitionToItself()))
        {
            TransitionToState(NextStateName, CurrEvent);
        }
    }
}

