automatic bit sorting in C++

bitsorter.cpp

#include <array>
#include <cstdint>
#include <cstddef>
#include <cmath>
#include <cstdio>
#include <climits>
#include <vector>
#include <cassert>

/**
 * Handles bitset lesser or greater than where
 * 0 is the least significant bit,
 * 0101 -> 0 and 2 are in the set.
 * */
bool getNextGreaterThan(size_t input, size_t target, size_t* out) {
    const size_t shifted = input >> (target + 1);
    if (!shifted) return false;
    *out = __builtin_ctzll(shifted) + target + 1;
    return true;
}

bool getNextGreaterThanOrEqual(size_t input, size_t target, size_t* out) {
    const size_t shifted = input >> (target);
    if (!shifted) return false;
    *out = __builtin_ctzll(shifted) + target;
    return true;
}

// Special function for getting all of a lowest block
bool getAllFromLowest(size_t input, size_t* out) {
    const size_t clamped = input & ((size_t)-1);
    if (!clamped) return false;
    *out = 63 - __builtin_clzll(clamped);
    return true;
}

bool getAllFromGreatest(size_t input, size_t* out) {
    if (!input) return false;
    *out = __builtin_ctzll(input);
    return false;
}


bool getNextLowerThan(size_t input, size_t target, size_t* out) {
    const size_t clamped = input & ((size_t{1} << (target)) - 1);
    if (!clamped) return false;
    *out = 63 - __builtin_clzll(clamped);
    return true;
}

bool getNextLowerThanOrEqual(size_t input, size_t target, size_t* out) {
    const size_t clamped = input & ((size_t{1} << (target + 1)) - 1);
    if (!clamped) return false;
    *out = 63 - __builtin_clzll(clamped);
    return true;
}


struct ConstexprBitset64 {
    uint64_t block = 0;

    constexpr void set(size_t idx) {
        block |= uint64_t{1} << idx;
    }

    constexpr void clear(size_t idx) {
        block &= ~(uint64_t{1} << idx);
    }

    constexpr bool test(size_t idx) const {
        return (block >> idx) & 1u;
    }

    constexpr bool getAllFromLowest(size_t* out) const {
        return ::getAllFromLowest(block, out);
    }

    constexpr bool getAllFromGreatest(size_t* out) const {
        return ::getAllFromGreatest(block, out);
    }

    constexpr bool getNextLowerThan(size_t value, size_t* out) const {
        return ::getNextLowerThan(block, value, out);
    }

    constexpr bool getNextLowerThanOrEqual(size_t value, size_t* out) const {
        return ::getNextLowerThanOrEqual(block, value, out);
    }

    constexpr bool getNextGreaterThan(size_t value, size_t* out) const {
        return ::getNextGreaterThan(block, value, out);
    }

    constexpr bool getNextGreaterThanOrEqual(size_t value, size_t* out) const {
        return ::getNextGreaterThanOrEqual(block, value, out);
    }
};

template<size_t maxIntegerBits>
struct SortTable {
    static constexpr size_t blockCount = maxIntegerBits / 64;
    ConstexprBitset64 cells[blockCount] = {0};

    constexpr void set(size_t idx) {
        const size_t block = idx >> 6;
        const size_t offset = idx & 63;
        cells[block].set(offset);
    }

    constexpr bool getNextLowerThan(size_t value, size_t* out) const {
        const size_t block = value >> 6;
        const size_t offset = value & 63;
        size_t i = block;
        size_t cur_out = 0;
        bool res = cells[i].getNextLowerThan(offset, &cur_out);
        while (!res && i > 0) {
            --i;
            res = cells[i].getAllFromLowest(&cur_out);
        }
        if (res) {
            *out = cur_out + (i << 6);
            return true;
        }
        return false;
    }

    constexpr bool getNextGreaterThan(size_t value, size_t* out) const {
        const size_t block = value >> 6;
        const size_t offset = value & 63;
        size_t i = block;
        size_t cur_out = 0;
        bool res = cells[i].getNextGreaterThan(offset, &cur_out);
        while (!res && i < blockCount) {
            ++i;
            res = cells[i].getAllFromGreatest(&cur_out);
        }
        if (res) {
            *out = cur_out + (i << 6);
            return true;
        }
        return false;
    }
};


union NodeSubData {
    size_t data;
    void* node;
};


static constexpr unsigned CODE_ABSENT = 0;
static constexpr unsigned CODE_LEAF = 1;
static constexpr unsigned CODE_STEM = 2;

struct NodeSubGroup {
    unsigned code = CODE_ABSENT;
    NodeSubData sdata;
};

enum class InsertResultState {
    AlreadyPresent,
    UnequalToLeaf,
    Absent
};

struct InsertResult {
    InsertResultState state = InsertResultState::Absent;
    NodeSubData data;
};

template<size_t maxIntegerBits>
struct SortNode {
    NodeSubGroup children[maxIntegerBits];
    SortTable<maxIntegerBits> table;

};


template<size_t maxIntegerBits>
struct SortTree {
    std::vector<SortNode<maxIntegerBits>> tree;

    SortTree() {
        tree.emplace_back();
    }

    void insert16Bit(size_t number) {

    }
};


static void printLowerThan(size_t set, size_t target) {
    size_t out = 0;
    if(getNextLowerThan(set, target, &out)) {
        std::printf("%zu\n", out);
    }
}


static void printGreaterThan(size_t set, size_t target) {
    size_t out = 0;
    if(getNextGreaterThan(set, target, &out)) {
        std::printf("%zu\n", out);
    }
}

static void printGreaterThanOrEqual(size_t set, size_t target) {
    size_t out = 0;
    if(getNextGreaterThanOrEqual(set, target, &out)) {
        std::printf("%zu\n", out);
    }
}


int main(int argc, char const *argv[])
{
    printGreaterThan(0b00100101, 0);
    printGreaterThan(0b00100101, 3);
    printGreaterThanOrEqual(0b00100100, 6);
    printGreaterThanOrEqual(0b00100100, 5);

    printLowerThan((size_t{1} << 63) | (uint64_t{1} << 58), 63);

    std::puts("sets\n");

    SortTable<256> foo;
    size_t out = 0;
    foo.set(144);
    foo.set(122);
    foo.set(78);
    foo.set(45);
    bool res = foo.getNextLowerThan(133, &out);
    std::printf("%zu\n", out);
    foo.set(126);
    res = foo.getNextLowerThan(133, &out);
    std::printf("%zu\n", out);
    //printGreaterThan((size_t{1} << 54) | (size_t{1} << 60), 0);
    res = foo.getNextGreaterThan(130, &out);
    std::printf("%zu\n", out);
    return 0;
}