initializer.h

#pragma once
 
#include "vex/vex.h"
#include <functional>
#include <string>
#include <vector>

#define INIT(name, func, meta) Initialization{name, func, meta}

#define FUNC_INIT(func) Initialization{"", func, 1}

#define WEIGHTED_INIT(func, weight) Initialization{"", func, weight}

#define RED_INIT(name, func) Initialization{name, func, ClrRed }

#define BLUE_INIT(name, func) Initialization{name, func, ClrBlue}

#define NEUTRAL_INIT(name, func) Initialization{name, func, ClrGray}
 

struct Initialization {
    const std::string name;           
    const std::function<void()> init; 
    const unsigned int meta;          
};
 
 

namespace Selector {
    
using selector_t = size_t();

constexpr size_t NO_SELECTION_INDEX = SIZE_MAX;

inline std::function<selector_t> select(size_t n) {
    return [=]() {
        return n;
    };
}

inline std::function<selector_t> random(size_t a, size_t b) {
    return [=]() {
        if(b < a) return (size_t) 0;
        if(a == b) return a;
        return a + ((size_t) rand()) % (b - a);
    };
}

inline std::function<selector_t> random(size_t n) {
    return random(0, n);
}

template<size_t FLAG = NO_SELECTION_INDEX>
inline std::function<selector_t> skippable(std::function<selector_t> selector) {
    return (FLAG == NO_SELECTION_INDEX && selector) ? selector : select(FLAG);
}

inline std::function<selector_t> timeout(std::function<selector_t> selector, uint64_t microsec, unsigned int fallback = NO_SELECTION_INDEX, std::function<void(size_t)> cancel = nullptr) {
    if(!selector) return [=]() { return fallback; };
    struct thread_args {
        std::function<selector_t> selector;
        size_t output;
    };
    return [=]() {
        vex::timer timer;
        uint64_t thread_start = timer.systemHighResolution();
        thread_args targs = {selector, NO_SELECTION_INDEX};
        
        vex::thread sThread([](void* args) {
            ((thread_args*) args)->output = ((thread_args*) args)->selector();
        }, &targs);
        
        while(sThread.joinable()) {
            if(timer.systemHighResolution() - thread_start > microsec) sThread.interrupt();
            else if(targs.output == NO_SELECTION_INDEX) {
                vexDelay(100);
                continue;
            }
 
            sThread.join();
        }
 
        if(targs.output == NO_SELECTION_INDEX) {
            cancel(fallback);
            return fallback;
        } else return targs.output;
    };
}

inline std::function<selector_t> delayed(std::function<selector_t> selector, unsigned int ms) {
    return [=]() {
        vexDelay(ms);
        return selector();
    };
}

inline std::function<selector_t> potentiometer(vex::pot& potentiometer, const size_t& initializations) {
    return [=, &potentiometer]() {
        vexDelay(250);
        double region = 100.0 / initializations;
        double angle = potentiometer.value(vex::percentUnits::pct);
        return (size_t) ((angle < 100) ? angle : 99.9) / region;
    };
}

inline std::function<selector_t> weighted_potentiometer(vex::pot& potentiometer, std::vector<Initialization>& initializations) {
    return [&]() {
        vexDelay(250);
        size_t total_weights = 0;
        for(Initialization initialization : initializations) total_weights += initialization.meta;
 
        size_t angle = (potentiometer.value(vex::percentUnits::pct)/100.0)*total_weights;
        for(int i = initializations.size()-1; i > 0; i--) {
            if(initializations.at(i).meta == 0) continue;
 
            total_weights -= initializations.at(i).meta;
            if(angle >= total_weights) return i;
        }
        return 0;
    };
}
 
}; // namespace Selector
 
 
class Initializer {
public:
    Initializer(std::function<void()> initialize);

    Initializer(std::vector<Initialization> initializations, std::function<Selector::selector_t> selector,
                std::function<void()> pre_init = nullptr, std::function<void()> post_init = nullptr);
 
    
    void initialize();

    const std::vector<Initialization>& initializations() const;

    const std::size_t initialization_count() const;

    const std::string selected_name() const;

    unsigned int selected_meta() const;

    unsigned int selected_index() const;

    bool uninitialized() const;
 
private:
    std::function<Selector::selector_t> selector;
    std::vector<Initialization> initialization_list;
 
    std::function<void()> pre_init;
    std::function<void()> post_init;
 
    Initialization *selected_initialization = nullptr;
    size_t selection = Selector::NO_SELECTION_INDEX;
    bool initialized = false;
};