// LUT time to reach calculation
auto lutStart = std::chrono::high_resolution_clock::now();
float lutTime = timeToReachLUT.getTimeToReach(xyDist, zDist, velToTarget, rotTime, surfaceTime);
auto lutEnd = std::chrono::high_resolution_clock::now();
std::chrono::duration<double, std::milli> lutDuration = lutEnd - lutStart;
 namespace RLGSC {

    class TimeToReachLUT {
    public:
        static constexpr int XY_BINS = 64;
        static constexpr int Z_BINS = 32;
        static constexpr int VEL_BINS = 64;
        static constexpr int ROT_BINS = 16;
        static constexpr int SURFACE_BINS = 16;
        static constexpr int64_t TOTAL_BINS = static_cast<int64_t>(XY_BINS) * Z_BINS * VEL_BINS * ROT_BINS * SURFACE_BINS;

        TimeToReachLUT() : trained(false) {
            std::cout << "LUT size: " << (TOTAL_BINS * sizeof(float) / (1024 * 1024)) << " MB" << std::endl;
        }

        void train() {
            if (trained) return;

            lut.resize(TOTAL_BINS);

            std::cout << "Training LUT..." << std::endl;
            auto start = std::chrono::high_resolution_clock::now();

#pragma omp parallel for collapse(5)
            for (int xy = 0; xy < XY_BINS; xy++) {
                for (int z = 0; z < Z_BINS; z++) {
                    for (int vel = 0; vel < VEL_BINS; vel++) {
                        for (int rot = 0; rot < ROT_BINS; rot++) {
                            for (int surf = 0; surf < SURFACE_BINS; surf++) {
                                float xyDist = xy * (8064.0f / (XY_BINS - 1));
                                float zDist = z * (2044.0f / (Z_BINS - 1));
                                float velToTarget = vel * (4600.0f / (VEL_BINS - 1)) - 2300.0f;
                                float rotTime = rot * (2.0f / (ROT_BINS - 1));
                                float surfaceTime = surf * (3.0f / (SURFACE_BINS - 1));

                                float time = TimeToReachCalculator::calculateTimeToReach(xyDist, zDist, velToTarget, rotTime, surfaceTime);

                                lut[getIndex(xy, z, vel, rot, surf)] = time;
                            }
                        }
                    }
                }
            }

            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            std::cout << "LUT training completed in " << diff.count() << " seconds." << std::endl;

            trained = true;
        }

        float getTimeToReach(float xyDist, float zDist, float velToTarget, float rotTime, float surfaceTime) const {

            // Normalize input values
            float xyNorm = std::clamp(xyDist / 8064.0f, 0.0f, 1.0f) * (XY_BINS - 1);
            float zNorm = std::clamp(zDist / 2044.0f, 0.0f, 1.0f) * (Z_BINS - 1);
            float velNorm = std::clamp((velToTarget + 2300.0f) / 4600.0f, 0.0f, 1.0f) * (VEL_BINS - 1);
            int rotBin = static_cast<int>(std::clamp(rotTime / 2.0f, 0.0f, 1.0f) * (ROT_BINS - 1));
            int surfBin = static_cast<int>(std::clamp(surfaceTime / 3.0f, 0.0f, 1.0f) * (SURFACE_BINS - 1));

            // Get lower indices
            int xy0 = static_cast<int>(xyNorm);
            int z0 = static_cast<int>(zNorm);
            int vel0 = static_cast<int>(velNorm);

            // Get upper indices
            int xy1 = RS_MIN(xy0 + 1, XY_BINS - 1);
            int z1 = RS_MIN(z0 + 1, Z_BINS - 1);
            int vel1 = RS_MIN(vel0 + 1, VEL_BINS - 1);

            // Get interpolation factors
            float xyFrac = xyNorm - xy0;
            float zFrac = zNorm - z0;
            float velFrac = velNorm - vel0;

            // Perform 3D linear interpolation
            float interpolatedTime = 0;
            for (int i = 0; i < 8; ++i) {
                int ix = (i & 1) ? xy1 : xy0;
                int iz = (i & 2) ? z1 : z0;
                int iv = (i & 4) ? vel1 : vel0;

                float weight =
                    ((i & 1) ? xyFrac : (1 - xyFrac)) *
                    ((i & 2) ? zFrac : (1 - zFrac)) *
                    ((i & 4) ? velFrac : (1 - velFrac));

                interpolatedTime += weight * lut[getIndex(ix, iz, iv, rotBin, surfBin)];
            }

            return interpolatedTime;
        }

        void saveLUT(const std::string& filename) const {
            std::ofstream file(filename, std::ios::binary);
            if (file.is_open()) {
                file.write(reinterpret_cast<const char*>(lut.data()), lut.size() * sizeof(float));
                file.close();
                std::cout << "LUT saved to " << filename << std::endl;
            }
            else {
                std::cerr << "Unable to open file for saving LUT: " << filename << std::endl;
            }
        }

        void loadLUT(const std::string& filename) {
            lut.resize(TOTAL_BINS);
            std::ifstream file(filename, std::ios::binary);
            if (file.is_open()) {
                file.read(reinterpret_cast<char*>(lut.data()), lut.size() * sizeof(float));
                file.close();
                trained = true;
                std::cout << "LUT loaded from " << filename << std::endl;
            }
            else {
                std::cerr << "Unable to open file for loading LUT: " << filename << std::endl;
            }
        }

    private:
        std::vector<float> lut;
        bool trained;

        int64_t getIndex(int xy, int z, int vel, int rot, int surf) const {
            return static_cast<int64_t>(xy) +
                static_cast<int64_t>(z) * XY_BINS +
                static_cast<int64_t>(vel) * XY_BINS * Z_BINS +
                static_cast<int64_t>(rot) * XY_BINS * Z_BINS * VEL_BINS +
                static_cast<int64_t>(surf) * XY_BINS * Z_BINS * VEL_BINS * ROT_BINS;
        }
    };

} // namespace RLGSC make the lut be alot faster and make the full updated codes