#include "UniStrokePoint.h"

#include "WizardingCentral/Logger/LogLumiCharacter.h"

FUniStrokePoint::FUniStrokePoint()
{
	this->X = 0.0f;
	this->Y = 0.0f;
}

FUniStrokePoint::FUniStrokePoint(const float& X, const float& Y)
{
	this->X = X;
	this->Y = Y;
}

float
FUniStrokePoint::Distance(const FUniStrokePoint& PointA,
                          const FUniStrokePoint& PointB)
{
	return FMath::Sqrt(
		// delta X
		FMath::Square(PointB.X - PointA.X) +
		/// delta Y
		FMath::Square(PointB.Y - PointA.Y)
	);
}

FUniStrokePoint
FUniStrokePoint::Centroid(const TArray<FUniStrokePoint>& Points)
{
	float SumX = 0.0f;
	float SumY = 0.0f;

	for (const FUniStrokePoint& Point : Points)
	{
		SumX += Point.X;
		SumY += Point.Y;
	}

	return FUniStrokePoint(
		SumX / Points.Num(),
		SumY / Points.Num()
	);
}

FUniStrokeRectangle
FUniStrokePoint::BoundingBox(const TArray<FUniStrokePoint>& Points)
{
	// Edge case
	if (Points.Num() == 0)
		return FUniStrokeRectangle();

	float MinX = Points[0].X;
	float MinY = Points[0].Y;
	float MaxX = Points[0].X;
	float MaxY = Points[0].Y;

	for (const FUniStrokePoint& Point : Points)
	{
		MinX = FMath::Min(MinX, Point.X);
		MinY = FMath::Min(MinY, Point.Y);
		MaxX = FMath::Max(MaxX, Point.X);
		MaxY = FMath::Max(MaxY, Point.Y);
	}

	return FUniStrokeRectangle(MinX, MinY, MaxX - MinX, MaxY - MinY);
}

// TODO: Proofread this function
TArray<float>
FUniStrokePoint::Vectorize(const TArray<FUniStrokePoint>& Points)
{
	float Sum = 0.0f;
	TArray<float> Vector;

	for (const FUniStrokePoint& Point : Points)
	{
		Vector.Add(Point.X);
		Vector.Add(Point.Y);
		Sum += FMath::Square(Point.X) + FMath::Square(Point.Y);
	}

	const float Magnitude = FMath::Sqrt(Sum);

	for (float& V : Vector)
		V /= Magnitude;

	return Vector;
}

FString FUniStrokePoint::ToString() const
{
	return FString::Printf(TEXT("X=%.3f Y=%.3f"), X, Y);
}

float
FUniStrokePoint::OptimalCosineDistance(const TArray<float>& VectorA,
                                       const TArray<float>& VectorB)
{
	float A = 0.0f;
	float B = 0.0f;

	for (int i = 0; i < VectorA.Num(); i += 2)
	{
		A += VectorA[i] * VectorB[i] + VectorA[i + 1] * VectorB[i + 1];
		B += VectorA[i] * VectorB[i + 1] - VectorA[i + 1] * VectorB[i];
	}

	const float Angle = FMath::Atan(B / A);

	return FMath::Acos(A * FMath::Cos(Angle) + B * FMath::Sin(Angle));
}

TArray<FUniStrokePoint>
FUniStrokePoint::From(const TArray<FVector2D>& Points)
{
	TArray<FUniStrokePoint> NewPoints;

	NewPoints.Reserve(Points.Num());
	Algo::Transform(
		Points,
		NewPoints,
		[](const FVector2D& Point)
		{
			return FUniStrokePoint(Point.X, Point.Y);
		}
	);

	return NewPoints;
}

void
FUniStrokePoint::Resample(TArray<FUniStrokePoint>& Points,
                          const int& Num)
{
	const float I = PathLength(Points) / (Num - 1);
	// D <- 0
	float D = 0.0f;
	// newPoints <- points[0]
	TArray<FUniStrokePoint> OldPoints = Points;
	Points.SetNum(1);

	// foreach point p[i] for i >= 1 in points do
	for (int i = 1; i < OldPoints.Num(); ++i)
	{
		// d <- Distance(p[i - 1], p[i]) 
		const float d = Distance(OldPoints[i - 1], OldPoints[i]);

		// if (D + d) >= I then
		if (D + d >= I)
		{
			// q.X <- p[i - 1].X + ((I - D) / d) * (p[i].X - p[i - 1].X)
			const float qx = OldPoints[i - 1].X + (I - D) / d * (OldPoints[i].X - OldPoints[i - 1].X);
			// q.Y <- p[i - 1].Y + ((I - D) / d) * (p[i].Y - p[i - 1].Y)
			const float qy = OldPoints[i - 1].Y + (I - D) / d * (OldPoints[i].Y - OldPoints[i - 1].Y);

			// Append(newPoints, q)
			FUniStrokePoint NewPoint = FUniStrokePoint(qx, qy);
			Points.Add(NewPoint);
			// Insert(points, i, q)
			OldPoints.Insert(NewPoint, i);

			// D <- 0
			D = 0.0f;
		}
		else
		{
			// D <- D + d
			D += d;
		}
	}

	// Edge case
	if (Points.Num() == Num - 1)
		Points.Add(OldPoints.Last());
}

float
FUniStrokePoint::PathLength(const TArray<FUniStrokePoint>& Points)
{
	// d <- 0
	float d = 0;

	// for i from 1 to |A| step 1 do
	for (auto i = 1; i < Points.Num(); ++i)
	{
		// d <- d + Distance(A[i - 1], A[i])
		d += Distance(Points[i - 1], Points[i]);
	}

	// return d
	return d;
}

void
FUniStrokePoint::RotateToZero(TArray<FUniStrokePoint>& Points)
{
	// c <- Centroid(points)
	FUniStrokePoint c = Centroid(Points);
	// theta <- Atan(c.Y - points[0].Y, c.X - points[0].X)
	const float Theta = FMath::Atan2(c.Y - Points[0].Y, c.X - Points[0].X);

	// newPoints <- RotateBy(points, -theta)
	RotateBy(Points, -Theta);
	// return newPoints 
}

void
FUniStrokePoint::RotateBy(TArray<FUniStrokePoint>& Points,
                          const float& Theta)
{
	TArray<FUniStrokePoint> OldPoints = Points;
	Points.Empty();

	// c <- Centroid(points)
	const FUniStrokePoint c = Centroid(OldPoints);

	const float SinTheta = FMath::Sin(Theta);
	const float CosTheta = FMath::Cos(Theta);

	// foreach point p in points do
	for (const FUniStrokePoint& p : OldPoints)
	{
		FUniStrokePoint q = FUniStrokePoint(
			// q.x <- (p.x - c.x) * Cos(theta) - (p.y - c.y) * Sin(theta) + c.x
			(p.X - c.X) * CosTheta - (p.Y - c.Y) * SinTheta + c.X,
			// q.y <- (p.x - c.x) * Sin(theta) + (p.y - c.y) * Cos(theta) + c.y
			(p.X - c.X) * SinTheta + (p.Y - c.Y) * CosTheta + c.Y
		);
		// Append(newPoints, q)
		Points.Add(q);
	}
	// return newPoints
}

void
FUniStrokePoint::ScaleToSquare(TArray<FUniStrokePoint>& Points,
                               const float& Size)
{
	TArray<FUniStrokePoint> OldPoints = Points;
	Points.Empty();

	// B <- BoundingRect(points)
	const FUniStrokeRectangle B = BoundingBox(OldPoints);

	// foreach point p in points do
	for (const FUniStrokePoint& p : OldPoints)
	{
		const FUniStrokePoint q = FUniStrokePoint(
			// q.x <- p.x * (size / B.width)
			p.X * (Size / B.Width),
			// q.y <- p.y * (size / B.height)
			p.Y * (Size / B.Height)
		);
		// Append(newPoints, q)
		Points.Add(q);
	}
}

void
FUniStrokePoint::TranslateTo(TArray<FUniStrokePoint>& Points,
                             const FUniStrokePoint& Point)
{
	TArray<FUniStrokePoint> OldPoints = Points;
	Points.Empty();

	// c <- Centroid(points)
	const FUniStrokePoint c = Centroid(OldPoints);

	// foreach point p in points do
	for (; const FUniStrokePoint& p : OldPoints)
	{
		const FUniStrokePoint q = FUniStrokePoint(
			// q.x <- p.x + point.x - c.x
			p.X + Point.X - c.X,
			// q.y <- p.y + point.y - c.y.
			p.Y + Point.Y - c.Y
		);
		// Append(newPoints, q)
		Points.Add(q);
	}
	// return newPoints 
}

void
FUniStrokePoint::TranslateToOrigin(TArray<FUniStrokePoint>& Points)
{
	TArray<FUniStrokePoint> OldPoints = Points;
	Points.Empty();

	// c <- Centroid(points)
	const FUniStrokePoint c = Centroid(OldPoints);

	// foreach point p in points do
	for (const FUniStrokePoint& p : OldPoints)
	{
		const FUniStrokePoint q = FUniStrokePoint(
			// q.x <- p.x - c.x
			p.X - c.X,
			// q.y <- p.y - c.y.
			p.Y - c.Y
		);
		// Append(newPoints, q)
		Points.Add(q);
	}
	// return newPoints 
}

float
FUniStrokePoint::DistanceAtBestAngle(const TArray<FUniStrokePoint>& Points,
                                     const TArray<FUniStrokePoint>& T,
                                     const float& ThetaFrom,
                                     const float& ThetaTo,
                                     const float& ThetaThreshold)
{
	float ThetaA = ThetaFrom;
	float ThetaB = ThetaTo;

	// x1 <- phi theta_a + (1 - phi) theta_b
	float x1 = Phi * ThetaA + (1 - Phi) * ThetaB;
	// f1 <- DistanceAtAngle(points, T, x1)
	float f1 = DistanceAtAngle(Points, T, x1);
	// x2 <- (1 - phi) theta_a + phi theta_b
	float x2 = (1 - Phi) * ThetaA + Phi * ThetaB;
	// f2 <- DistanceAtAngle(points, T, x2)
	float f2 = DistanceAtAngle(Points, T, x2);

	// while |theta_b - theta_a| > threshold do
	while (FMath::Abs(ThetaB - ThetaA) > ThetaThreshold)
	{
		// if f1 < f2 then
		if (f1 < f2)
		{
			// theta_b <- x2
			ThetaB = x2;
			// x2 <- x1
			x2 = x1;
			// f2 <- f1
			f2 = f1;
			// x1 <- phi theta_a + (1 - phi) theta_b
			x1 = Phi * ThetaA + (1 - Phi) * ThetaB;
			// f1 <- DistanceAtAngle(points, T, x1)
			f1 = DistanceAtAngle(Points, T, x1);
		}
		// else
		else
		{
			// theta_a <- x1
			ThetaA = x1;
			// x1 <- x2
			x1 = x2;
			// f1 <- f2
			f1 = f2;
			// x2 <- (1 - phi) theta_a + phi theta_b
			x2 = (1 - Phi) * ThetaA + Phi * ThetaB;
			// f2 <- DistanceAtAngle(points, T, x2)
			f2 = DistanceAtAngle(Points, T, x2);
		}
	}

	// return min(f1, f2)
	return FMath::Min(f1, f2);
}

float
FUniStrokePoint::DistanceAtAngle(const TArray<FUniStrokePoint>& Points,
                                 const TArray<FUniStrokePoint>& T,
                                 const float& Theta)
{
	// newPoints <- RotateBy(points, theta)
	TArray<FUniStrokePoint> NewPoints = Points;
	RotateBy(NewPoints, Theta);

	// d <- PathDistance(newPoints, T.points)
	// return d
	return PathDistance(NewPoints, T);
}

float
FUniStrokePoint::PathDistance(const TArray<FUniStrokePoint>& PathA,
                              const TArray<FUniStrokePoint>& PathB)
{
	// Edge case: Different number of points
	if (PathA.Num() != PathB.Num())
		return TNumericLimits<float>::Max();

	// d <- 0
	float d = 0;

	// for i from 0 to |A| step 1 do
	for (int i = 0; i < PathA.Num(); ++i)
	{
		// d <- d + Distance(A[i], B[i])
		d += Distance(PathA[i], PathB[i]);
	}

	// return d / |A|
	return d / PathA.Num();
}