// Constants
constexpr float SIDE_WALL_X = 4096.0f;
constexpr float BACK_WALL_Y = 5120.0f;
constexpr float CEILING_Z = 2044.0f;
constexpr float BACK_NET_Y = 6000.0f;
constexpr float GRAVITY_Z = -650.0f;
constexpr float BALL_RADIUS = 92.75f; // Example value, adjust as necessary

// Movement Acceleration Constants
// Ground
constexpr float DRIVE_ACCEL = 1600.0f;
constexpr float BOOST_ACCEL = 991.667f;
constexpr float AIR_ACCEL = 66.66667f;
constexpr float BOOST_ACCEL_AIR = 1058.33333f;

// Speed Limits
constexpr float MAX_TOTAL_SPEED = 2300.0f;
constexpr float MAX_DRIVE_SPEED = 1410.0f;

// Other Parameters
constexpr float STEP_INTERVAL = 1 / 60.f;
constexpr float DIST_REACH_THRESH = 80.0f;
constexpr float FLIP_FORCE = 495.0f;

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

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

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

    Vec dirToBall = (ballPos - playerPos).Normalized();
    float dotProduct = clamp(playerForward.Dot(dirToBall), -1.0f, 1.0f);
    float angleDiff = std::acos(dotProduct); // radians

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

float RLBotBot::calculateTimeToSurface(const GameState& gs) {
    const auto& car = gs.players[this->index];
    const auto& ball = gs.ball;

    Vec pos = car.phys.pos;
    Vec vel = car.phys.vel;

    std::vector<float> times;

    // Time to Ground (z = 0)
    if (vel.z < 0) { // Only if moving downward
        // Solve for t in: pos.z + vel.z * t + 0.5 * GRAVITY_Z * t^2 = 0
        // 0.5 * GRAVITY_Z * t^2 + vel.z * t + pos.z = 0
        float a = 0.5f * GRAVITY_Z;
        float b = vel.z;
        float c = pos.z;
        float discriminant = b * b - 4 * a * c;

        if (discriminant >= 0) {
            float sqrt_discriminant = std::sqrt(discriminant);
            float t1 = (-b - sqrt_discriminant) / (2 * a);
            float t2 = (-b + sqrt_discriminant) / (2 * a);
            if (t1 >= 0)
                times.push_back(t1);
            if (t2 >= 0)
                times.push_back(t2);
        }
    }

    // Time to Ceiling (z = CEILING_Z)
    if (vel.z > 0) { // Only if moving upward
        // Solve for t in: pos.z + vel.z * t + 0.5 * GRAVITY_Z * t^2 = CEILING_Z
        float a = 0.5f * GRAVITY_Z;
        float b = vel.z;
        float c = pos.z - CEILING_Z;
        float discriminant = b * b - 4 * a * c;

        if (discriminant >= 0) {
            float sqrt_discriminant = std::sqrt(discriminant);
            float t1 = (-b - sqrt_discriminant) / (2 * a);
            float t2 = (-b + sqrt_discriminant) / (2 * a);
            if (t1 >= 0)
                times.push_back(t1);
            if (t2 >= 0)
                times.push_back(t2);
        }
    }

    // Time to Side Walls (x = ±SIDE_WALL_X)
    if (vel.x != 0.0f) {
        float tLeft = (-SIDE_WALL_X - pos.x) / vel.x;
        float tRight = (SIDE_WALL_X - pos.x) / vel.x;
        if (tLeft >= 0)
            times.push_back(tLeft);
        if (tRight >= 0)
            times.push_back(tRight);
    }

    // Time to Back Walls (y = ±BACK_WALL_Y)
    if (vel.y != 0.0f) {
        float tBack = (BACK_WALL_Y - pos.y) / vel.y;
        float tFront = (-BACK_WALL_Y - pos.y) / vel.y;
        if (tBack >= 0)
            times.push_back(tBack);
        if (tFront >= 0)
            times.push_back(tFront);
    }

    // Return the minimum positive time to any surface
    if (!times.empty()) {
        return *std::min_element(times.begin(), times.end());
    }

    // If no surfaces are reachable in the current trajectory
    return std::numeric_limits<float>::infinity();
}

float RLBotBot::calculateTimeToReach(const GameState& gs, float accel, float boostAccel, bool isAir) {
    // Depending on whether the bot is in the air or on the ground, use appropriate accelerations
    const auto& car = gs.players[this->index];
    const auto& ball = gs.ball;

    Vec carPos = car.phys.pos;
    Vec carVel = car.phys.vel;
    Vec targetPos = ball.pos;
    Vec dirToTarget = (targetPos - carPos).Normalized();

    float dist = (targetPos - carPos).Length() - BALL_RADIUS;
    float vel = carVel.Dot(dirToTarget);
    float rotTime = calculateRotationTime(gs);

    float t = 0.0f;
    float curDist = dist;
    float curVel = vel;

    while (curDist > DIST_REACH_THRESH && t < 10.0f) { // Adding a max time to prevent infinite loops
        bool hasRotated = t >= rotTime;

        // Initial flip (only once at t == 0)
        if (curVel < MAX_TOTAL_SPEED && t == 0.0f) {
            // Flip force
            curVel += FLIP_FORCE;
        }

        // Apply accelerations
        if (curVel < 0.0f) {
            curVel += (BOOST_ACCEL + accel) * STEP_INTERVAL;
        }
        else if (curVel < (isAir ? MAX_TOTAL_SPEED : MAX_DRIVE_SPEED)) {
            curVel += (accel + boostAccel * hasRotated) * STEP_INTERVAL;
        }
        else if (curVel < MAX_TOTAL_SPEED) {
            curVel += boostAccel * hasRotated * STEP_INTERVAL;
        }
        else {
            // At maximum speed, no further acceleration
            return t + (curDist / curVel);
        }

        // Clamp velocity to max limits
        if (curVel > (isAir ? MAX_TOTAL_SPEED : MAX_DRIVE_SPEED)) {
            curVel = isAir ? MAX_TOTAL_SPEED : MAX_DRIVE_SPEED;
        }

        // Update distance and time
        curDist -= curVel * STEP_INTERVAL;
        t += STEP_INTERVAL;

        // Optional: Update velocity based on gravity if in air (only affects vertical component)
        if (isAir) {
            carVel.z += GRAVITY_Z * STEP_INTERVAL;
        }

        // Prevent negative distance
        if (curDist < 0.0f)
            curDist = 0.0f;
    }

    return t;
}

float RLBotBot::calculateTotalTimeToBall(const GameState& gs) {
    const auto& car = gs.players[this->index];
    // Determine if the bot is currently in the air
    bool isInAir = car.phys.pos.z > 0.0f && car.phys.vel.z != 0.0f;

    // *** Direct Air Path ***
    float timeDirectAir = std::numeric_limits<float>::infinity();
    // Only consider direct air path if the bot is in the air or can become airborne
    // For simplicity, assume the bot can always take the air path
    timeDirectAir = calculateTimeToReach(gs, AIR_ACCEL, BOOST_ACCEL_AIR, true);

    // *** Surface-Assisted Path ***
    float timeToSurface = calculateTimeToSurface(gs);

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

    if (timeToSurface != std::numeric_limits<float>::infinity()) {
        // Simulate landing on the surface
        const auto& car = gs.players[this->index];

        // Calculate landing position
        Vec landingPos = car.phys.pos + car.phys.vel * timeToSurface + Vec{ 0.0f, 0.0f, 0.5f * GRAVITY_Z * timeToSurface * timeToSurface };

        // Clamp to the surface (assuming ground for simplicity)
        landingPos.z = 0.0f;

        // Calculate new velocity after landing: horizontal components remain, vertical set to zero
        Vec landingVel = Vec{ car.phys.vel.x, car.phys.vel.y, 0.0f };

        // Calculate new distance from landing position to ball
        Vec toBall = gs.ball.pos - landingPos;
        float distanceAfterLanding = toBall.Length() - CommonValues::BALL_RADIUS;

        // Calculate the time to reach the ball from the landing position
        // Create a temporary GameState reflecting the landing state
        GameState landingGs = gs;
        landingGs.players[index].phys.pos = landingPos;
        landingGs.players[index].phys.vel = landingVel;

        // Calculate rotation time after landing
        float rotTimeAfterLanding = calculateRotationTime(landingGs);

        // Calculate time to reach the ball from landing position using ground accelerations
        // Here we reuse calculateTimeToReach with ground parameters
        float driveTime = calculateTimeToReach(landingGs, DRIVE_ACCEL, BOOST_ACCEL, false);

        totalTimeViaSurface = timeToSurface + driveTime;
    }
    RG_LOG(timeDirectAir);
    RG_LOG(totalTimeViaSurface);
    // *** Compare Both Paths ***
    float optimalTime = RS_MIN(timeDirectAir, totalTimeViaSurface);

    return optimalTime;
}
 fix this code and ensure all the logic is correct