Screenspace position from worldspace position

#include "NScreenSpace.h"
#include "NUtilDefs.h"
#include "NGeometry.h"

#include "NBaseGame.h"
#include "NFont.h"

NScreenSpaceOutput NScreenSpace::GetScreenSpacePosition(const NVec3& Position, NCamera* pCamera, const NScreenSpaceParams& Params)
{
	opt_assert(pCamera);

	// Solving the edge-of-screen HUD indicator problem (maybe reusable in Banon!):

	// 0. We may wish to define a subsection of the screen for title safety within which indicators are clamped.
	// This could be implemented as a scalar on the tangent of the camera's FOV for considering elements to be "off-screen."

	// 1. If an element is "off-screen" (whether for real or as determined by a title safety region, etc.),
	// it should be denoted by an angle, not a position in screen space. This is to prevent bugs when, e.g.,
	// lerping between positions on the opposite sides of the screen.

	// 2. Elements in front of the camera are projected into screen space and clamped. An angle can then be found with an atan2().

	// 3. Elements behind the camera are probably calculated as an angle relative to...the facing angle?
	// The "FPS forward" vector? Looking up/down is where this gets doubly complicated.

	// 4. Once I have an angle, I project its normalized vector onto the edge of the region. Is this elliptical or circular?







	NVec3 FlatForward = pCamera->getFwdFPS();
	NVec3 FlatUp = pCamera->getUpFPS();
	NVec3 FlatLeft = pCamera->getLeftFPS();

	NVec3 PositionProjectedOntoFlatForwardLeftPlane = Position - Projection(Position - pCamera->getPos(), FlatUp);
	PositionProjectedOntoFlatForwardLeftPlane;

	NPlane FlatForwardPlane(FlatForward, pCamera->getPos());
	NPlane FlatUpPlane(FlatUp, pCamera->getPos());
	float FlatForwardDot = Dot(FlatForward, Normalize(PositionProjectedOntoFlatForwardLeftPlane - pCamera->getPos()));
	float FlatLeftDot = Dot(FlatLeft, Normalize(PositionProjectedOntoFlatForwardLeftPlane - pCamera->getPos()));

	float FlatForwardAngle = acos(Saturate(FlatForwardDot));

	float ToLeft = 0.0f;
	float ToRight = 0.0f;

	if (FlatLeftDot >= 0.0f)
	{
		// It's on our left side, so FlatForwardAngle is the angle it is to our left.
		ToLeft = (HALF_PI - FlatForwardAngle);
		ToRight = (HALF_PI + FlatForwardAngle);
	}
	else
	{
		// It's on our right side, so FlatForwardAngle is the angle it is to our right.
		ToLeft = (HALF_PI + FlatForwardAngle);
		ToRight = (HALF_PI - FlatForwardAngle);
	}

	NMat4 ViewMat1 = CreateViewMatrix(pCamera->getPos(), FlatForward, FlatLeft, FlatUp);
	NMat4 ViewMat2 = pCamera->getViewMatrix();

	ViewMat1, ViewMat2;

	NMat4 RotLeftMat = CreateRotationMatrix(FlatUp, ToLeft);
	NMat4 RotRightMat = CreateRotationMatrix(FlatUp, -ToRight);

	NVec3 LeftPos = (RotLeftMat * (Position - pCamera->getPos()));// + pCamera->getPos();
	NVec3 RightPos = (RotRightMat * (Position - pCamera->getPos()));// + pCamera->getPos();

	NVec3 ProjLeftPos = LeftPos - Projection(LeftPos, FlatForward);
	NVec3 ProjRightPos = RightPos - Projection(RightPos, FlatForward);

	ProjLeftPos, ProjRightPos;

	// Okay, these are wrong. I DON'T actually want these to be the point projected onto the camera plane;
	// I want them to be the point projected right at the edge of the title safety region.
	// Then I also don't need the lerp between the edge of that region and the camera plane;
	// as soon as it's outside that region, I lerp around the bottom of the screen.
	// Actually, no, that would break for things just ABOVE the title safety region. That's bad.
	// Hrm. Maybe this is okay for now?
	// Or maybe I just need to care about left/right edges of the region.
	// There's also an issue when looking some distance up or down and then MOVING past a point of interest until it is behind the plane rather than TURNING past.
	ProjLeftPos = (/*CreateTranslationMatrix(-pCamera->getPos()) **/ CreateRotationMatrix(FlatForward, FlatLeft, FlatUp)) * ProjLeftPos;
	ProjRightPos = (/*CreateTranslationMatrix(-pCamera->getPos()) **/ CreateRotationMatrix(FlatForward, FlatLeft, FlatUp)) * ProjRightPos;

	float LeftAngle = atan2(ProjLeftPos.y, ProjLeftPos.z);
	float RightAngle = atan2(ProjRightPos.y, ProjRightPos.z);

	opt_assert(LeftAngle >= 0.0f && LeftAngle <= PI);
	opt_assert(RightAngle <= 0.0f && RightAngle >= -PI);

	LeftAngle, RightAngle;




	// These are in degrees.
	float VFOV = pCamera->getFOV();
	float HFOV = HorizontalFOV(VFOV, pCamera->getAspectRatio());
	// Let's take the diagonal FOV and use that as the starting point for where we lerp to the camera plane angles.
	float HFOVRad = HFOV * DEG2RAD;
	float VFOVRad = VFOV * DEG2RAD;
	float TanHalfHFOV = tan(HFOVRad * 0.5f);
	float TanHalfVFOV = tan(VFOVRad * 0.5f);
	TanHalfHFOV *= Params.TitleSafetyWidth;
	TanHalfVFOV *= Params.TitleSafetyHeight;
	float TanHalfDFOV = sqrt(((TanHalfHFOV)*(TanHalfHFOV))+((TanHalfVFOV)*(TanHalfVFOV)));
	float HalfDFOVRad = atan(TanHalfDFOV);// * 2.0f;
	float HalfDFOV = HalfDFOVRad * RAD2DEG;
	HalfDFOV;

	float LerpStartCos = cos(HalfDFOVRad);
	float LerpEndCos = 0.0f;

	float Interpolant = Dot(pCamera->getForward(), Normalize(Position - pCamera->getPos()));

	Interpolant = Saturate(Normalize(Interpolant, LerpStartCos, LerpEndCos));

	// Now we use this Interpolant to lerp between the angle calculated from the camera-projected position and the angle on the left or right side as appropriate.
	Interpolant;








	NScreenSpaceOutput RetVal;

	float DotProduct = Dot(Position - pCamera->getPos(), pCamera->getForward());
	if (DotProduct > 0.0f)
	{
		// It's in front of the camera. We should be able to use its screen-space-projected position directly.
		NVec3 Projected = pCamera->project(Position);
		Projected;

		ANCHOR;

		if (Abs(Projected.x) < Params.TitleSafetyWidth &&
			Abs(Projected.y) < Params.TitleSafetyHeight)
		{
			// It's within the title safety region.
			// We can show an icon directly over the point of interest.
			ANCHOR;

			RetVal.PositionClassification = ESSP_WithinTitleSafetyRegion;

			RetVal.Position = Projected;
			RetVal.AngleToPosition = 0.0f;
		}
		else
		{
			// It's not. We should treat it as "off-screen" regardless of position,
			// and we need to be sure this aligns with any behind-the-plane behavior.
			ANCHOR;

			RetVal.PositionClassification = ESSP_BetweenTitleSafetyRegionAndCameraPlane;

			// Here is where we use Interpolant.
			// But first we need to calculate the angle of the projected point.
			float ProjAngle = atan2(-Projected.x, Projected.y);

			// TODO: Make sure we take the nearest direction here.
			// We should just need to know left/right side.

			opt_assert(ProjAngle >= -PI && ProjAngle <= +PI);
			float LerpedAngle = 0.0f;
			if (ProjAngle >= 0.0f)
			{
				LerpedAngle = Lerp(ProjAngle, LeftAngle, Interpolant);
			}
			else
			{
				LerpedAngle = Lerp(ProjAngle, RightAngle, Interpolant);
			}

			RetVal.Position = NVec2(-sin(LerpedAngle), cos(LerpedAngle));
			RetVal.Position = RetVal.Position / Max(Abs(RetVal.Position.x) / Params.TitleSafetyWidth, Abs(RetVal.Position.y) / Params.TitleSafetyHeight);

			RetVal.AngleToPosition = LerpedAngle;
		}
	}
	else
	{
		// It's behind the camera. Do something else.
		ANCHOR;

		RetVal.PositionClassification = ESSP_BehindCameraPlane;

		// Lerp from LeftAngle to RightAngle based on FPS facing.
		float LeftDot = Dot(FlatLeft, Normalize(Position - pCamera->getPos()));
		while (RightAngle < LeftAngle)
		{
			RightAngle += TAU;
		}
		float LerpedAngle = Lerp(LeftAngle, RightAngle, Saturate(Normalize(LeftDot, +1.0f, -1.0f)));
		RetVal.Position = NVec2(-sin(LerpedAngle), cos(LerpedAngle));
		RetVal.Position = RetVal.Position / Max(Abs(RetVal.Position.x) / Params.TitleSafetyWidth, Abs(RetVal.Position.y) / Params.TitleSafetyHeight);

		while (LerpedAngle < -PI) {LerpedAngle += TAU;}
		while (LerpedAngle > +PI) {LerpedAngle -= TAU;}

		RetVal.AngleToPosition = LerpedAngle;
	}

	//ANCHOR;
	//NFont* TheFont = NBaseGame::GetInstance()->GetDefaultInGameFont();
	//float dx = 0.0f;
	//float dy = 0.0f;
	//TheFont->MeasureText_Legacy("X",1,dx,dy);
	//TheFont->DrawText_Legacy("X",1,
	//	Lerp(0.0f, NBaseGame::GetInstance()->SCREEN_WIDTH / ((float)NBaseGame::GetInstance()->SCREEN_HEIGHT / 1080.0f)/*(float)NBaseGame::GetInstance()->SCREEN_WIDTH*/, Normalize(RetVal.Position.x, -1.0f, +1.0f)) - (dx*0.5f),
	//	Lerp(0.0f, 1080.0f/*(float)NBaseGame::GetInstance()->SCREEN_HEIGHT*/, Normalize(RetVal.Position.y, +1.0f, -1.0f)) - (dy*0.5f));

	return RetVal;
}