// 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;
} remake this code to be a perfect lower bound estimateion of time to reach the ball, fix all code that dosent make sense and make sure that its perfectly calcualtes the time to reach