// Constants for angular accelerations
constexpr float MAX_YAW_ACCEL = 9.11f;      // radians/s^2
constexpr float MAX_PITCH_ACCEL = 12.46f;   // radians/s^2
constexpr float MAX_ROLL_ACCEL = 38.34f;    // radians/s^2

// Constants for speed modeling while turning
constexpr float V_MAX_ACCEL_TURN = 1234.0f; // Max forward velocity while accelerating
constexpr float TAU_ACCEL_TURN = 0.74704f;

constexpr float V_MIN_DECEL_TURN = 1234.0f; // Min forward velocity while decelerating
constexpr float V_INITIAL_DECEL = 2300.0f;

// Piecewise deceleration parameters
struct DecelSegment {
    float time_min;
    float time_max;
    float m; // Slope
    float b; // Intercept
};

// Define deceleration segments based on provided piecewise functions
const std::vector<DecelSegment> decelSegments = {
    {7.5f, std::numeric_limits<float>::infinity(), 0.0f, 1234.0f}, // t >= 7.5
    {4.5f, 7.5f, 0.0025f, 245.4857f},                         // 4.5 <= t < 7.5
    {4.0f, 4.5f, 0.03636f, 50.18f},
    {3.2f, 4.0f, 0.01538f, 23.53846f},
    {2.6f, 3.2f, 0.007595f, 13.24f},
    {1.3f, 2.6f, 0.00307f, 6.9f},
    {0.3f, 1.3f, 0.0025f, 5.86f},
    {0.1f, 0.3f, 0.00303f, 7.039f},
    {0.0f, 0.1f, 0.01f, 23.0f}
};

// Function to calculate speed while accelerating during turning
float get_speed_from_time_turning_accel(float time) {
    return V_MAX_ACCEL_TURN * (1.0f - std::exp(-time / TAU_ACCEL_TURN));
}

// Function to calculate speed while decelerating during turning
float get_speed_from_time_turning_decel(float time) {
    if (time >= 7.5f) {
        return V_MIN_DECEL_TURN;
    }
    for (const auto& segment : decelSegments) {
        if (time >= segment.time_min && time < segment.time_max) {
            return segment.m * time + segment.b;
        }
    }
    return V_INITIAL_DECEL; // Default value if no segment matches
}

// Function to calculate time since starting deceleration based on speed
float get_time_turning_from_speed_decel(float speed) {
    if (speed <= V_MIN_DECEL_TURN) {
        return 7.5f;
    }
    if (speed <= 1248.0f) {
        return 245.4857f - 0.192857f * speed;
    }
    if (speed <= 1270.0f) {
        return 50.18f - 0.03636f * speed;
    }
    if (speed <= 1322.0f) {
        return 23.53846f - 0.01538f * speed;
    }
    if (speed <= 1401.0f) {
        return 13.24f - 0.007595f * speed;
    }
    if (speed <= 1824.0f) {
        return 6.9f - 0.00307f * speed;
    }
    if (speed <= 2224.0f) {
        return 5.86f - 0.0025f * speed;
    }
    if (speed <= 2290.0f) {
        return 7.039f - 0.00303f * speed;
    }
    if (speed <= 2300.0f) {
        return 23.0f - 0.01f * speed;
    }
    return 0.0f;
}

// Clamp function
float clamp(float value, float min_val, float max_val) {
    return RS_MAX(min_val, RS_MIN(value, max_val));
}

// Spherical Linear Interpolation (Slerp) for Vec class
Vec Slerp(const Vec& from, const Vec& to, float t) {
    float dot_product = clamp(from.Dot(to), -1.0f, 1.0f);
    float theta = std::acos(dot_product) * t;
    Vec relative_vec = (to - from * dot_product).Normalized();
    return (from * std::cos(theta) + relative_vec * std::sin(theta)).Normalized();
}

// Define a maximum prediction time to prevent infinite loops
constexpr float MAX_PREDICTION_TIME = 4.0f; // seconds
constexpr float TIME_STEP = 1.0f / 60.0f;     // Simulation step (1/60th of a second)

// Improved calculateMinAirTime function
float calculateMinAirTime(const Vec& playerPos, const Vec& playerVel, bool hasFlip) {
    constexpr float GRAVITY = 650.0f; // Units per second squared (adjust based on game physics)

    // Initialize simulation parameters
    float t = 0.0f;
    Vec pos = playerPos;
    Vec vel = playerVel;
    float timeToGround = std::numeric_limits<float>::infinity();
    float timeToSideWall = std::numeric_limits<float>::infinity();
    float timeToBackWall = std::numeric_limits<float>::infinity();
    float timeToCeiling = std::numeric_limits<float>::infinity();

    // Simulate the player's trajectory over time
    while (t < MAX_PREDICTION_TIME) {
        // Update position and velocity
        // Only vertical velocity is affected by gravity
        pos.z += vel.z * TIME_STEP + 0.5f * (-GRAVITY) * TIME_STEP * TIME_STEP;
        vel.z += (-GRAVITY) * TIME_STEP;

        pos.x += vel.x * TIME_STEP;
        pos.y += vel.y * TIME_STEP;

        // Check for ground collision
        if (pos.z <= CommonValues::BALL_RADIUS) { // Assuming ground is at z = 0
            timeToGround = t;
            break; // Ground collision is the earliest possible
        }

        // Check for ceiling collision
        if (vel.z > 0.0f && pos.z + CommonValues::BALL_RADIUS >= CommonValues::CEILING_Z) {
            timeToCeiling = t;
            // Do not break; the player might hit the ceiling before the ground
            // But ground collision is prioritized
            break;
        }

        // Check for side wall collision
        if (vel.x != 0.0f) {
            float timeToCurrentSideWall = 0.0f;
            if (vel.x > 0.0f) {
                timeToCurrentSideWall = (CommonValues::SIDE_WALL_X - pos.x - CommonValues::BALL_RADIUS) / vel.x;
            }
            else {
                timeToCurrentSideWall = (-CommonValues::SIDE_WALL_X - pos.x + CommonValues::BALL_RADIUS) / vel.x;
            }
            if (timeToCurrentSideWall >= 0.0f && timeToCurrentSideWall <= TIME_STEP) {
                timeToSideWall = t;
                //RG_LOG(timeToSideWall);
                break;
            }
        }

        // Check for back wall collision
        if (vel.y != 0.0f) {
            float timeToCurrentBackWall = 0.0f;
            if (vel.y > 0.0f) {
                timeToCurrentBackWall = (CommonValues::BACK_WALL_Y - pos.y - CommonValues::BALL_RADIUS) / vel.y;
            }
            else {
                timeToCurrentBackWall = (-CommonValues::BACK_WALL_Y - pos.y + CommonValues::BALL_RADIUS) / vel.y;
            }
            if (timeToCurrentBackWall >= 0.0f && timeToCurrentBackWall <= TIME_STEP) {
                timeToBackWall = t;
                //RG_LOG(timeToBackWall);
                break;
            }
        }

        // Increment time
        t += TIME_STEP;
    }

    // Determine the minimum time among all collision possibilities
    float minAirTime = RS_MIN(timeToGround, timeToSideWall, timeToBackWall, timeToCeiling);
    // Ensure the minimum air time is not negative or NaN
    if (minAirTime < 0.0f || std::isnan(minAirTime)) {
        minAirTime = 0.0f;
    }

    return minAirTime;
}

float getTimeToReachBall(const PlayerData& player, const Vec& ballPos) {
    constexpr float DIST_REACH_THRESH = 10.0f;
    constexpr float STEP_INTERVAL = 1.0f / 120.0f; // Assuming 120 Hz tick rate
    constexpr float FLIP_FORCE = 500.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;

    Vec playerPos = player.phys.pos;
    Vec playerForward = player.phys.rotMat.forward;
    Vec playerVel = player.phys.vel;
    bool hasFlip = player.carState.HasFlipOrJump();

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

    // Initialize rotation state
    float remainingAngle = angleDiff;
    float angularVelocity = 0.0f; // radians/s
    Vec currentForward = playerForward;

   

    // Initialize movement state
    float t = 0.0f;
    float curDist = (ballPos - playerPos).Length();
    float curVel = playerVel.Dot(dirToBall); // Projection of velocity towards the ball

    // Calculate minimum air time
    float minAirTime = calculateMinAirTime(playerPos, playerVel, hasFlip);
    //RG_LOG(minAirTime);

    // Main simulation loop
    while (t < minAirTime && curDist > DIST_REACH_THRESH) {
        // Rotation dynamics
        float angularAccel = MAX_YAW_ACCEL; // radians/s^2
        if (remainingAngle > 0.0f) {
            angularVelocity += angularAccel * STEP_INTERVAL;
            float deltaAngle = angularVelocity * STEP_INTERVAL;
            if (deltaAngle > remainingAngle) {
                deltaAngle = remainingAngle;
                angularVelocity = 0.0f;
            }
            remainingAngle -= deltaAngle;
            currentForward = Slerp(currentForward, dirToBall, deltaAngle / angleDiff);
        }

        bool hasFullyRotated = (remainingAngle <= 0.001f); // Threshold for completion
        //RG_LOG(remainingAngle);
        // Apply flip force if available
        if (hasFlip && t == 0.0f) {
            curVel += FLIP_FORCE;
        }

        // Speed modeling
        float accel;
        if (hasFullyRotated) {
            accel = get_speed_from_time_turning_accel(t) < V_MAX_ACCEL_TURN
                ? AIR_ACCEL + BOOST_ACCEL_AIR
                : AIR_ACCEL;
        }
        else {
            accel = AIR_ACCEL;
        }

        

        curVel += accel * STEP_INTERVAL;
        curVel = RS_MIN(curVel, MAX_TOTAL_SPEED);

        curDist -= curVel * STEP_INTERVAL;
        t += STEP_INTERVAL;
    }

    // Handle remaining distance after rotation is complete
    while (curDist > DIST_REACH_THRESH) {
        if (curVel < MAX_DRIVE_SPEED) {
            curVel += (DRIVE_ACCEL + BOOST_ACCEL_GROUND) * STEP_INTERVAL;
        }
        else if (curVel < MAX_TOTAL_SPEED) {
            curVel += BOOST_ACCEL_GROUND * STEP_INTERVAL;
        }
        else {
            float remainingTime = curDist / curVel;
            t += remainingTime;
            break;
        }

        curVel = RS_MIN(curVel, MAX_TOTAL_SPEED);
        curDist -= curVel * STEP_INTERVAL;
        t += STEP_INTERVAL;
    }
    RG_LOG(t);
    return t;
}

void RLBotBot::RenderTimeToReachBall(const RLGSC::GameState& gs) {
    if (!renderTimeToReachBallEnabled || !ballPredTracker) return;
    rlbot::ScopedRenderer renderer("time_to_reach_renderer");

    constexpr float PREDICTION_DURATION = 6.0f; // Predict 6 seconds into the future
    constexpr int NUM_PREDICTION_POINTS = 8; // We want 8 prediction points

    // Update ball prediction
    RocketSim::BallState currentBallState;
    currentBallState.pos = RocketSim::Vec(gs.ball.pos.x, gs.ball.pos.y, gs.ball.pos.z);
    currentBallState.vel = RocketSim::Vec(gs.ball.vel.x, gs.ball.vel.y, gs.ball.vel.z);
    currentBallState.angVel = RocketSim::Vec(gs.ball.angVel.x, gs.ball.angVel.y, gs.ball.angVel.z);
    currentBallState.rotMat = RocketSim::RotMat(gs.ball.rotMat.forward, gs.ball.rotMat.right, gs.ball.rotMat.up);
    int numTicks = static_cast<int>(PREDICTION_DURATION * 120); // Assuming 120 ticks per second
    ballPredTracker->UpdatePredManual(currentBallState, numTicks);

    const auto& ballPredData = ballPredTracker->predData;
    size_t predictionInterval = ballPredData.size() / NUM_PREDICTION_POINTS;

    // Find the opponent (if any)
    const PlayerData* opponent = nullptr;
    for (const auto& player : gs.players) {
        if (player.team != gs.players[index].team) {
            opponent = &player;
            break;
        }
    }

    std::vector<float> localTimeToReach(NUM_PREDICTION_POINTS);
    std::vector<float> opponentTimeToReach(NUM_PREDICTION_POINTS);
    float localPossession = 0.0f;
    float opponentPossession = 0.0f;

    for (size_t j = 0; j < NUM_PREDICTION_POINTS; ++j) {
        size_t index = j * predictionInterval;
        if (index >= ballPredData.size()) break;

        Vec ballPos(ballPredData[index].pos.x, ballPredData[index].pos.y, ballPredData[index].pos.z);
        localTimeToReach[j] = getTimeToReachBall(gs.players[this->index], ballPos);

        if (opponent) {
            opponentTimeToReach[j] = getTimeToReachBall(*opponent, ballPos);
            if (localTimeToReach[j] < opponentTimeToReach[j]) {
                localPossession += 1.0f;
            }
            else {
                opponentPossession += 1.0f;
            }
        }
        else {
            // If there's no opponent, local player always has possession
            localPossession += 1.0f;
        }
    }

    float totalPossession = localPossession + opponentPossession;
    localPossession /= totalPossession;
    opponentPossession /= totalPossession;

    // Render possession information
    std::stringstream localSS;
    localSS << "Local player possession: " << std::fixed << std::setprecision(2) << localPossession * 100 << "%";
    renderer.DrawString2D(localSS.str(), rlbot::Color::blue, rlbot::flat::Vector3{ 10, 10, 0 }, 2.0f, 2.0f);

    if (opponent) {
        std::stringstream opponentSS;
        opponentSS << "Opponent possession: " << std::fixed << std::setprecision(2) << opponentPossession * 100 << "%";
        renderer.DrawString2D(opponentSS.str(), rlbot::Color::white, rlbot::flat::Vector3{ 10, 40, 0 }, 2.0f, 2.0f);
    }

    // Render the path and time to reach for both players
    std::vector<rlbot::flat::Vector3> points;
    for (size_t j = 0; j < NUM_PREDICTION_POINTS; ++j) {
        size_t index = j * predictionInterval;
        if (index >= ballPredData.size()) break;

        points.push_back({
            static_cast<float>(ballPredData[index].pos.x),
            static_cast<float>(ballPredData[index].pos.y),
            static_cast<float>(ballPredData[index].pos.z)
            });

        std::stringstream localSS;
        localSS << std::fixed << std::setprecision(2) << localTimeToReach[j] << "s";

        rlbot::flat::Vector3 localTextPos = {
            static_cast<float>(ballPredData[index].pos.x),
            static_cast<float>(ballPredData[index].pos.y),
            static_cast<float>(ballPredData[index].pos.z + 50.0f)
        };

        renderer.DrawString3D(localSS.str(), rlbot::Color::blue, localTextPos, 1.0f, 1.0f);

        if (opponent) {
            std::stringstream opponentSS;
            opponentSS << std::fixed << std::setprecision(2) << opponentTimeToReach[j] << "s";

            rlbot::flat::Vector3 opponentTextPos = {
                static_cast<float>(ballPredData[index].pos.x),
                static_cast<float>(ballPredData[index].pos.y),
                static_cast<float>(ballPredData[index].pos.z - 50.0f)
            };

            renderer.DrawString3D(opponentSS.str(), rlbot::Color::white, opponentTextPos, 1.0f, 1.0f);
        }
    }

    // Render the linked path
    if (!points.empty()) {
        // Draw a line from the car to the first prediction point
        rlbot::flat::Vector3 carPos = {
            static_cast<float>(gs.players[this->index].phys.pos.x),
            static_cast<float>(gs.players[this->index].phys.pos.y),
            static_cast<float>(gs.players[this->index].phys.pos.z)
        };
        renderer.DrawLine3D(rlbot::Color::yellow, carPos, points[0]);

        // Draw lines between prediction points
        for (size_t i = 1; i < points.size(); ++i) {
            renderer.DrawLine3D(rlbot::Color::yellow, points[i - 1], points[i]);
        }
    }

    // Render overall possession summary
    std::stringstream summaryStream;
    summaryStream << "Team Possession - "
        << (gs.players[this->index].team == Team::BLUE ? "Blue: " : "Orange: ")
        << std::fixed << std::setprecision(2) << localPossession * 100 << "%";
    if (opponent) {
        summaryStream << " | "
            << (gs.players[this->index].team == Team::BLUE ? "Orange: " : "Blue: ")
            << opponentPossession * 100 << "%";
    }
    renderer.DrawString2D(summaryStream.str(), rlbot::Color::white, rlbot::flat::Vector3{ 10, 70, 0 }, 2.0f, 2.0f);
} i want you too also render what this outputs float calculateMinAirTime(const Vec& playerPos, const Vec& playerVel, bool hasFlip) {
    constexpr float GRAVITY = 650.0f; // Units per second squared (adjust based on game physics)

    // Initialize simulation parameters
    float t = 0.0f;
    Vec pos = playerPos;
    Vec vel = playerVel;
    float timeToGround = std::numeric_limits<float>::infinity();
    float timeToSideWall = std::numeric_limits<float>::infinity();
    float timeToBackWall = std::numeric_limits<float>::infinity();
    float timeToCeiling = std::numeric_limits<float>::infinity();

    // Simulate the player's trajectory over time
    while (t < MAX_PREDICTION_TIME) {
        // Update position and velocity
        // Only vertical velocity is affected by gravity
        pos.z += vel.z * TIME_STEP + 0.5f * (-GRAVITY) * TIME_STEP * TIME_STEP;
        vel.z += (-GRAVITY) * TIME_STEP;

        pos.x += vel.x * TIME_STEP;
        pos.y += vel.y * TIME_STEP;

        // Check for ground collision
        if (pos.z <= CommonValues::BALL_RADIUS) { // Assuming ground is at z = 0
            timeToGround = t;
            break; // Ground collision is the earliest possible
        }

        // Check for ceiling collision
        if (vel.z > 0.0f && pos.z + CommonValues::BALL_RADIUS >= CommonValues::CEILING_Z) {
            timeToCeiling = t;
            // Do not break; the player might hit the ceiling before the ground
            // But ground collision is prioritized
            break;
        }

        // Check for side wall collision
        if (vel.x != 0.0f) {
            float timeToCurrentSideWall = 0.0f;
            if (vel.x > 0.0f) {
                timeToCurrentSideWall = (CommonValues::SIDE_WALL_X - pos.x - CommonValues::BALL_RADIUS) / vel.x;
            }
            else {
                timeToCurrentSideWall = (-CommonValues::SIDE_WALL_X - pos.x + CommonValues::BALL_RADIUS) / vel.x;
            }
            if (timeToCurrentSideWall >= 0.0f && timeToCurrentSideWall <= TIME_STEP) {
                timeToSideWall = t;
                //RG_LOG(timeToSideWall);
                break;
            }
        }

        // Check for back wall collision
        if (vel.y != 0.0f) {
            float timeToCurrentBackWall = 0.0f;
            if (vel.y > 0.0f) {
                timeToCurrentBackWall = (CommonValues::BACK_WALL_Y - pos.y - CommonValues::BALL_RADIUS) / vel.y;
            }
            else {
                timeToCurrentBackWall = (-CommonValues::BACK_WALL_Y - pos.y + CommonValues::BALL_RADIUS) / vel.y;
            }
            if (timeToCurrentBackWall >= 0.0f && timeToCurrentBackWall <= TIME_STEP) {
                timeToBackWall = t;
                //RG_LOG(timeToBackWall);
                break;
            }
        }

        // Increment time
        t += TIME_STEP;
    }

    // Determine the minimum time among all collision possibilities
    float minAirTime = RS_MIN(timeToGround, timeToSideWall, timeToBackWall, timeToCeiling);
    // Ensure the minimum air time is not negative or NaN
    if (minAirTime < 0.0f || std::isnan(minAirTime)) {
        minAirTime = 0.0f;
    }

    return minAirTime;
}