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

		float t = 0;
		for (float curDist = dist, curVel = vel; curDist > DIST_REACH_THRESH; t += STEP_INTERVAL, curDist -= curVel * STEP_INTERVAL) {
			bool hasRotated = t >= rotTime;
			bool onSurface = t >= timeToSurface;

			if (curVel < MAX_TOTAL_SPEED && t == 0) {
				curVel += FLIP_FORCE;
			}

			if (onSurface) {
				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 / curVel);
			}
		}
		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 = 66.66667f;
	static constexpr float BOOST_ACCEL_AIR = 1058.33333f;
	static constexpr float MAX_TOTAL_SPEED = 2300.0f;
	static constexpr float BRAKE_ACCEL = 3500.0f;
	static constexpr float BOOST_ACCEL_GROUND = 991.66667f;
	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 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) {
		float timeToSideWalls = calculateTimeToSideWalls(player);
		float timeToBackWalls = calculateTimeToBackWalls(player);
		float timeToFloor = calculateTimeToFloor(player);

		float minTimeBetweenBackWallsAndFloor = RS_MIN(timeToBackWalls, timeToFloor);
		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 angleDiff = std::acos(RS_CLAMP(playerForward.Dot(dirToTarget), -1.0f, 1.0f));
		return angleDiff / MAX_ANG_VEL;
	}
};

 this has to be exactly tha same as this // Constants related to the arena dimensions
constexpr float SIDE_WALL_X = 4096.0f;
constexpr float CLOSE_DISTANCE = 10.0f;        // Distance threshold for proximity to the walls
constexpr float NEAR_WALL_DISTANCE = 4077.0f;  // Threshold for x close to the wall
constexpr float Z_THRESHOLD = 256.0f;          // Threshold for z axis proximity to the ground
constexpr float EPSILON = 1e-3f;               // Small value for floating point comparison

// Movement-related constants
constexpr float DIST_REACH_THRESH = 80.0f;
constexpr float STEP_INTERVAL = 1.0f / 60.0f;    // Time step (e.g., 60 FPS)
constexpr float FLIP_FORCE = 495.0f;
constexpr float AIR_ACCEL = 66.66667f;
constexpr float BOOST_ACCEL_AIR = 1058.33333f;
constexpr float MAX_TOTAL_SPEED = 2300.0f;
constexpr float BRAKE_ACCEL = 3500.0f;
constexpr float BOOST_ACCEL_GROUND = 991.66667f;
constexpr float DRIVE_ACCEL = 1600.0f;
constexpr float MAX_DRIVE_SPEED = 1410.0f;
constexpr float GRAVITY_Z = -650.0f; // Example gravity value, adjust as needed

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

    // Initialize minimum time to infinity
    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; // Player is very close to the side walls
    }

    // ** Calculate Time to Positive Side Wall (x = SIDE_WALL_X) **
    if (vel.x > EPSILON) { // Player is moving towards the positive side wall
        float a = 0.5f * DRIVE_ACCEL;   // Acceleration coefficient (positive)
        float b = vel.x;                // Initial velocity towards positive x
        float c = pos.x - SIDE_WALL_X;  // Initial position offset from positive side wall

        // Quadratic equation: a*t^2 + b*t + c = 0
        float discriminant = b * b - 4.0f * a * c;

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

            // Select the smallest positive time
            if (t1 > EPSILON && t1 < minTime) {
                minTime = t1;
            }
            if (t2 > EPSILON && t2 < minTime) {
                minTime = t2;
            }
        }
    }

    // ** Calculate Time to Negative Side Wall (x = -SIDE_WALL_X) **
    if (vel.x < -EPSILON) { // Player is moving towards the negative side wall
        float a = 0.5f * (-DRIVE_ACCEL);   // Acceleration coefficient (negative)
        float b = vel.x;                   // Initial velocity towards negative x
        float c = pos.x + SIDE_WALL_X;     // Initial position offset from negative side wall

        // Quadratic equation: a*t^2 + b*t + c = 0
        float discriminant = b * b - 4.0f * a * c;

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

            // Select the smallest positive time
            if (t1 > EPSILON && t1 < minTime) {
                minTime = t1;
            }
            if (t2 > EPSILON && t2 < minTime) {
                minTime = t2;
            }
        }
    }

    return minTime;
}


// Constants related to the arena dimensions
constexpr float BACK_WALL_Y = 5120.0f;
constexpr float NEG_BACK_WALL_Y = -5120.0f;     // Negative back wall at y = -5120.0f
constexpr float MAX_BACKWALL_Y = 5100.0f;       // New threshold as per user request
constexpr float BOOST_ACCEL_POS = 1000.0f;      // Boost acceleration towards positive back wall (units/s²)
constexpr float BOOST_ACCEL_NEG = -1000.0f;     // Boost acceleration towards negative back wall (units/s²)

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

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

    // ** New Condition: If y is >=5100 or <=-5100, set time to back wall as 0 **
    if (pos.y >= MAX_BACKWALL_Y || pos.y <= -MAX_BACKWALL_Y) {
        return 0.0f; // Immediate collision with back wall
    }

    // ** Existing Immediate Collision Checks **
    if (pos.y >= BACK_WALL_Y) {
        return 0.0f; // Immediate collision with positive back wall
    }

    if (pos.y <= NEG_BACK_WALL_Y) {
        return 0.0f; // Immediate collision with negative back wall
    }

    // ** Calculate Time to Positive Back Wall (y = BACK_WALL_Y) **
    if (vel.y > EPSILON) { // Player is moving towards the positive back wall
        float a = 0.5f * BOOST_ACCEL_POS;          // Acceleration coefficient (positive)
        float b = vel.y;                           // Initial velocity towards positive y
        float c = pos.y - BACK_WALL_Y;            // Initial position offset from positive back wall

        // Quadratic equation: a*t^2 + b*t + c = 0
        float discriminant = b * b - 4.0f * a * c;

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

            // Select the smallest positive time
            if (t1 > EPSILON && t1 < minTime) {
                minTime = t1;
            }
            if (t2 > EPSILON && t2 < minTime) {
                minTime = t2;
            }
        }
    }

    // ** Calculate Time to Negative Back Wall (y = NEG_BACK_WALL_Y) **
    if (vel.y < -EPSILON) { // Player is moving towards the negative back wall
        float a = 0.5f * BOOST_ACCEL_NEG;         // Acceleration coefficient (negative)
        float b = vel.y;                           // Initial velocity towards negative y
        float c = pos.y - NEG_BACK_WALL_Y;        // Initial position offset from negative back wall

        // Quadratic equation: a*t^2 + b*t + c = 0
        float discriminant = b * b - 4.0f * a * c;

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

            // Select the smallest positive time
            if (t1 > EPSILON && t1 < minTime) {
                minTime = t1;
            }
            if (t2 > EPSILON && t2 < minTime) {
                minTime = t2;
            }
        }
    }

    return minTime;
}

// Constants related to the arena dimensions and gravity
constexpr float FLOOR_Z = 18.0f;      // Assuming the floor is at z = 0
constexpr float BOOST_ACCEL_DOWN = -1000.0f;

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

    // ** Immediate Floor Proximity Check **
    if (pos.z <= FLOOR_Z) {
        return 0.0f; // Player is already at or below the floor
    }

    // ** Calculate Total Downward Acceleration **
    // Sum of gravity and boosting acceleration (both downward, hence negative)
    float totalAccel = GRAVITY_Z + BOOST_ACCEL_DOWN; // e.g., -650 + (-1000) = -1650 units/s²

    // ** Define Quadratic Coefficients **
    // Using the kinematic equation: 0.5 * a * t² + v₀ * t + s₀ = 0
    float a = 0.5f * totalAccel;    // Acceleration coefficient (negative)
    float b = vel.z;                 // Initial vertical velocity
    float c = pos.z;                 // Initial position above the floor

    // ** Calculate Discriminant **
    float discriminant = b * b - 4.0f * a * c;

    // ** Check for Real Solutions **
    if (discriminant < 0.0f) {
        return std::numeric_limits<float>::infinity(); // No real solution; player won't reach the floor
    }

    // ** Calculate the Two Possible Times Using the Quadratic Formula **
    float sqrtD = std::sqrt(discriminant);
    float t1 = (-b - sqrtD) / (2.0f * a); // Smaller time
    float t2 = (-b + sqrtD) / (2.0f * a); // Larger time

    // ** Select the Positive and Smaller Time **
    float timeToFloor = std::numeric_limits<float>::infinity();

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

    return timeToFloor;
}


#include <algorithm>  // for std::min

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;
}


float RLBotBot::calculateTimeToReach(const RLGSC::GameState& gs, const Vec& targetPos) {
    constexpr float STEP_INTERVAL = 1 / 60.f;
    constexpr float DIST_REACH_THRESH = 80;
    constexpr float FLIP_FORCE = 495;
    constexpr float MAX_TOTAL_SPEED = 2300.0f;
    constexpr float MAX_DRIVE_SPEED = 1410.0f;
    constexpr float BREAK_ACCEL = 3500.0f;
    constexpr float BOOST_ACCEL = 991.667f;
    constexpr float DRIVE_ACCEL = 1600.0f;

    const auto& car = gs.players[this->index];
    Vec carPos = car.phys.pos;
    Vec carVel = car.phys.vel;
    Vec dirToTarget = (targetPos - carPos).Normalized();
    float dist = (targetPos - carPos).Length() - CommonValues::BALL_RADIUS;
    float vel = carVel.Dot(dirToTarget);
    float rotTime = calculateRotationTime(gs, targetPos);
    float timeToSurface = calculateTimeToReachAnySurface(car);

    float t = 0;
    for (float curDist = dist, curVel = vel; curDist > DIST_REACH_THRESH; t += STEP_INTERVAL, curDist -= curVel * STEP_INTERVAL) {
        bool hasRotated = t >= rotTime;
        bool onSurface = t >= timeToSurface;

        if (curVel < MAX_TOTAL_SPEED && t == 0) {
            // Flip force
            curVel += FLIP_FORCE;
        }

        if (onSurface) {
            if (curVel < 0) {
                curVel += (BREAK_ACCEL + BOOST_ACCEL * hasRotated) * STEP_INTERVAL;
            }
            else if (curVel < MAX_DRIVE_SPEED) {
                curVel += (DRIVE_ACCEL + BOOST_ACCEL * hasRotated) * STEP_INTERVAL;
            }
            else if (curVel < MAX_TOTAL_SPEED) {
                curVel += BOOST_ACCEL * hasRotated * STEP_INTERVAL;
            }
        }

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

float RLBotBot::calculateRotationTime(const RLGSC::GameState& gs, const Vec& targetPos) {
    constexpr float MAX_ANG_VEL = 5.5f * 0.9f;  // Maximum angular velocity in radians per second

    const auto& car = gs.players[this->index];

    Vec playerPos = car.phys.pos;
    Vec playerForward = car.phys.rotMat.forward;

    Vec dirToTarget = (targetPos - playerPos).Normalized();
    float angleDiff = std::acos(RS_CLAMP(playerForward.Dot(dirToTarget), -1.0f, 1.0f)); // radians

    // Assuming constant maximum angular velocity
    return angleDiff / MAX_ANG_VEL;
}