// Fill out your copyright notice in the Description page of Project Settings.


#include "AdventureMap.h"
#include "HexTile.h"
#include "StarFog.h"
#include "AdventurePlayerController.h"
#include "Kismet/GameplayStatics.h"
#include "Algo/Reverse.h"
#include "Util/IndexPriorityQueue.h"
#include <tgmath.h>

// Sets default values
AAdventureMap::AAdventureMap()
{
	FHexVector NBs[] = { E, SSE, SSW, W, NNW, NNE };
	UnitVectors.Append(NBs, UE_ARRAY_COUNT(NBs));
	FHexVector DNBs[] = { N, ENE, ESE, S, WSW, WNW };
	DiagonalUnitVectors.Append(DNBs, UE_ARRAY_COUNT(DNBs));
}

// Called when the game starts or when spawned
void AAdventureMap::BeginPlay()
{
	Super::BeginPlay();
	World = GetWorld();

	if (IsValid(BaseTileClass)) { 
		MakeGrid();
	}
	for (auto& Tile : Grid) {
		AStarFog* Fog = World->SpawnActor<AStarFog>(BaseFogClass, Tile->GetActorTransform());
		Fog->CoveredHex = Tile;
		Tile->CoveringFog = Fog;
	}
}

// Called once on Begin Play
void AAdventureMap::MakeGrid() 
{	
	FVector NextHexAt = FVector();
	float HexWidth = sqrt(3) * TileSize;
	int QOffset = 0;

	for (int r = 1; r <= GridSize; r++) {
		float XOffset = 0.f;
		if (r % 2 != 0)	{ if (r > 1) { QOffset--; } }
		else { XOffset = HexWidth / 2; }

		for (int q = 1; q <= GridSize; q++) {
			NextHexAt.X = XOffset + (HexWidth * q);
			NextHexAt.Y = TileSize * 1.5f * r;
			NextHexAt.Z = 0.f;
			FTransform SpawnTransform = FTransform(NextHexAt);
			AHexTile* Tile = World->SpawnActor<AHexTile>(BaseTileClass, SpawnTransform);
			Grid.Add(Tile);
			Tile->Q = q - 1 + QOffset;
			Tile->R = r - 1;
		}
	}
	for (auto& tile : Grid)	{
		tile->Index = GridIndex(tile->Q, tile->R);
	}
	bHexGridReady = true;
}

// Every Hex Tile's index within the Grid Array can be derived from its Axial Q and R coordinates
int32 AAdventureMap::GridIndex(int32 qAxial, int32 rAxial)
{
	/*
	*	The Q axis is (i.e. columns are) oriented diagonally.
	*	The Hex Grid has a rough square shape, hence the Q coordinates must be offset by -1 every other row.
	*/
	int32 column = qAxial + FMath::FloorToInt(rAxial / 2.);
	return (rAxial * GridSize) + column;
}

AHexTile* AAdventureMap::RandomHex()
{
	//debug
	GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, TEXT("Picking A Random Hex"));

	int32 RandHex = FMath::RandRange(0, GridSize*GridSize-1);
	return Grid[RandHex];
}

FHexVector AAdventureMap::AxialRound(float qf, float rf)
{
	float sf = -qf - rf;

	int32 q = round(qf);
	int32 r = round(rf);
	int32 s = round(sf);

	float q_diff = abs(q - qf);
	float r_diff = abs(r - rf);
	float s_diff = abs(s - sf);

	if (q_diff > r_diff && q_diff > s_diff)
		{ q = -r - s; }
	else if (r_diff > s_diff)
		{ r = -q - s; }
	else
		{ s = -q - r; }

	return FHexVector(q, r);
}

float AAdventureMap::Lerp(int32 a, int32 b, float t)
{
	return float(a + (b - a) * t);
}

TArray<AHexTile*> AAdventureMap::Neighbors(AHexTile* OfHex, bool bFreeOnly = false)
{
	TArray<AHexTile*> Results;
	for (FHexVector NeighborVector : UnitVectors) {
		int32 Index = GridIndex(OfHex->Q + NeighborVector.Q, OfHex->R + NeighborVector.R);
		if (Grid.IsValidIndex(Index)) {
			AHexTile* Hex = Grid[Index];
			if (bFreeOnly && !Hex->bFree) { continue; }
			if (Hex->Distance(OfHex) == 1) { Results.Add(Hex); }
		}
	}
	return Results;
}

TArray<AHexTile*> AAdventureMap::FreeDiagonals(AHexTile* OfHex)
{
	TArray<AHexTile*> Results;
	for (auto& V : DiagonalUnitVectors) {
		int32 I = GridIndex(OfHex->Q + V.Q, OfHex->R + V.R);
		if (!Grid.IsValidIndex(I)) { continue; }
		else {
			bool bReachable = true;
			for (AHexTile* PotentialBlock : Neighbors(OfHex)) {
				if (PotentialBlock->Distance(Grid[I]) != 1) { continue; }
				if (!PotentialBlock->bFree) {
					bReachable = false;
					break;
				}
			}
			if (bReachable) { Results.Add(Grid[I]); }
		}
	}
	return Results;
}

TSet<AHexTile*> AAdventureMap::BreadthFirstSearch(AHexTile* Start, int32 Radius)
{
	TSet<AHexTile*> Results;
	TArray<AHexTile*> Frontier;
	TSet<AHexTile*> Processed;
	Results.Add(Start);
	Frontier.Add(Start);

	while (!Frontier.IsEmpty()) {
		AHexTile* Current = Frontier[0];
		Processed.Add(Current);
		Frontier.Remove(Current);

		for (AHexTile* Neighbor : Neighbors(Current)) {
			if (Neighbor->Distance(Current) > 1)	{ continue; }
			if (Processed.Contains(Neighbor))		{ continue; }
			if (Neighbor->Distance(Start) > Radius)	{ continue; }

			Frontier.Add(Neighbor);
			Results.Add(Neighbor);
		}
	}
	return Results;
}



/* // Faulty implementation which uses an actual PriorityQueue
TArray<AHexTile*> AAdventureMap::FindPathAStar(AHexTile* Start, AHexTile* Goal, bool bDiags)
{

	TSet<AHexTile*> Processed;
	TSet<AHexTile*> ToSearch;
	TPriorityQueue<AHexTile*> Frontier;

	Start->GCost = 0;
	Frontier.Push(Start, .0f);
	ToSearch.Add(Start);

	while (!Frontier.IsEmpty()) {
		AHexTile* Current = Frontier.Pop();
		ToSearch.Remove(Current);

		if (Current == Goal) { break; }
		Processed.Add(Current);

		for (AHexTile* Neighbor : Neighbors(Current, true)) {
			if (Processed.Contains(Neighbor)) { continue; }
			bool bInToSearch = ToSearch.Contains(Start);

			int32 NewGCost = Current->GCost + Neighbor->MoveCost;

			if (!bInToSearch || NewGCost < Neighbor->GCost)	{
				Neighbor->GCost = NewGCost;
				Neighbor->CameFrom = Current;

				if (!bInToSearch) {
					Neighbor->HCost = Neighbor->Distance(Goal);
					Frontier.Push(Neighbor, NewGCost + Neighbor->HCost);
					ToSearch.Add(Neighbor);
				}
			}
		}
	}

	TArray<AHexTile*> Path;
	if (!IsValid(Goal->CameFrom)) { return Path; }
	AHexTile* iPathNode = Goal;
	while (iPathNode != Start) {
		Path.Emplace(iPathNode);
		iPathNode = iPathNode->CameFrom;
	}
	Algo::Reverse(Path);
	return Path;
}
*/

TArray<AHexTile*> AAdventureMap::FindPathAStar(AHexTile * Start, AHexTile * Goal, bool bDiags)
{
	TArray<AHexTile*> Frontier;
	TSet<AHexTile*> Processed;
	Frontier.Add(Start);
	while (!Frontier.IsEmpty()) {
		// Pop
		AHexTile* Candidate = Frontier[0];

		// Exit
		if (Candidate == Goal) { break; }

		// Find contender with an even lower F-cost
		for (AHexTile* Other : Frontier) {
			if (Other->FCost < Candidate->FCost 
				|| Other->FCost == Candidate->FCost && Other->HCost < Candidate->HCost) 
			{ Candidate = Other; }
		}

		Frontier.Remove(Candidate);
		Processed.Add(Candidate);
		// Expand frontier, make connections when appropriate
		for (AHexTile* Neighbor : Neighbors(Candidate, true)) {
			if (Processed.Contains(Neighbor)) { continue; }

			bool bInFrontier = Frontier.Contains(Neighbor);
			int32 NewGCost = Candidate->GCost + Neighbor->MoveCost;

			if (NewGCost < Neighbor->GCost || !bInFrontier) {
				Neighbor->GCost = NewGCost;
				Neighbor->HCost = Neighbor->Distance(Goal);
				Neighbor->FCost = Neighbor->GCost + Neighbor->HCost;
				Neighbor->CameFrom = Candidate; // chain
				if (!bInFrontier) {
					Frontier.Add(Neighbor);
				}
			}
		}
	}

	// Build and return path
	TArray<AHexTile*> Path;
	AHexTile* iPathNode = Goal;
	while (iPathNode != Start) {
		if (!IsValid(iPathNode->CameFrom) || !iPathNode->bFree) {
			Path.Empty();
			return Path; 
		}
		Path.Emplace(iPathNode);
		iPathNode = iPathNode->CameFrom;
	}
	Algo::Reverse(Path);
	return Path;
}

// very bro-sciency approach to pathfinding for diagonal Hex-movement
// DO NOT USE
TArray<AHexTile*> AAdventureMap::ShortcutAStar(TArray<AHexTile*> Path)
{
	TArray<AHexTile*> Shortcut;

	int32 Len = Path.Num();
	TArray<AHexTile*> WorkingSegment;
	AHexTile* CurrentHex = PCRef->CurrentHex;
	WorkingSegment.Add(CurrentHex);
	int32 h = 0;

	// create segments for each bend
	FHexVector PrevDir = FHexVector(Path[0], CurrentHex);
	FHexVector DirASave = PrevDir;
	FHexVector DirA;
	int32 HexesBeforeBend = 1;
	for (h; h < Len-1; h++) {
		WorkingSegment.Add(Path[h]);
		DirA = FHexVector(Path[h+1], Path[h]);
		if (DirA != PrevDir) { break; } // save Path[h] into Array of Bends
		HexesBeforeBend++;
		PrevDir = DirA;
	}
	PrevDir = DirA;
	FHexVector DirB;
	int32 HexesAfterBend = 0;
	for (h; h < Len - 1; h++) {
		WorkingSegment.Add(Path[h+1]);
		DirB = FHexVector(Path[h+1], Path[h]);
		if (DirB != PrevDir) { break; }
		HexesAfterBend++;
		PrevDir = DirB;
	}

	if (HexesAfterBend == 0) 
		{ return Path; }
	FHexVector UnitDiag = UnitDiagFromUnitNB(DirASave, DirB);
	AHexTile* Milestone = WorkingSegment.Last();
	int32 NumDiags = (HexesBeforeBend >= HexesAfterBend) ? HexesAfterBend : HexesBeforeBend;
	int32 NumTries = (HexesBeforeBend >= HexesAfterBend) ? HexesBeforeBend : HexesAfterBend;

	bool bDiagAdded = false;
	for (int i = 0; i < NumTries; i++) {
		if (NumDiags == 0) {
			Shortcut.Append(FindPathAStar(CurrentHex, Milestone, false));
			break;
		}
		if (DiagIsReachable(CurrentHex, UnitDiag)) {
			int32 CanIndex = GridIndex(CurrentHex->Q + UnitDiag.Q, CurrentHex->R + UnitDiag.R);
			if (Grid.IsValidIndex(CanIndex)) {
				AHexTile* Current = Grid[CanIndex];
				if (Current->bFree) {
					Shortcut.Add(Current);
					bDiagAdded = true;
					CurrentHex = Current;
					NumDiags--;
					continue;
		}	}	}
		if (!bDiagAdded && !DiagIsReachable(CurrentHex, UnitDiag)) {
			Shortcut.Add(CurrentHex);
			CurrentHex = WorkingSegment[i + 1];
			NumDiags--;
			continue;
		}
		if (bDiagAdded && !DiagIsReachable(CurrentHex, UnitDiag)) {
			Shortcut.Append(FindPathAStar(CurrentHex, Milestone, true));
			break;
		}
	}
	if (Milestone != Path.Last()) {	Shortcut.Append(FindPathAStar(Milestone, Path.Last(), true)); }

	UE_LOG(LogTemp, Warning, TEXT("Hexes before bend: %d"), HexesBeforeBend);
	UE_LOG(LogTemp, Warning, TEXT("Hexes after bend: %d"), HexesAfterBend);

	return Shortcut;
}

FHexVector AAdventureMap::UnitDiagFromUnitNB(FHexVector InVecA, FHexVector InVecB) {
	if (InVecA == NNW && InVecB == NNE||InVecB == NNW && InVecA == NNE)	{ return N; }
	if (InVecA == NNE && InVecB == E  ||InVecB == NNE && InVecA == E)	{ return ENE; }
	if (InVecA == E   && InVecB == SSE||InVecB == E   && InVecA == SSE)	{ return ESE; }
	if (InVecA == SSE && InVecB == SSW||InVecB == SSE && InVecA == SSW)	{ return S; }
	if (InVecA == SSW && InVecB == W  ||InVecB == SSW && InVecA == W)	{ return WSW; }
	if (InVecA == W   && InVecB == NNW||InVecB == W   && InVecA == NNW)	{ return WNW; }
	return FHexVector();
}

bool AAdventureMap::DiagIsReachable(AHexTile* InStart, FHexVector InDiagUnitVec) {
	FHexVector BlockA;
	FHexVector BlockB;
	if (InDiagUnitVec == N) { BlockA = NNW, BlockB = NNE; }
	if (InDiagUnitVec == ENE) { BlockA = NNE, BlockB = E; }
	if (InDiagUnitVec == ESE) { BlockA = E, BlockB = SSE; }
	if (InDiagUnitVec == S)	{ BlockA = SSE, BlockB = SSW; }
	if (InDiagUnitVec == WSW) { BlockA = SSW, BlockB = W; }
	if (InDiagUnitVec == WNW) { BlockA = W, BlockB = NNW; }
	int32 IndexA = GridIndex(InStart->Q + BlockA.Q, InStart->R + BlockA.R);
	int32 IndexB = GridIndex(InStart->Q + BlockB.Q, InStart->R + BlockB.R);
	if (!Grid.IsValidIndex(IndexA) || !Grid.IsValidIndex(IndexB)) { return false; }
	AHexTile* HexA = Grid[IndexA];
	AHexTile* HexB = Grid[IndexB];
	return (HexA->bFree && HexB->bFree);
}