#pragma once

namespace RLGSC {

	class TimeToReachCalculator {
	public:
		static float calculateTimeToReach(const PlayerData& player, const Vec& targetPos) {
			const auto& car = player;
			Vec carPos = car.phys.pos;
			Vec carVel = car.phys.vel;
			Vec toTargetVec = targetPos - carPos;
			Vec dirToTarget = toTargetVec.Normalized();
			float dist = toTargetVec.Length() - CommonValues::BALL_RADIUS;
			float vel = carVel.Dot(dirToTarget);
			float rotTime = calculateRotationTime(player, targetPos);
			float timeToSurface = calculateTimeToReachAnySurface(car);

			// Determine whether to use aerial acceleration
			float aerialTime = 0;
			float aerialDist = dist;
			float aerialVel = vel;
			while (aerialDist > DIST_REACH_THRESH && aerialTime < timeToSurface) {
				aerialVel += (AIR_ACCEL + BOOST_ACCEL_AIR) * STEP_INTERVAL;
				if (aerialVel > MAX_TOTAL_SPEED) aerialVel = MAX_TOTAL_SPEED;
				aerialDist -= aerialVel * STEP_INTERVAL;
				aerialTime += STEP_INTERVAL;
			}
			bool useAerial = aerialTime < timeToSurface;

			float t = 0;
			for (float curDist = dist, curVel = vel; curDist > DIST_REACH_THRESH; t += STEP_INTERVAL, curDist -= curVel * STEP_INTERVAL) {
				bool hasRotated = t >= rotTime;
				if (curVel < MAX_TOTAL_SPEED && t == 0) {
					// Flip force
					curVel += FLIP_FORCE;
				}
				if (useAerial) {
					// Aerial acceleration
					if (curVel < MAX_DRIVE_SPEED) {
						curVel += (AIR_ACCEL + BOOST_ACCEL_AIR * hasRotated) * STEP_INTERVAL;
					}
					else if (curVel < MAX_TOTAL_SPEED) {
						curVel += BOOST_ACCEL_AIR * hasRotated * STEP_INTERVAL;
					}
				}
				else {
					// Ground acceleration
					if (curVel < 0) {
						curVel += (BRAKE_ACCEL + BOOST_ACCEL_GROUND * hasRotated) * STEP_INTERVAL;
					}
					else if (curVel < MAX_DRIVE_SPEED) {
						curVel += (DRIVE_ACCEL + BOOST_ACCEL_GROUND * hasRotated) * STEP_INTERVAL;
					}
					else if (curVel < MAX_TOTAL_SPEED) {
						curVel += BOOST_ACCEL_GROUND * hasRotated * STEP_INTERVAL;
					}
				}

				if (curVel >= MAX_TOTAL_SPEED) {
					return t + (curDist / MAX_TOTAL_SPEED);
				}
			}
			return t;
		}

	private:
		// Constants
		static constexpr float SIDE_WALL_X = 4096.0f;
		static constexpr float CLOSE_DISTANCE = 10.0f;
		static constexpr float NEAR_WALL_DISTANCE = 4077.0f;
		static constexpr float Z_THRESHOLD = 256.0f;
		static constexpr float EPSILON = 1e-3f;
		static constexpr float DIST_REACH_THRESH = 80.0f;
		static constexpr float STEP_INTERVAL = 1.0f / 60.0f;
		static constexpr float FLIP_FORCE = 495.0f;
		static constexpr float AIR_ACCEL = 133.334f;
		static constexpr float BOOST_ACCEL_AIR = 2116.666f;
		static constexpr float MAX_TOTAL_SPEED = 2300.0f;
		static constexpr float BRAKE_ACCEL = 3500.0f;
		static constexpr float BOOST_ACCEL_GROUND = 1983.332f;
		static constexpr float DRIVE_ACCEL = 1600.0f;
		static constexpr float MAX_DRIVE_SPEED = 1410.0f;
		static constexpr float GRAVITY_Z = -650.0f;
		static constexpr float BACK_WALL_Y = 5120.0f;
		static constexpr float NEG_BACK_WALL_Y = -5120.0f;
		static constexpr float MAX_BACKWALL_Y = 5100.0f;
		static constexpr float BOOST_ACCEL_POS = 1000.0f;
		static constexpr float BOOST_ACCEL_NEG = -1000.0f;
		static constexpr float FLOOR_Z = 18.0f;
		static constexpr float BOOST_ACCEL_DOWN = -1000.0f;
		static constexpr float MAX_TIME = 10.0f;
		static constexpr float JUMP_IMPULSE = 291.667f;
		static constexpr float GROUND_Z_THRESHOLD = 50.0f;
		static constexpr float MAX_SIMULATION_TIME = 10.0f;

		static float calculateTimeToSideWalls(const PlayerData& player) {
			const Vec& pos = player.phys.pos;
			const Vec& vel = player.phys.vel;

			float minTime = std::numeric_limits<float>::infinity();

			if (std::abs(pos.x) >= NEAR_WALL_DISTANCE && std::abs(pos.x) <= SIDE_WALL_X) {
				return 0.0f;
			}

			if (vel.x > EPSILON) {
				float a = 0.5f * DRIVE_ACCEL;
				float b = vel.x;
				float c = pos.x - SIDE_WALL_X;

				float discriminant = b * b - 4.0f * a * c;

				if (discriminant >= 0.0f) {
					float sqrtD = std::sqrt(discriminant);
					float t1 = (-b + sqrtD) / (2.0f * a);
					float t2 = (-b - sqrtD) / (2.0f * a);

					if (t1 > EPSILON && t1 < minTime) {
						minTime = t1;
					}
					if (t2 > EPSILON && t2 < minTime) {
						minTime = t2;
					}
				}
			}

			if (vel.x < -EPSILON) {
				float a = 0.5f * (-DRIVE_ACCEL);
				float b = vel.x;
				float c = pos.x + SIDE_WALL_X;

				float discriminant = b * b - 4.0f * a * c;

				if (discriminant >= 0.0f) {
					float sqrtD = std::sqrt(discriminant);
					float t1 = (-b + sqrtD) / (2.0f * a);
					float t2 = (-b - sqrtD) / (2.0f * a);

					if (t1 > EPSILON && t1 < minTime) {
						minTime = t1;
					}
					if (t2 > EPSILON && t2 < minTime) {
						minTime = t2;
					}
				}
			}

			return minTime;
		}

		static float calculateTimeToBackWalls(const PlayerData& player) {
			const Vec& pos = player.phys.pos;
			const Vec& vel = player.phys.vel;

			float minTime = std::numeric_limits<float>::infinity();

			if (pos.y >= MAX_BACKWALL_Y || pos.y <= -MAX_BACKWALL_Y) {
				return 0.0f;
			}

			if (pos.y >= BACK_WALL_Y || pos.y <= NEG_BACK_WALL_Y) {
				return 0.0f;
			}

			if (vel.y > EPSILON) {
				float a = 0.5f * BOOST_ACCEL_POS;
				float b = vel.y;
				float c = pos.y - BACK_WALL_Y;

				float discriminant = b * b - 4.0f * a * c;

				if (discriminant >= 0.0f) {
					float sqrtD = std::sqrt(discriminant);
					float t1 = (-b + sqrtD) / (2.0f * a);
					float t2 = (-b - sqrtD) / (2.0f * a);

					if (t1 > EPSILON && t1 < minTime) {
						minTime = t1;
					}
					if (t2 > EPSILON && t2 < minTime) {
						minTime = t2;
					}
				}
			}

			if (vel.y < -EPSILON) {
				float a = 0.5f * BOOST_ACCEL_NEG;
				float b = vel.y;
				float c = pos.y - NEG_BACK_WALL_Y;

				float discriminant = b * b - 4.0f * a * c;

				if (discriminant >= 0.0f) {
					float sqrtD = std::sqrt(discriminant);
					float t1 = (-b + sqrtD) / (2.0f * a);
					float t2 = (-b - sqrtD) / (2.0f * a);

					if (t1 > EPSILON && t1 < minTime) {
						minTime = t1;
					}
					if (t2 > EPSILON && t2 < minTime) {
						minTime = t2;
					}
				}
			}

			return minTime;
		}

		static float calculateTimeToFloor(const PlayerData& player) {
			const Vec& pos = player.phys.pos;
			const Vec& vel = player.phys.vel;

			if (pos.z <= FLOOR_Z) {
				return 0.0f;
			}

			float totalAccel = GRAVITY_Z + BOOST_ACCEL_DOWN;

			float a = 0.5f * totalAccel;
			float b = vel.z;
			float c = pos.z - FLOOR_Z;

			float discriminant = b * b - 4.0f * a * c;

			if (discriminant < 0.0f) {
				return std::numeric_limits<float>::infinity();
			}

			float sqrtD = std::sqrt(discriminant);
			float t1 = (-b - sqrtD) / (2.0f * a);
			float t2 = (-b + sqrtD) / (2.0f * a);

			float timeToFloor = std::numeric_limits<float>::infinity();

			if (t1 > 0.0f) {
				timeToFloor = t1;
			}
			if (t2 > 0.0f && t2 < timeToFloor) {
				timeToFloor = t2;
			}

			return timeToFloor;
		};




		static float calculateTimeToReachAnySurface(const PlayerData& player) {
			// Calculate individual times to each surface
			float timeToSideWalls = calculateTimeToSideWalls(player);
			// RG_LOG(timeToSideWalls); // Uncomment if logging is needed

			float timeToBackWalls = calculateTimeToBackWalls(player);
			//RG_LOG(timeToBackWalls); // Uncomment if logging is needed

			float timeToFloor = calculateTimeToFloor(player);
			// RG_LOG(timeToFloor); // Uncomment if logging is needed

			// Assign identifier variables for clarity
			float minTimeBetweenBackWallsAndFloor = RS_MIN(timeToBackWalls, timeToFloor);

			// Now find the minimum time among side walls and the previously computed minimum
			float minTime = RS_MIN(timeToSideWalls, minTimeBetweenBackWallsAndFloor);

			return minTime;
		}

		static float calculateRotationTime(const PlayerData& player, const Vec& targetPos) {
			constexpr float MAX_ANG_VEL = 5.5f * 0.9f;

			Vec playerPos = player.phys.pos;
			Vec playerForward = player.phys.rotMat.forward;
			Vec dirToTarget = (targetPos - playerPos).Normalized();
			float dotProduct = RS_CLAMP(playerForward.Dot(dirToTarget), -1.0f, 1.0f);
			float angleDiff = std::acos(dotProduct);

			return angleDiff / MAX_ANG_VEL;
		}
	};

} make this whole into a LUT do what zealan explains and make the full code wit hall the codes and implmentations its O(1) for the minimum time to pos for all cars
or a car
i mean im not counting the number of cars as n cause its max 6 so
fredrik._. — Yesterday at 11:35 PM
Do you like precompile your time to reach?
Zealan — Yesterday at 11:47 PM
precompile??
do you mean precompute?
fredrik._. — Yesterday at 11:48 PM
Yeah lol mb not familiar with terms yet 
fredrik._. — Today at 1:18 AM
Erm🤓 for your time to reach lookup table to only be 500mb isnt it like really innacurate?
fredrik._. — Today at 1:28 AM
Simce after my calculations accounting for every possible, position, xy and z, and also all orientations and lets not forget the target pos the lookuptable will be prob many many terabytes, so im wondering eth youre doing to only get 500mb lmao
Zealan — Today at 1:29 AM
every possible xyz pos xyz vel xyz target pos angvel fraction and rotation diff is like
impossible
4 billion ^ 3 * 4 billion ^ 3 *  4 billion ^ 3 * 4 billion ^ 2
thats 4e105
thats more than there are atoms in universe i think
fredrik._. — Today at 1:30 AM
😂
Then how can i make it not be more atoms than in the universe😂
Zealan — Today at 1:31 AM
divide
decide how many subdivisions
but that will make accuracy ass, so you then need to interpolate between subdimensions to get most of that accuracy back 
then you're basically good
fredrik._. — Today at 1:32 AM
Ok ill try that thx
Zealan — Today at 1:34 AM
also i forgor how i made it smaller
cause initially it needed 500gb
oh yea right
fredrik._. — Today at 1:34 AM
💀
Zealan — Today at 1:34 AM
i only calculated delta pos
so we go from 30^9 which is an insane number
to 30^6
which is doable (700mb) 
and due to the way my system works i reduced it further
basically you only want a lookup table for that expensive loop you're doing
fredrik._. — Today at 1:35 AM
Delta pos too what? The target?
Zealan — Today at 1:35 AM
no to your mom
fredrik._. — Today at 1:35 AM
Not velocity etc
😂😭
Bro shooting strays
But yeah so i should do deltapos instead of all the velocity checks, is that what youre suggesting?
Zealan — Today at 1:36 AM
haha i dont even do 3d deltapos
i forgor i compressed it much further for higher accuracy
i do 2d deltapos
fredrik._. — Today at 1:37 AM
How do you eeven calculate aerial time then?
Zealan — Today at 1:37 AM
no no
not xy
or xz or something
fredrik._. — Today at 1:37 AM
Ah i see
Zealan — Today at 1:37 AM
the first dimension is the distance to the ball on xy 
the second is the z delta
fredrik._. — Today at 1:37 AM
Ok makes sense
But wont this systems accuracy be really bad? Compared to the normal one?
Zealan — Today at 1:38 AM
i compute time to surface not using the lookup table cause it would be too many dimensions
Zealan — Today at 1:38 AM
nah i compared them side by side
its pretty damn close
fredrik._. — Today at 1:38 AM
Damn
fredrik._. — Today at 1:39 AM
Too many dimentions?
Zealan — Today at 1:40 AM
that is what i said
fredrik._. — Today at 1:40 AM
Oh holy shit, i just figured out yeah looking at delta pos you can easily figure out the players velocity toot he target lmao
Thats soo fucking smart😂
Zealan — Today at 1:41 AM
yeah my thing is just
[xy dist][z dist][vel to target (1d float)][rot time][surface time] 
5 dimensions
fredrik._. — Today at 1:42 AM
Vel to target 1d float? How would that work?
What if its heading upwards
Zealan — Today at 1:43 AM
bruh
do you know how vpb works
💀
its like that
fredrik._. — Today at 1:43 AM
Oh shit
True🫣
Forgor
fredrik._. — Today at 1:55 AM
With this i assume you account for all possible values of these 4 dimentions excluding time to surfsce
Zealan — Today at 1:56 AM
"all possible values" wtf
a float has 4 billion possible values
fredrik._. — Today at 1:57 AM
Ohhh….
Zealan — Today at 1:58 AM
so with just 1 float
we need 4 billion floats in our lookup table
which is 16gb of ram