#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/timer.hpp>
#include "biggles/io.hpp"
#include "tools/sundries.hpp"
#include "detail/random.hpp"
#include "detail/physics.hpp"
#include "tools/debug.hpp"

using namespace biggles;

const float MAX_X = 8.f;
const float MAX_Y = 8.f;
const time_stamp END_TIME = 20;

int encode3_obs(const observation& o) {
    return (int(x(o)) % 3 + 3 * (int(y(o)) % 3));
}

float auto_correlation(data_iter b, data_iter e, size_t lag) {
    size_t len = std::distance(b, e);
    BOOST_ASSERT(lag>0);
    BOOST_ASSERT(len > lag+1);
    data_iter b2 = b;
    data_iter e1 = b;
    std::advance(b2, lag);
    std::advance(e1, len-lag);
    std::pair<float, float> ms1 = mean_stdev(b, e1);
    std::pair<float, float> ms2 = mean_stdev(b2, e);
    std::deque<float> products(len-1);
    std::transform(b, e1, b2, products.begin(), std::multiplies<float>());
    float sum = std::accumulate(products.begin(), products.end(), 0.f);
    return (sum - (len-lag)*ms1.first*ms2.first)/((len-lag-1)*ms1.second*ms2.second);
}

float correlation(data_iter b1, data_iter e1, data_iter b2) {
    size_t len = std::distance(b1, e1);
    data_iter e2 = b2;
    std::advance(e2, len);
    std::deque<float> products(len);
    std::pair<float, float> ms1 = mean_stdev(b1, e1);
    std::pair<float, float> ms2 = mean_stdev(b2, e2);
    std::transform(b1, e1, b2, products.begin(), std::multiplies<float>());
    float sum = std::accumulate(products.begin(), products.end(), 0.f);
    return (sum - len*ms1.first*ms2.first)/((len-1)*ms1.second*ms2.second);
}

std::string track_to_str(const track & track0, time_stamp end_ts) {

    time_stamp ts=track0.first_time_stamp();
    std::stringstream track_ss;
    track_ss << std::string(ts, ' ');
    for (; ts < track0.last_time_stamp(); ++ts) {
        observation_collection::const_range obs_at_t(track0.observations().at_time_stamp(ts));
        if (obs_at_t.first != obs_at_t.second)
            track_ss << encode3_obs(*obs_at_t.first);
        else
           track_ss << "_";
    }
    while (ts++ < end_ts) {
        track_ss << " ";
    }
    return track_ss.str();
}

float pdr_sample_stdev(const float& n_forth, const float& total, const float& n_back) {
    return sqrtf( n_forth*n_forth/(total*total*n_back*n_back) * (
        n_forth*(1.f - n_forth/total) + n_forth/n_back*n_forth * (1.f - n_back/n_forth)
        ) );
}

partition_ptr_t demote(partition_ptr_t& part_sample) {
    clutter_ptr clutter(new clutter_t(part_sample->clutter()));
    boost::shared_ptr<track_collection> tracks(new track_collection());
    BOOST_FOREACH(const boost::shared_ptr<const track>& t, part_sample->tracks()) {
        BOOST_FOREACH(const observation& o, t->observations()) {
            clutter->insert(o);
        }
    }
    return partition_ptr_t(new partition(tracks, clutter, part_sample->expansion()));
}

bool parse_args(int argc, char** argv, general_paras& gen_pars) {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    random_gen_paras& paras(gen_pars.rgp);
    if (gen_pars.seed == 0)
        gen_pars.seed = std::time(0);

    desc.add_options()
        ("samples,n", po::value< int >(&gen_pars.total)->default_value(int(gen_pars.total)),
            "number of samples")
        ("min-tracks,m", po::value< int >(&paras.min_tracks)->default_value(int(paras.min_tracks)),
            "min. number of tracks")
        ("max-tracks,x", po::value< int >(&paras.max_tracks)->default_value(int(paras.max_tracks)),
            "max. number of tracks")
        ("p-yes", po::value< float >(&paras.p_yes)->default_value(float(paras.p_yes)), "p-yes")
        ("p-no", po::value< float >(&paras.p_no)->default_value(float(paras.p_no)), "p-no")
        ("p-tr", po::value< float >(&paras.p_tr)->default_value(float(paras.p_tr)), "p-tr")
        ("lambda,l", po::value< float >(&paras.lambda)->default_value(float(paras.lambda)), "lambda for track length")
        ("seed,s", po::value< std::string >()->default_value(boost::str(boost::format("%x") % gen_pars.seed)),
            "random seed in hex format")
        ("input,i", po::value< std::string >(&paras.input)->default_value(""), "partition input file")
        ("output,o", po::value< std::string >(&paras.output)->default_value(""), "partition output file")
        ("opts", po::value< std::string >(&gen_pars.opts), "additional options string")
        ("help,h", "print help")
    ;

    po::options_description argsdesc("Command line arguments");
    argsdesc.add(desc);

    po::variables_map args;
    po::store(po::command_line_parser(argc, argv).options(argsdesc).run(), args);
    po::notify(args);
    std::stringstream interpreter;
    interpreter << std::hex << args["seed"].as<std::string>();
    interpreter >> gen_pars.seed;
    if (args.count("help") > 0) {
        std::cout << argsdesc << std::endl;
        return false;
    }
    return true;
}

bool are_equal(const biggles::clutter_t& oc1, const biggles::clutter_t& oc2) {
    if ((oc1.size() == 0) xor (oc2.size() == 0)) {
        return false;
    }
    biggles::time_stamp first_ts = oc1.first_time_stamp();
    biggles::time_stamp last_ts = oc1.last_time_stamp();
    if (first_ts != oc2.first_time_stamp()) return false;
    if (last_ts != oc2.last_time_stamp()) return false;
    for (biggles::time_stamp ts = first_ts; ts < last_ts; ++ts) {
        if (oc1.size(ts) != oc2.size(ts)) return false;
        BOOST_FOREACH(const biggles::observation& o, oc1.observations(ts)) {
            if (not oc2.contains(o))
                return false;
        }
    }
    return true;
}

std::string clutter_to_str(const clutter_t& clutter, time_stamp end_ts) {
    time_stamp ts=clutter.first_time_stamp();
    std::stringstream track_ss;
    for (int i = 0; i < int(ts); ++i)
        track_ss << " :";
    for (; ts < clutter.last_time_stamp(); ++ts) {
        std::deque<observation> obs_at_t(clutter.observations(ts));
        if (not obs_at_t.empty()) {
            for (std::deque<observation>::iterator it = obs_at_t.begin(); it != obs_at_t.end(); ++it) {
                track_ss << encode3_obs(*it);
            }
            track_ss << ":";
        }
        else {
           track_ss << " :";
        }
    }
    while (ts++ < end_ts) {
        track_ss << " :";
    }
    return track_ss.str();
}

partition_ptr_t random_debug_partition(const random_gen_paras& paras) {
    if (paras.input.size() > 0) {
        std::ifstream fh(paras.input.c_str());
        partition_ptr_t part(io::read_partition_from_json_stream(fh));
        return part;
    }
    BOOST_ASSERT(paras.min_tracks <= paras.max_tracks);
    BOOST_ASSERT(7 > paras.max_tracks);
    BOOST_ASSERT(paras.min_tracks >= 0);
    boost::shared_ptr<track_collection> tracks(new track_collection());
    std::deque<observation> clutter_obs;
    const float p_yes = paras.p_yes;
    const float p_no = paras.p_no;
    float p = p_yes;
    for (int y = 0; y < 3; ++y) {
        for (int t = 0 ; t < 10; ++t) {
            if (sampling::uniform_real(0, 1) < p) {
                p = p_yes;
                clutter_obs.push_back(new_obs(1, y, t));
            } else {
                p = p_no;
            }
        }
    }
    int num_tracks = sampling::uniform_int(paras.min_tracks, paras.max_tracks + 1);
    int i_track = 0;
    float p_tr = paras.p_tr;
    for (int x = 0; x<3; x+=2) {
        for (int y = 0; y < 3; ++y) {
            if (i_track >= num_tracks) break;
            observation_collection tr_obs;
            time_stamp tlo, thi;
            bool done = false;
            while (not done) {
                tr_obs.clear();
                tlo = sampling::uniform_int(0, 8);
                thi = std::min(sampling::poisson(paras.lambda) + 1 + tlo, time_stamp(10));
                for (time_stamp t = tlo; t < thi; ++t)
                    if (sampling::uniform_real(0, 1) < p_tr) tr_obs.insert(
                        new_obs(x, y, t));
                done = tr_obs.size() > 1;
            }
            tracks->insert(track(tlo, thi, tr_obs.begin(), tr_obs.end(), 1.0f));
            ++i_track;
        }
    }
    clutter_ptr clutter(new clutter_t(clutter_obs.begin(), clutter_obs.end()));
    partition_ptr_t part(new partition(tracks, clutter, partition::expansion_2d(0.f, 2.f, 0.f, 2.f)));
    if (paras.output.size() > 0) {
        std::ofstream fh(paras.output.c_str());
        io::write_partition_to_json_stream(fh, *part);
    }
    return part;
}

observation sample_obs(const observation& last_obs, const time_stamp time, const float speed_limit) {
    float angle = sampling::uniform_real() * 2.f * 3.141592653589793f;
    float radius = sampling::uniform_real() * std::abs(t(last_obs)-time) * speed_limit;
    return new_obs(radius*std::cos(angle) + x(last_obs), radius*std::sin(angle) + y(last_obs), time);
}

observation sample_obs(const float x, const float y, const time_stamp time) {
    return new_obs(sampling::uniform_real() * x, sampling::uniform_real() * y, time);
}

partition_ptr_t random_debug_partition_adv(const random_gen_paras& paras) {
    if (paras.input.size() > 0) {
        std::ifstream fh(paras.input.c_str());
        partition_ptr_t part(io::read_partition_from_json_stream(fh));
        return part;
    }
    BOOST_ASSERT(paras.min_tracks <= paras.max_tracks);
    BOOST_ASSERT(7 > paras.max_tracks);
    BOOST_ASSERT(paras.min_tracks >= 0);
    boost::shared_ptr<track_collection> tracks(new track_collection());
    std::deque<observation> clutter_obs;
    const float p_yes = paras.p_yes;
    const float p_no = paras.p_no;
    float p = p_yes;
    for (int y = 0; y < 3; ++y) {
        for (int t = 0 ; t < 10; ++t) {
            if (sampling::uniform_real(0, 1) < p) {
                p = p_yes;
                clutter_obs.push_back(new_obs(1, y, t));
            } else {
                p = p_no;
            }
        }
    }
    int num_tracks = sampling::uniform_int(paras.min_tracks, paras.max_tracks + 1);
    int i_track = 0;
    float p_tr = paras.p_tr;
    while (i_track < num_tracks) {
        observation_collection tr_obs;
        time_stamp tlo, thi;
        bool done = false;
        while (not done) {
            bool started = false;
            tr_obs.clear();
            tlo = sampling::uniform_int(0, 8);
            thi = std::min(sampling::poisson(paras.lambda) + 1 + tlo, time_stamp(END_TIME));
            for (time_stamp t = tlo; t < thi; ++t) {
                if (sampling::uniform_real() < p_tr) {
                    if (not started) {
                        tr_obs.insert(sample_obs(MAX_X, MAX_Y, t));
                        started = true;
                    } else {
                        tr_obs.insert(sample_obs(tr_obs.back(), t, detail::speed_of_light_));
                    }
                }
            }
            done = tr_obs.size() > 1;
        }
        tracks->insert(track(tlo, thi, tr_obs.begin(), tr_obs.end(), 1.0f));
        ++i_track;
    }
    clutter_ptr clutter(new clutter_t(clutter_obs.begin(), clutter_obs.end()));
    partition_ptr_t part(new partition(tracks, clutter, partition::expansion_2d(0.f, 2.f, 0.f, 2.f)));
    if (paras.output.size() > 0) {
        std::ofstream fh(paras.output.c_str());
        io::write_partition_to_json_stream(fh, *part);
    }
    return part;
}

std::deque<std::string> move_pdr_test(const partition_ptr_t& orignal_part_ptr, const std::string& forward_move,
        const int total, test_t& test_results)
{
    std::string backward_move;
    if (forward_move == "Birth") backward_move = "Death";
    if (forward_move == "Death") backward_move = "Birth";
    if (forward_move == "Extend") backward_move = "Reduce";
    if (forward_move == "Reduce") backward_move = "Extend";
    if (forward_move == "Merge") backward_move = "Split";
    if (forward_move == "Split") backward_move = "Merge";
    if (forward_move == "Update") backward_move = "Update";
    if (forward_move == "Transfer") backward_move = "Transfer";
    if (forward_move == "Cross-over") backward_move = "Cross-over";

    time_stamp global_fst = orignal_part_ptr->first_time_stamp();
    time_stamp global_lst = orignal_part_ptr->last_time_stamp();

    dump_not_push messages;
    //std::deque<std::string> messages;
    messages.push_back("Note: the test has two elements:");
    messages.push_back("1) the sampled 'F/B' ratio needs to be the similar to the calculated.");
    messages.push_back("   if increasing the number of samples does not help, then there is something WRONG!");
    messages.push_back("2) F-PDR and B-PDR need to be indentical. The F-PDR + B-PDR is reported, if there is a problem.");

    messages.push_back(boost::str(
        boost::format("original '%s'") % partition_to_string(*orignal_part_ptr)
    ));

    messages.push_back(boost::str(
        boost::format("clutter '%s'") % clutter_to_str(orignal_part_ptr->clutter(), orignal_part_ptr->last_time_stamp())
    ));

    messages.push_back("Pos 0123456789");
    BOOST_FOREACH (const std::string& s, partition_to_string_list(*orignal_part_ptr)) {
        messages.push_back(boost::str( boost::format("   %s") % s ));
    }

    messages.push_back(boost::str(
        boost::format("Forward move (F): '%s', Backward move (B): '%s'") % forward_move % backward_move
    ));

    biggles_move forward(forward_move);
    biggles_move backward(backward_move);

    std::map<std::string, float> forward_freq;
    std::map<std::string, float> move_pdr;
    std::map<std::string, std::set<float> > backward_pdr;
    std::map<std::string, float> undo;
    int fail_count = 0;
    const int back_counts = total;
    bool done = false;
    float count_pdr_add_to_0 = 0.f;
    float reversed = 0;
    //OK(orignal_part_ptr->get_capability_recorder()->transfer_pairs().size());
    for (int i = 0; i < total ; i++) {
        float forth_prob=0.f;
        partition_ptr_t target_part_ptr = orignal_part_ptr;
        done = forward(orignal_part_ptr, target_part_ptr, forth_prob);
        if (not done) ++fail_count;
        else {
            std::string part_str(partition_to_string(*target_part_ptr));
            if (forward_freq.find(part_str) == forward_freq.end()) {
                target_part_ptr->set_first_time_stamp(global_fst);
                target_part_ptr->set_last_time_stamp(global_lst);
                float back_prob=0.f;
                capability_recorder_ptr cap_rec_ptr(new_cap_recorder(*target_part_ptr));
                target_part_ptr->set_capability_recorder(cap_rec_ptr);
                //OK(part_str);
                //OK(cap_rec_ptr->transfer_pairs().size());
                for (int i2 = 0; i2 < back_counts; ++i2) {
                    partition_ptr_t backward_part_ptr = target_part_ptr;
                    done = backward(target_part_ptr, backward_part_ptr, back_prob);
                    if (done) {
                        backward_part_ptr->set_first_time_stamp(global_fst);
                        backward_part_ptr->set_last_time_stamp(global_lst);
                    }
                    if (*orignal_part_ptr == *backward_part_ptr) {
                        undo[part_str]++;
                        count_pdr_add_to_0 += (forth_prob + back_prob == 0.f) ? 1.f : 0.f;
                        backward_pdr[part_str].insert(back_prob);
                        reversed ++;
                    }
                }
            }
            forward_freq[part_str]++;
            move_pdr[part_str] = forth_prob;
        }
    }
    float event_count = 0.f;
    //size_t in_limits = 0;
    std::map<int, int> error_limits;
    bool pdr_test_ok = true;
    for (std::map<std::string, float >::const_iterator it = forward_freq.begin(); it != forward_freq.end(); ++it) {
        std::string forward_partition(it->first);
        float forward_part_freq = it->second;
        const float pdr = move_pdr[forward_partition];
        std::set<float> back_pdrs = backward_pdr[forward_partition];
        const float ratio = expf(-pdr);
        float forward_ratio = forward_part_freq/float(total);
        float backward_part_freq = undo[forward_partition];
        float backward_ratio = backward_part_freq/back_counts;
        float first_back_pdr = 0.f;
        //float error_est = pdr_sample_stdev(forward_part_freq, total, backward_part_freq);
        float forw_prob = forward_part_freq/total;
        float forw_err = binom_prob_error_est(forward_part_freq, total);
        float back_prob = backward_part_freq/back_counts;
        float back_err = binom_prob_error_est(backward_part_freq, back_counts);
        float error_est = error_of_fraction(forw_prob, forw_err, back_prob, back_err);
        if (not back_pdrs.empty())
            first_back_pdr = *back_pdrs.begin();
        float est_diff = fabsf(ratio - (forward_ratio/backward_ratio));
        if (error_est > 0)
            error_limits[int(est_diff/ error_est)]++;
        else if (est_diff == 0)
            error_limits[0]++;
        else
            error_limits[9]++;
        if (fabsf(pdr + first_back_pdr) < 5.e-6) {
            messages.push_back(boost::str(
                boost::format("'%s', F = %6.2f%%, B|F = %6.2f%%, F/B = %.2f +/- %.2f (calc: %.2f), F-PDR = %.2f")
                % forward_partition % (100.f * forward_ratio) % (100.f * backward_ratio) % (forward_ratio/backward_ratio)
                % error_est % ratio % pdr
            ));
        } else {
            messages.push_back(boost::str(
                boost::format("'%s', F = %6.2f%%, B|F = %6.2f%%, F/B = %.2f +/- %.2f (calc: %.2f), F+B = %.2f + %.2f = %g")
                % forward_partition % (100.f * forward_ratio) % (100.f * backward_ratio) % (forward_ratio/backward_ratio)
                % error_est % ratio % pdr % first_back_pdr % (pdr+first_back_pdr)
            ));
            pdr_test_ok = false;
        }
        event_count += forward_part_freq;
        if (back_pdrs.size() > 1) {
            messages.push_back(boost::str(boost::format("  num B-PDR = %d") % back_pdrs.size()));
            BOOST_FOREACH( const float& pdr, back_pdrs) {
                messages.push_back(boost::str(boost::format("  B-PDR = %.2f") % pdr));
            }
        }
    }
    messages.push_back(boost::str(
        boost::format("failed, Freq = %.2f%%%%") % (float(fail_count)/float(total)*100.f)
    ));
    messages.push_back(boost::str(
        boost::format("reversed, Freq = %.2f%%%%") % (reversed/float(total)*100.f)
    ));
    messages.push_back(boost::str(
        boost::format("fail-count %d, event-count %d, sum = %d") % fail_count % int(event_count) % (fail_count + int(event_count))
    ));
    messages.push_back(boost::str(
        boost::format("count_pdr_add_to_0/reversed =  %.2f") % (count_pdr_add_to_0/reversed)
    ));
    for (std::map<int, int>::iterator it = error_limits.begin(); it != error_limits.end(); ++it) {
        messages.push_back(boost::str(
            boost::format("%d/%d (%6.2f%%) samples are in %d sigma range of calculated ratio")
                % (it->second) % forward_freq.size()
                % ( 100.f * float(it->second)/ float(forward_freq.size()))
                % (it->first + 1)
        ));
    }
    test_results["fail count + event count == total"] = float(fail_count) + event_count == float(total);
    test_results["All PDR tests passed"] = pdr_test_ok;
    //test_results["count of PDRs that add to 0 == undone moves"] = count_pdr_add_to_0 == reversed;
    return std::deque<std::string>();
    //return messages;
}

std::deque<std::string> move_pdr_test_adv(const partition_ptr_t& orignal_part_ptr, const std::string& forward_move,
        const int total, test_t& test_results)
{
    std::string backward_move;
    if (forward_move == "Birth") backward_move = "Death";
    if (forward_move == "Death") backward_move = "Birth";
    if (forward_move == "Extend") backward_move = "Reduce";
    if (forward_move == "Reduce") backward_move = "Extend";
    if (forward_move == "Merge") backward_move = "Split";
    if (forward_move == "Split") backward_move = "Merge";
    if (forward_move == "Update") backward_move = "Update";
    if (forward_move == "Transfer") backward_move = "Transfer";
    if (forward_move == "Cross-over") backward_move = "Cross-over";

    time_stamp global_fst = orignal_part_ptr->first_time_stamp();
    time_stamp global_lst = orignal_part_ptr->last_time_stamp();

    dump_not_push messages;
    //std::deque<std::string> messages;
    messages.push_back("Note: the test has two elements:");
    messages.push_back("1) the sampled 'F/B' ratio needs to be the similar to the calculated.");
    messages.push_back("   if increasing the number of samples does not help, then there is something WRONG!");
    messages.push_back("2) F-PDR and B-PDR need to be indentical. The F-PDR + B-PDR is reported, if there is a problem.");

    messages.push_back(boost::str(
        boost::format("original '%s'") % (orignal_part_ptr->as_string())
    ));

    messages.push_back(boost::str(
        boost::format("clutter '%s'") % clutter_to_str(orignal_part_ptr->clutter(), orignal_part_ptr->last_time_stamp())
    ));

    /*
    messages.push_back("Pos 0123456789");
    BOOST_FOREACH (const std::string& s, partition_to_string_list(*orignal_part_ptr)) {
        messages.push_back(boost::str( boost::format("   %s") % s ));
    }
    */

    messages.push_back(boost::str(
        boost::format("Forward move (F): '%s', Backward move (B): '%s'") % forward_move % backward_move
    ));

    biggles_move forward(forward_move);
    biggles_move backward(backward_move);

    std::map<std::string, float> forward_freq;
    std::map<std::string, float> move_pdr;
    std::map<std::string, std::set<float> > backward_pdr;
    std::map<std::string, float> undo;
    int fail_count = 0;
    const int back_counts = total;
    bool done = false;
    float count_pdr_add_to_0 = 0.f;
    float reversed = 0;
    //OK(orignal_part_ptr->get_capability_recorder()->transfer_pairs().size());
    for (int i = 0; i < total ; i++) {
        float forth_prob=0.f;
        partition_ptr_t target_part_ptr = orignal_part_ptr;
        done = forward(orignal_part_ptr, target_part_ptr, forth_prob);
        if (not done) ++fail_count;
        else {
            std::string part_str(target_part_ptr->as_string());
            if (forward_freq.find(part_str) == forward_freq.end()) {
                target_part_ptr->set_first_time_stamp(global_fst);
                target_part_ptr->set_last_time_stamp(global_lst);
                float back_prob=0.f;
                capability_recorder_ptr cap_rec_ptr(new_cap_recorder(*target_part_ptr));
                target_part_ptr->set_capability_recorder(cap_rec_ptr);
                //OK(part_str);
                //OK(cap_rec_ptr->transfer_pairs().size());
                boost::timer timer;
                for (int i2 = 0; i2 < back_counts; ++i2) {
                    partition_ptr_t backward_part_ptr = target_part_ptr;
                    timer.restart();
                    done = backward(target_part_ptr, backward_part_ptr, back_prob);
                    if (done) {
                        backward_part_ptr->set_first_time_stamp(global_fst);
                        backward_part_ptr->set_last_time_stamp(global_lst);
                    }
                    if (*orignal_part_ptr == *backward_part_ptr) {
                        undo[part_str]++;
                        count_pdr_add_to_0 += (forth_prob + back_prob == 0.f) ? 1.f : 0.f;
                        backward_pdr[part_str].insert(back_prob);
                        reversed ++;
                    }
                }
            }
            forward_freq[part_str]++;
            move_pdr[part_str] = forth_prob;
        }
    }
    float event_count = 0.f;
    //size_t in_limits = 0;
    std::map<int, int> error_limits;
    bool pdr_test_ok = true;
    for (std::map<std::string, float >::const_iterator it = forward_freq.begin(); it != forward_freq.end(); ++it) {
        std::string forward_partition(it->first);
        float forward_part_freq = it->second;
        const float pdr = move_pdr[forward_partition];
        std::set<float> back_pdrs = backward_pdr[forward_partition];
        const float ratio = expf(-pdr);
        float forward_ratio = forward_part_freq/float(total);
        float backward_part_freq = undo[forward_partition];
        float backward_ratio = backward_part_freq/back_counts;
        float first_back_pdr = 0.f;
        //float error_est = pdr_sample_stdev(forward_part_freq, total, backward_part_freq);
        float forw_prob = forward_part_freq/total;
        float forw_err = binom_prob_error_est(forward_part_freq, total);
        float back_prob = backward_part_freq/back_counts;
        float back_err = binom_prob_error_est(backward_part_freq, back_counts);
        float error_est = error_of_fraction(forw_prob, forw_err, back_prob, back_err);
        if (not back_pdrs.empty())
            first_back_pdr = *back_pdrs.begin();
        float est_diff = fabsf(ratio - (forward_ratio/backward_ratio));
        if (error_est > 0)
            error_limits[int(est_diff/ error_est)]++;
        else if (est_diff == 0)
            error_limits[0]++;
        else
            error_limits[9]++;
        if (fabsf(pdr + first_back_pdr) < 5.e-6) {
            messages.push_back(boost::str(
                boost::format("'%s', F = %6.2f%%, B|F = %6.2f%%, F/B = %.2f +/- %.2f (calc: %.2f), F-PDR = %.2f")
                % forward_partition % (100.f * forward_ratio) % (100.f * backward_ratio) % (forward_ratio/backward_ratio)
                % error_est % ratio % pdr
            ));
        } else {
            messages.push_back(boost::str(
                boost::format("'%s', F = %6.2f%%, B|F = %6.2f%%, F/B = %.2f +/- %.2f (calc: %.2f), F+B = %.2f + %.2f = %g")
                % forward_partition % (100.f * forward_ratio) % (100.f * backward_ratio) % (forward_ratio/backward_ratio)
                % error_est % ratio % pdr % first_back_pdr % (pdr+first_back_pdr)
            ));
            pdr_test_ok = false;
        }
        event_count += forward_part_freq;
        if (back_pdrs.size() > 1) {
            messages.push_back(boost::str(boost::format("  num B-PDR = %d") % back_pdrs.size()));
            BOOST_FOREACH( const float& pdr, back_pdrs) {
                messages.push_back(boost::str(boost::format("  B-PDR = %.2f") % pdr));
            }
        }
    }
    messages.push_back(boost::str(
        boost::format("failed, Freq = %.2f%%%%") % (float(fail_count)/float(total)*100.f)
    ));
    messages.push_back(boost::str(
        boost::format("reversed, Freq = %.2f%%%%") % (reversed/float(total)*100.f)
    ));
    messages.push_back(boost::str(
        boost::format("fail-count %d, event-count %d, sum = %d") % fail_count % int(event_count) % (fail_count + int(event_count))
    ));
    messages.push_back(boost::str(
        boost::format("count_pdr_add_to_0/reversed =  %.2f") % (count_pdr_add_to_0/reversed)
    ));
    int max_std = 0;
    for (std::map<int, int>::iterator it = error_limits.begin(); it != error_limits.end(); ++it) {
        max_std = std::max(max_std, it->first);
        messages.push_back(boost::str(
            boost::format("%d/%d (%6.2f%%) samples are in %d sigma range of calculated ratio")
                % (it->second) % forward_freq.size()
                % ( 100.f * float(it->second)/ float(forward_freq.size()))
                % (it->first + 1)
        ));
    }
    test_results["fail count + event count == total"] = float(fail_count) + event_count == float(total);
    test_results["All PDR tests passed"] = pdr_test_ok;
    //test_results["Sampled F/B is similar to calculated F/B"] = max_std < 8;
    //test_results["count of PDRs that add to 0 == undone moves"] = count_pdr_add_to_0 == reversed;
    return std::deque<std::string>();
    //return messages;
}

namespace biggles { namespace sampling {
    float sample_beta(float alpha, float beta) {
        typedef boost::gamma_distribution<float> dist_t;
        dist_t dist_x(alpha);
        dist_t dist_y(beta);
        boost::variate_generator<boost::mt19937&, dist_t > sample_x(detail::random_generator, dist_x);
        boost::variate_generator<boost::mt19937&, dist_t > sample_y(detail::random_generator, dist_y);
        float x(sample_x());
        float y(sample_y());
        return x/(x+y);
    }
}}


#include <iostream>
#include <vector>

#include <boost/scope_exit.hpp>

#include <amgcl/backend/builtin.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/mpi/distributed_matrix.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

int main(int argc, char *argv[]) {
    MPI_Init(&argc, &argv);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator comm(MPI_COMM_WORLD);

    int n = 16;
    int chunk_len = (n + comm.size - 1) / comm.size;
    int chunk_beg = std::min(n, chunk_len * comm.rank);
    int chunk_end = std::min(n, chunk_len * (comm.rank + 1));
    int chunk = chunk_end - chunk_beg;

    std::vector<int>    ptr; ptr.reserve(chunk + 1); ptr.push_back(0);
    std::vector<int>    col; col.reserve(chunk * 4);
    std::vector<double> val; val.reserve(chunk * 4);

    for(int i = 0, j = chunk_beg; i < chunk; ++i, ++j) {
        if (j > 0) {
            col.push_back(j - 1);
            val.push_back(-1);
        }

        col.push_back(j);
        val.push_back(2);

        if (j+1 < n) {
            col.push_back(j+1);
            val.push_back(-1);
        }

        if (j+5 < n) {
            col.push_back(j+5);
            val.push_back(-0.1);
        }

        ptr.push_back(col.size());
    }

    typedef amgcl::backend::builtin<double> Backend;
    typedef amgcl::mpi::distributed_matrix<Backend> Matrix;

    Matrix A(comm, std::tie(chunk, ptr, col, val), chunk);

    {
        std::ostringstream fname;
        fname << "A_loc_" << comm.rank << ".mtx";
        amgcl::io::mm_write(fname.str(), *A.local());
    }

    {
        std::ostringstream fname;
        fname << "A_rem_" << comm.rank << ".mtx";
        amgcl::io::mm_write(fname.str(), *A.remote());
    }

    auto B = transpose(A);

    {
        std::ostringstream fname;
        fname << "B_loc_" << comm.rank << ".mtx";
        amgcl::io::mm_write(fname.str(), *B->local());
    }

    {
        std::ostringstream fname;
        fname << "B_rem_" << comm.rank << ".mtx";
        amgcl::io::mm_write(fname.str(), *B->remote());
    }

}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//

#include <boost/test/unit_test.hpp>
#include "armnnCaffeParser/ICaffeParser.hpp"
#include "ParserPrototxtFixture.hpp"

BOOST_AUTO_TEST_SUITE(CaffeParser)

struct DropoutFixture : public ParserPrototxtFixture<armnnCaffeParser::ICaffeParser>
{
    DropoutFixture()
    {
        m_Prototext = "name: \"DropoutFixture\"\n"
            "layer {\n"
            "    name: \"data\"\n"
            "    type: \"Input\"\n"
            "    top: \"data\"\n"
            "    input_param { shape: { dim: 1 dim: 1 dim: 2 dim: 2 } }\n"
            "}\n"
            "layer {\n"
            "    bottom: \"data\"\n"
            "    top: \"drop1\"\n"
            "    name: \"drop1\"\n"
            "    type: \"Dropout\"\n"
            "}\n"
            "layer {\n"
            "    bottom: \"drop1\"\n"
            "    bottom: \"drop1\"\n"
            "    top: \"add\"\n"
            "    name: \"add\"\n"
            "    type: \"Eltwise\"\n"
            "}\n";
        SetupSingleInputSingleOutput("data", "add");
    }
};

BOOST_FIXTURE_TEST_CASE(ParseDropout, DropoutFixture)
{
    RunTest<4>(
        {
            1, 2,
            3, 4,
        },
        {
            2, 4,
            6, 8
        });
}

BOOST_AUTO_TEST_SUITE_END()


#include <Oceaniolib/Oceanio.hpp>
#include <Oceaniolib/print.hpp>
#include <Oceaniolib/singleton.hpp>
#include <Oceaniolib/crypto.h>
#include <boost/algorithm/string.hpp>

using namespace Oceanio;
using namespace std;

typedef uint128_t key_n_t;

class hello : public Oceanio::contract {
    public:
        using contract::contract;

        hello( account_name self )
        : contract(self)
        , _keyval(_self, _self)
        , counter(28)
        {}

        int counter;

        // @abi table
        struct keyval {
            uint64_t id;
            key_n_t key_n;
            string key_s;
            string val;

            uint64_t primary_key() const { return id; }
            key_n_t get_key_n() const { return key_n; }

            OceanLIB_SERIALIZE( keyval, (id)(key_n)(key_s)(val) )
        };

        string u64_to_string(name value)const {
            static const char* charmap = ".12345abcdefghijklmnopqrstuvwxyz";

            string str(13,'.');

            uint64_t tmp = value;
            for( uint32_t i = 0; i <= 12; ++i ) {
                char c = charmap[tmp & (i == 0 ? 0x0f : 0x1f)];
                str[12-i] = c;
                tmp >>= (i == 0 ? 4 : 5);
            }

            boost::algorithm::trim_right_if( str, []( char c ){ return c == '.'; } );
            return str;
        }

        static constexpr uint64_t string_to_u64( const char* str ) {

            uint32_t len = 0;
            while( str[len] ) ++len;

            uint64_t value = 0;

            for( uint32_t i = 0; i <= 12; ++i ) {
                uint64_t c = 0;
                if( i < len && i <= 12 ) c = uint64_t(char_to_symbol( str[i] ));

                if( i < 12 ) {
                    c &= 0x1f;
                    c <<= 64-5*(i+1);
                }
                else {
                    c &= 0x0f;
                }

                value |= c;
            }

            return value;
        }

        static const int maxl = 25;
        static const int maxb = 128;

        static constexpr uint128_t string_to_u128( const char* str ) {

            uint32_t len = 0;
            while( str[len] ) ++len;

            uint128_t value = 0;

            for( uint32_t i = 0; i <= maxl; ++i ) {
                uint128_t c = 0;
                if( i < len && i <= maxl ) c = uint64_t(char_to_symbol( str[i] ));

                if( i < maxl ) {
                    c &= 0x1f;
                    c <<= maxb-5*(i+1);
                }
                else {
                    c &= 0x07;
                }

                value |= c;
            }

            return value;
        }

        static string u128_to_string(uint128_t  value) {
            static const char* charmap = ".12345abcdefghijklmnopqrstuvwxyz";

            string str(maxl+1,'.');

            uint128_t tmp = value;
            for( uint32_t i = 0; i <= maxl; ++i ) {
                char c = charmap[tmp & (i == 0 ? 0x07 : 0x1f)];
                str[maxl-i] = c;
                tmp >>= (i == 0 ? 3 : 5);
            }

            boost::trim_right_if( str, []( char c ){ return c == '.'; } );
            return str;
        }

        /// @abi action
        void addprm( name user, string key, string val, uint64_t id ) {
            print( "add key val for user by primary idx, user = ", user, ": ", getCounter());

            Oceanio::multi_index<N(keyval), keyval
            , indexed_by< N(by_key_n),   const_mem_fun<keyval, key_n_t, &keyval::get_key_n> >
            > tbl(_self, user);

            auto primary_idx = tbl.find(id);
            if (primary_idx!=tbl.end())
            {
                print("key already found = ", key.c_str(), "id= ", primary_idx->id);

                tbl.modify( primary_idx, user, [&]( auto& row) {
                    // row.key_n = string_to_u64(key.c_str());
                    row.key_n = string_to_u128(key.c_str());
                    row.key_s = key;
                    row.val = val;
                });

            }
            // get here when need to insert
            else
            {
                print("adding new key  ", key.c_str());
                tbl.emplace( user, [&]( auto& row) {
                    row.id = incCounter();
                    // row.key_n = string_to_u64(key.c_str());
                    row.key_n = string_to_u128(key.c_str());
                    row.key_s = key;
                    row.val = val;
                });
            }
        }

        /// @abi action
        void addsec( name user, string key, string val ) {
            print( "add key val for user by secondary idx (key), user= ", user, "\n");

            Oceanio::multi_index<N(keyval), keyval
            , indexed_by< N(by_key_n),   const_mem_fun<keyval, key_n_t, &keyval::get_key_n> >
            > tbl(_self, user);
            // > tbl(_self, _self /*user*/);

            auto keyn_idx = tbl.get_index<N(by_key_n)>();
            // auto it = keyn_idx.find(string_to_u64(key.c_str()));
            auto it = keyn_idx.find(string_to_u128(key.c_str()));
            if (it != keyn_idx.end())
            {
                print("key already found = ", key.c_str(), " id= ", it->id);
                auto pkidx = tbl.find(it->id);

                tbl.modify( pkidx, user, [&]( auto& row) {
                    row.val = val;
                });
            }
            else
            {
                print("adding new key  ", key.c_str());
                tbl.emplace( user, [&]( auto& row) {
                    row.id = incCounter();
                    // row.key_n = string_to_u64(key.c_str());
                    row.key_n = string_to_u128(key.c_str());
                    row.key_s = key;
                    row.val = val;
                });
            }
        }

        /// @abi action
        void remove( name user, uint64_t id ) {
            print("got remove cmd: ", user, "id=  ", id);
            auto it = db_find_i64(_self, /*_self*/user, N(keyval), id);
            if (it >= 0) {
                db_remove_i64(it);
            }
        }

    private:

        multi_index<N(keyval), keyval> _keyval;

        singleton<N(hello3), N(g_counter), N(hello3), uint64_t>  g_counter;

        uint64_t incCounter() {
            uint64_t next = g_counter.exists() ? g_counter.get()+1 : 0;
            g_counter.set(next);
            return next;
        }

        uint64_t getCounter() {
            uint64_t cur = g_counter.exists() ? g_counter.get() : 0;
            return cur;
        }

};

OceanIO_ABI( hello, (remove)(addprm)(addsec) )


// Copyright 2019. All rights reserved.
// Institute of Measurement and Control Systems
// Karlsruhe Institute of Technology, Germany
//
// authors:
//  Johannes Graeter (johannes.graeter@posteo.de)
//  Johannes Beck (johannes.beck@kit.edu)

#pragma once
#include <exception>
#include <iostream>
#include <type_traits>
#include <Eigen/Dense>
#include <internal/coordinate_system_storage.hpp>
#include <internal/tf_traits.hpp>

namespace ct {

// ///////////////// Template declarations.
template <typename ToCoord, typename FromCoord, typename DataType>
class Transform;

template <typename C, typename DataType>
class Point;

// ///////////////// Namespace for all non-trait conversions.
namespace conversion {
/**
 * @brief toDynamic, wraps up traits to cast a transform (static, partially static or dynamic) to dynamic.
 * @todo Auto only returns by value. If toDynamic is called for already purely dynamic transforms, the copy is
 * unnecessary.
 * @param t, transform to cast (static, partially static or dynamic).
 * @return casted dynamic transform
 */
template <typename To, typename From, typename DataType>
static auto toDynamic(const Transform<To, From, DataType>& t) {
    auto coordTo = TfTrait<To>::toDynamic(t.getToCoord());
    auto coordFrom = TfTrait<From>::toDynamic(t.getFromCoord());
    return Transform<decltype(coordTo), decltype(coordFrom), DataType>(t.getData(), coordTo, coordFrom);
}

/**
 * @brief toDynamic, wraps up traits to cast a Point (static dynamic) to dynamic.
 * @todo Auto only returns by value. If toDynamic is called for already purely dynamic points, the copy is
 * unnecessary.
 * @param t, Point to cast (static or dynamic).
 * @return casted dynamic Point
 */
template <typename T, typename DataType>
static auto toDynamic(const Point<T, DataType>& p) {
    auto coord = TfTrait<T>::toDynamic(p.getCoord());
    return Point<decltype(coord), DataType>(p.getData(), coord);
}
} // namespace conversion

template <typename C, typename DataType>
class Point : public CoordinateSystemStorage<TfTrait<C>::IsStatic, CoordinateTag::Other, C> {
public:
    // Specify Eigen Alignment in case DataType is Eigen type.
    // This should be obsolete with c++17.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
    explicit Point(DataType data) : data_(std::move(data)) {
        static_assert(TfTrait<C>::IsStatic,
                      "Error in save_coordinate_transform: Point has no coordinate system given, but is dynamic.");
    }

    Point(DataType data, const C& coordinateSystem)
            : CoordinateSystemStorage<TfTrait<C>::IsStatic, CoordinateTag::Other, C>(coordinateSystem),
              data_(std::move(data)) {
    }

    const DataType& getData() const {
        return data_;
    }

    /**
     * @brief getCoord, Getter for coordinate system.
     * Cannot be const C& since coordinate system is a temporary object if coordinate system is static.
     */
    constexpr C getCoord() const {
        return CoordinateSystemStorage<TfTrait<C>::IsStatic, CoordinateTag::Other, C>::get();
    }

private:
    DataType data_; ///> Data of point.
};

/**
 * @brief Typedef for a set of points, operator* of transforms is specialized so that format is compatible to
 * Eigen::Isometry. Size can also be Eigen::Dynamic.
 */
template <typename C, typename ScalarType, int Size>
using Points = Point<C, Eigen::Matrix<ScalarType, 3, Size>>;

/**
 * @brief The TransformException struct
 * Excpetion thrown if coordinate systems do not match.
 */
struct TransformException : public std::exception {
    const char* what() const noexcept override {
        return "Error in coordinate_save: Coordinate systems do not match!";
    }
};

/**
 * @brief The Transform class
 * Use this class to assign coordinate systems to trasnsformations.
 * Depending on the type of coordinate system, the correct concatenation of transforms is checked at compile- or
 * runtime.
 * The type of the transform data (DataType) is flexible, however it must implemented operator* since it is used for
 * concatenation.
 * For usage examples look at unittests.
 */
template <typename ToCoord, typename FromCoord, typename DataType>
class Transform : public CoordinateSystemStorage<TfTrait<ToCoord>::IsStatic, CoordinateTag::To, ToCoord>,
                  CoordinateSystemStorage<TfTrait<FromCoord>::IsStatic, CoordinateTag::From, FromCoord> {
public:
    // Specify Eigen Alignment in case DataType is Eigen type.
    // This should be obsolete with c++17.
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
    /**
     * @brief Transform
     * @param data transform data.
     */
    explicit Transform(const DataType& data) : data_(data) {
        static_assert(TfTrait<FromCoord>::IsStatic && TfTrait<ToCoord>::IsStatic,
                      "Error in save_coordinate_transform: Transform has no coordinate system given, but has dynamic "
                      "coordinate systems.");
    }

    /**
     * @brief Transform
     * @param data transform data.
     * @param toCoord coordinate system to transform to.
     * @param fromCoord coordinate system to trasnform from.
     */
    Transform(const DataType& data, const ToCoord& toCoord, const FromCoord& fromCoord)
            : CoordinateSystemStorage<TfTrait<ToCoord>::IsStatic, CoordinateTag::To, ToCoord>(toCoord),
              CoordinateSystemStorage<TfTrait<FromCoord>::IsStatic, CoordinateTag::From, FromCoord>(fromCoord),
              data_(data) {
    }

    /**
     * @brief operator* Concatenate two transforms.
     * @param otherT Transform to concatenate.
     * @return Concatenated transform.
     */
    template <typename OtherTo, typename OtherFrom>
    Transform<ToCoord, OtherFrom, DataType> operator*(const Transform<OtherTo, OtherFrom, DataType>& otherT) const {
        // Test at compile time of coordinate systems are correct.
        // For dynamic systems of the same type this is always true.
        if constexpr (TfTrait<FromCoord>::IsStatic && TfTrait<OtherTo>::IsStatic) {
            static_assert(TfTrait<FromCoord>::isSame(otherT.getToCoord()));
        } else {
            // Check at runtime if coordinate systems are correct.
            if (!TfTrait<FromCoord>::isSame(getFromCoord(), otherT.getToCoord())) {
                throw TransformException();
            }
        }
        return Transform<ToCoord, OtherFrom, DataType>(
            this->getData() * otherT.getData(), getToCoord(), otherT.getFromCoord());
    }

    /**
     * @brief operator*= Concatenation for transforms with same dynamic type or static convertible type.
     * @param rhs Transform to concatenate.
     * @return Concatenated transform.
     */
    template <typename OtherTo, typename OtherFrom>
    Transform<ToCoord, FromCoord, DataType>& operator*=(const Transform<OtherTo, OtherFrom, DataType>& rhs) {
        // This only works for dynamic types -> convert.
        auto rhsDyn = conversion::toDynamic(rhs);

        assert(TfTrait<ToCoord>::isSame(getToCoord(), rhsDyn.getFromCoord()));
        static_assert(!TfTrait<ToCoord>::IsStatic, "In Transform: Concatenation only valid if lhs is dynamic.");
        static_assert(!TfTrait<FromCoord>::IsStatic, "In Transform: Concatenation only valid if lhs is dynamic.");
        static_assert(
            std::is_same<decltype(rhsDyn.getFromCoord()), FromCoord>::value,
            "In Transform: Concatenation only if FromCoord of rhs has same or castable type to FromCoord of lhs.");

        *this = rhsDyn * (*this);
        return *this;
    }

    /**
     * @brief operator* Transform Point to ToCoord.
     * @param point Point to transform. PointDataType must be compatible with DataType.
     * @return Transformed point.
     */
    template <typename OtherFrom, typename PointDataType>
    Point<ToCoord, PointDataType> operator*(const Point<OtherFrom, PointDataType>& point) const {
        // Test at compile time of coordinate systems are correct.
        // For dynamic systems of the same type this is always true.
        if constexpr (TfTrait<FromCoord>::IsStatic && TfTrait<OtherFrom>::IsStatic) {
            static_assert(TfTrait<FromCoord>::isSame(point.getCoord()));
        } else {
            // Test dynamic system.
            if (!TfTrait<FromCoord>::isSame(getFromCoord(), point.getCoord())) {
                throw TransformException();
            }
        }
        return Point<ToCoord, PointDataType>(this->getData() * point.getData(), getToCoord());
    }

    /**
     * @brief operator* Overload for points to bring them in correct format for multipliying.
     * @param points Set of points to transform. Internally represented as Eigen::Matrix. Size can be Eigen::Dynamic.
     * @return Transformed points.
     */
    template <typename OtherFrom, typename ScalarType, int Size>
    Points<ToCoord, ScalarType, Size> operator*(const Points<OtherFrom, ScalarType, Size>& points) const {
        // Test at compile time of coordinate systems are correct.
        // For dynamic systems of the same type this is always true.
        if constexpr (TfTrait<FromCoord>::IsStatic && TfTrait<OtherFrom>::IsStatic) {
            static_assert(TfTrait<FromCoord>::isSame(points.getCoord()));
        } else {
            // Test dynamic system.
            if (!TfTrait<FromCoord>::isSame(getFromCoord(), points.getCoord())) {
                throw TransformException();
            }
        }
        return Points<ToCoord, ScalarType, Size>(this->getData() * points.getData().colwise().homogeneous(),
                                                 getToCoord());
    }

    /**
     * @brief getData Getter for data.
     * @return Data without coordinate systems.
     */
    const DataType& getData() const {
        return data_;
    }

    /**
     * @brief getToCoord Getter for coordinate system.
     * Cannot be const C& since coordinate system is a temporary object if coordinate system is static.
     * @return coordinate system
     */
    constexpr ToCoord getToCoord() const {
        return CoordinateSystemStorage<TfTrait<ToCoord>::IsStatic, CoordinateTag::To, ToCoord>::get();
    }

    /**
     * @brief getFromCoord Getter for coordinate system.
     * Cannot be const C& since coordinate system is a temporary object if coordinate system is static.
     * @return coordinate system
     */
    constexpr FromCoord getFromCoord() const {
        return CoordinateSystemStorage<TfTrait<FromCoord>::IsStatic, CoordinateTag::From, FromCoord>::get();
    }

private:
    DataType data_; ///> Data of transform.
};
} // namespace ct


/// @file  triangle.hpp
/// @brief Declarations for methods based on triangle constraints

#pragma once
#ifndef OGT_EMBED_TRIANGLE_HPP
#define OGT_EMBED_TRIANGLE_HPP

#include <ogt/config.hpp>
#include <ogt/core/collection.hpp>
#include <ogt/embed/embed.hpp>
#include <Eigen/Dense>
#include <iostream>
#include <json.hpp>
#include <memory>
#include <vector>

using OGT_NAMESPACE::core::Collection;
using OGT_NAMESPACE::core::Oracle;
using OGT_NAMESPACE::core::Traversal;

namespace OGT_NAMESPACE {
namespace embed {

struct CmpConstraint; // Forward declaration

/// A triangle with two edges' lengths constrained to be scaled to within some
/// interval of the length of the third edge.
struct TriangleConstraint {

	/// Initializing constructor
	TriangleConstraint(size_t p, size_t x, size_t y, double xmin,
		double xmax, double ymin, double ymax)
		: p(p), x(x), y(y), xmin(xmin), xmax(xmax), ymin(ymin), ymax(ymax)
	{
	}

	/// Initialize from a vector, e.g. loaded from .csv
	TriangleConstraint(const Eigen::VectorXd& vec)
		: p(vec(0)), x(vec(1)), y(vec(2)), xmin(vec(3)), xmax(vec(4))
		, ymin(vec(5)), ymax(vec(6))
	{
	}

	/// Elementwise equality test
	bool operator==(const TriangleConstraint& other) const {
		return p == other.p && x == other.x && y == other.y
			&& xmin == other.xmin && xmax == other.xmax && ymin == other.ymin
			&& ymax == other.ymax;
	}

	/// Tighten the constraint using the triangle inequality.
	///
	/// We can update xmin and ymin as follows.
	/// ~~~~
	/// xy <= px + py <= px + xy * ymax
	///    --> px >= xy(1 - ymax)
	///    --> px >= xy * max(xmin, 1 - ymax)
	///    --> py >= xy * max(ymin, 1 - xmax) (by symmetry)
	/// ~~~~
	void tighten();

	/// Rotate the constraint to use a different edge as the reference distance.
	/// If toX is true, the output's p will be the input's x point. Otherwise,
	/// the output's p will be the input's y point.
	///
	/// The update rule is derived as follows.
	/// ~~~~
	/// xmin <= px/xy <= xmax
	///     --> 1/xmax <= xy/px <= 1/xmin
	/// py <= ymax * xy
	///    <= ymax * px / xmin
	///    --> ymin / xmax <= py/px <= ymax / xmin
	/// ~~~~
	TriangleConstraint rotate(bool toX, double maxRatio = INFINITY) const;

	/// The point incident to the two constrained edges.
	size_t p;

	/// One of the points incident to the reference edge.
	size_t x;

	/// One of the points incident to the reference edge.
	size_t y;

	/// A lower bound on the ratio dist(p,x) / dist(x,y)
	double xmin;

	/// An upper bound on the ratio dist(p,x) / dist(x,y)
	double xmax;

	/// A lower bound on the ratio dist(p,y) / dist(x,y)
	double ymin;

	/// An upper bound on the ratio dist(p,y) / dist(x,y)
	double ymax;
};

/// Manages distance intervals from a set of constraints.
struct DistIntervals {

	/// Initialize to NaN matrices.
	DistIntervals(size_t nObj);

	/// Set the specified distance and interval width
	void set(Eigen::Index ii, Eigen::Index jj, double dmin, double dmax);

	/// Set the appropriate entries from a constraint, if the reference distance
	/// is already set. Returns true if successful.
	bool setFromCon(const TriangleConstraint& con);

	/// The center of each interval. Can be used as distance estimates.
	/// Uninitialized elements have the value NaN.
	Eigen::MatrixXd D;

	/// The width of each interval.
	/// Uninitialized elements have the value NaN.
	Eigen::MatrixXd W;
};

/// Stream a constraint to output
std::ostream& operator<<(std::ostream& os, const TriangleConstraint& con);

/// Sorts constraints into the order needed for embedTrianglesWithSpheres().
/// Throws std::invalid_argument on failure.
void sortTriangleConstraints(std::vector<TriangleConstraint>& cons, bool prune);

/// Validate the triangle constraints to determine whether they fully specify
/// an embedding in some dimensionality.
bool validateTriangleConstraints(const std::vector<TriangleConstraint>& cons,
	nlohmann::json* json);

/// The algorithm to use when selecting reference edges for triangle constraints
enum EdgeMethod {

	/// Pair farthest edges together
	EdgeFarthest,

	/// Select edges by density
	EdgeByDensity,

	/// Select all edges
	EdgeByAll,

	/// Recurse into local regions based on density rules
	EdgeByLocality
};

/// Find triangular constraints for a collection, using some traversal order to
/// identify reference edges. The first point on an edge will be selected by the
/// order, and the second point will be the farthest point from it.
/// If visitBoth = true, the second point will be visited for each edge.
/// This is useful for Collection::frftTraversal() and
/// Collection::stopWhenAffDep(), but not for Collection::stopAfter().
std::vector<TriangleConstraint> findTriangleConstraints(
	std::shared_ptr<Collection> coll, Traversal order, size_t nDim,
	bool visitBoth, bool basicIntervals, EdgeMethod edges);

/// Find triangular constraints for a collection with reference to some pair.
std::vector<TriangleConstraint> findTriangleConstraints(
	std::shared_ptr<Collection> coll, size_t pt1, size_t pt2,
	bool basicIntervals);

/// Find triangular constraints using the specified distance matrix.
/// The constraints will be the correct distances plus/minus eps.
/// Useful for testing.
std::vector<TriangleConstraint> findTriangleConstraints(
	const Eigen::MatrixXd& dists, double eps, size_t nRef);

/// Update triangle constraints using distances from an embedding of a subset
/// of the objects.
std::vector<TriangleConstraint> updateTriangleConstraints(
	const std::vector<TriangleConstraint>& inCons,
	std::shared_ptr<Oracle> ranker, const Eigen::MatrixXd& pos,
	const Eigen::VectorXd& objects);

/// Load triangle constraints from a data file.
std::vector<TriangleConstraint> loadTriangleConstraints(std::string path);

/// Count the number of violated triangle constraints.
size_t numBadTriangles(const std::vector<TriangleConstraint>& cons,
	const Eigen::MatrixXd& dists);

/// Calculate the loss for a single constraint
double triangleLoss(const TriangleConstraint& con,
	const Eigen::MatrixXd& dists);

/// Calculate the loss for a set of triangle constraints and distances.
template<class Container>
double triangleLoss(const Container& cons, const Eigen::MatrixXd& dists) {
	double loss = 0;
	for (const TriangleConstraint& con : cons) {
		loss += triangleLoss(con, dists);
	}
	return loss;
}

/// Calculate the loss for a set of triangle constraints and distances.
template<class ForwardIterator>
double triangleLoss(ForwardIterator begin, ForwardIterator end,
	const Eigen::MatrixXd& dists) {
	double loss = 0;
	for (auto it = begin; it != end; it++) {
		loss += triangleLoss(*it, dists);
	}
	return loss;
}

/// Sorts constraints into the order needed for embedTrianglesWithSpheres()
/// and embedTrianglesWithShells().
/// Throws std::invalid_argument on failure.
void sortConstraints(std::vector<TriangleConstraint>& cons, bool addRotations,
	double maxRotationRatio);

/// Reduce constraint bounds based on embedded coordinates.
/// Used for embedTrianglesWithSpheres() and embedTrianglesWithShells().
std::vector<TriangleConstraint> tightenConstraints(
	const std::map<size_t,std::vector<const TriangleConstraint*>> conMap,
	const Eigen::MatrixXd& X);

/// Find an embedding in R^nDim consistent with the triangle ratio constraints.
/// This method may throw EmbedErr on some errors.
///
/// This method works by first positioning the reference distances, and then
/// placing all the other points with respect to them.
/// It therefore requires that there be an ordering of reference distances such
/// that the points in each distance are constrained by the prior distances.
/// The points of the first reference distance are placed, arbitrarily, at the
/// origin (0, 0, 0, 0, ...) and the first elementary vector (1, 0, 0, 0, ...).
/// The method will attempt to find an ordering of constraints to satisfy its
/// needs, and will throw std::invalid_argument when it can't.
/// The constraints returned by findTriangleConstraints() when it is passed some
/// traversal order are generally valid input.
EmbedResult embedTrianglesWithSpheres(
	std::vector<TriangleConstraint> cons, EmbedConfig config);

/// Find an embedding in R^nDim consistent with the triangle ratio constraints.
/// This method may throw EmbedErr on some errors.
///
/// This method works similarly to embedTrianglesWithSpheres(), but takes the
/// spherical shells into account instead of simply aiming for the center of
/// each shell.
EmbedResult embedTrianglesWithShells(
	std::vector<TriangleConstraint> cons, EmbedConfig config);

/// Find an embedding in R^d (d = X0.cols()) consistent with the triangle
/// ratio constraints.
/// This approach uses a quasi-newton method (L-BFGS) to minimize a loss
/// function defined in terms of the constraints.
/// Returns the embedding and the loss achieved.
EmbedResult embedTrianglesWithOpt(
	std::vector<TriangleConstraint> cons, const Eigen::MatrixXd& X0,
	EmbedConfig config);

/// Find an embedding in R^d consistent with the triangle ratio constraints.
/// Distances are found to a set of d+1 reference vertices using the triangle
/// constraints, and then the set is embedded from the distances.
EmbedResult embedTrianglesWithCM(std::vector<TriangleConstraint> cons,
	EmbedConfig config);

/// Embeds a dataset using a mixture of triangles and comparisons.
EmbedResult embedTrianglesWithMixture(std::vector<TriangleConstraint> tris,
	std::vector<CmpConstraint> cmps, double lambda, const Eigen::MatrixXd& X0,
	EmbedConfig config);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_TRIANGLE_HPP */


#include <Eigen/Dense>
#include "mex.h"
#include "math.h"
#include <iostream>
#include <omp.h>
#include <vector>
#include "MinIndexedPQ.h"

using namespace std;

using Eigen::Matrix2d;
using Eigen::Vector2d;
using Eigen::VectorXd;
using Eigen::SelfAdjointEigenSolver;
using Eigen::Map;

// To cache the dot products that we need to compute between the
// query image features and the predictor image features.  Each
// cachecell is associated with one locaton in the query image
struct CacheCell{
  // stores the locations in the predictor image where the
  // cache is valid
  int xmin;
  int ymin;
  int xmax;
  int ymax;
  // The actual dot products.
  double* data;
  CacheCell():xmin(0),ymin(0),xmax(0),ymax(0),data(NULL){}
};

vector<vector<CacheCell> > cache;

//extern bool mxUnshareArray(const mxArray *pr, const bool noDeepCopy);

mxArray* getfield(mxArray* str, const string& fnam){
  int field_num = mxGetFieldNumber(str, fnam.c_str());
  return mxGetFieldByNumber(str, 0, field_num);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  // Note that you should disable the parallel loop
  // before you enable debugging, or you will get segfaults!
  const bool DEBUG=false;
  // TODO: The first thing this mex function does is make a copy of its
  // input, which is really stupid because the structure of corresp does
  // not change, and the old values don't need to be kept.  Supposedly
  // these lines will let you avoid the copy, but I haven't tested it.
  //mxUnshareArray(const_cast<mxarray*> prhs[1],false);
  //mxArray* corresp=const_cast<mxarray*>(prhs[1]);
  mxArray* corresp=mxDuplicateArray(prhs[1]);
  int maxupdates = (int) mxGetScalar(prhs[3]);
  const mxArray* hogim = prhs[0];
  const int* hogimdims=mxGetDimensions(hogim);
  double* hogdata = mxGetPr(hogim);
  int ndims=hogimdims[0];
  int hogrows=hogimdims[1];
  int hogcols=hogimdims[2];
  int npyrs=* mxGetDimensions(prhs[2]);
  int* correspidx=(int *) mxGetData(prhs[4]);
  bool* inferred=(bool*)mxGetData(prhs[5]);
  int* inferinds=(int *)mxGetData(prhs[6]);
  int ntoinfer=*mxGetDimensions(prhs[6]);
  double* confidences = mxGetPr(prhs[7]);
  double lambda = mxGetScalar(prhs[8]);
  bool deleteCache = mxGetScalar(prhs[9])!=0;
  double* numneighbors = mxGetPr(prhs[10]);
  double lambdaprime = mxGetScalar(prhs[11]);
  if(DEBUG){mexPrintf("lam2 %f\n",lambda);mexEvalString("drawnow;");}
  if(DEBUG){mexPrintf("%d %d\n",ntoinfer,npyrs);}
  double** transf_out = new double*[npyrs];
  int ntransfs=0;
  // if deleteCache is 1, we're starting a new image, so clear out all
  // the cached dot products.
  if(deleteCache){
    for(int i = 0; i<cache.size(); ++i){
      for(int j=0; j<cache[i].size(); ++j){
        delete cache[i][j].data;
      }
    }
    cache.clear();
  }
  if(cache.size()==0){
    for(int i = 0; i<npyrs; ++i){
      cache.push_back(vector<CacheCell>(hogrows*hogcols));
    }
  }
  int cachehit=0;
  int cachemiss=0;
  string errmsg="";
  bool error=false;
  if(DEBUG){mexPrintf("start main loop over pyramids\n");}

  // Each pyramid's correspondence can be inferred in parallel.
  #pragma omp parallel for reduction(+:cachehit,cachemiss) shared(error,errmsg)
  for(int pyridx=0; pyridx<npyrs; ++pyridx){
    // These help us estimate when we need to return,
    // first by counting the number of times we've updated a
    // mu and sigma pair, and second by seeing how long it's
    // been since something moved a lot.
    int nupdates=0;
    int nsincebigmove=0;

    // transf==alpha in the paper.  Figure out which cells need to have an
    // alpha value inferred before we can start updating mu's and sigmas.
    // alpha's aren't kept around between calls to optimizecorresp, so
    // when we begin we need to infer an alpha for every cell in the
    // condition region.
    //
    // Likewise, cellheap associates with each cell an estimate of how
    // much that cell will change if it's updated.  We attempt to update
    // first any cell that seems like it will move a lot; this lets us
    // focus our computation on the parts of f that make the biggest
    // difference.
    vector<int> mustcomptransf;
    MinIndexedPQ cellheap(hogrows*hogcols);
    for(int ti = 0; ti<ntoinfer; ++ti){
      mustcomptransf.push_back(inferinds[ti]);
      cellheap.insert(inferinds[ti],-1000000);
    }
    vector<Matrix2d> transfs(hogrows*hogcols);
    if(DEBUG){cout<<"start main optimization loop\n";}
    // On each iteration of this loop, we update one mu and one sigma.
    while(!error){
      ++nupdates;
      ++nsincebigmove;

      // However, we may need to compute updates for many alphas, especially
      // at the beginning.
      for(int ti=0; ti<mustcomptransf.size(); ++ti){
        int infidx=mustcomptransf[ti];
        int xpos=infidx/hogrows;
        int ypos=infidx-xpos*hogrows;
        Matrix2d xsigma=Matrix2d::Zero();
        Matrix2d ysigma=Matrix2d::Zero();
        Vector2d xb=Vector2d::Zero();
        Vector2d yb=Vector2d::Zero();
        // Iterate over the edges of the lattice that this \alpha participates
        // in.  We aggregate statistics for both vertical and horizontal edges
        // simultaneously.  xsigma/ysigma aggregate the quadratic term
        // of equation 15, and xb/yb aggregate the linear term.
        for(int edgex=-2; edgex<=2; ++edgex){
          for(int edgey=-2; edgey<=2; ++edgey){
            if(edgex>-2&&edgex+xpos-1>=0&&edgex+xpos<hogcols&&edgey+ypos>=0&&edgey+ypos<hogrows &&
                  inferred[edgey+ypos+hogrows*(edgex+xpos-1)]&&inferred[edgey+ypos+hogrows*(edgex+xpos)]){
              mxArray* othcorresp=mxGetCell(corresp,edgey+ypos+(edgex+xpos)*hogrows);
              mxArray* othcorresp2=mxGetCell(corresp,edgey+ypos+(edgex+xpos-1)*hogrows);
              Map<Vector2d> othmu(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 0))+2*pyridx);
              Map<Matrix2d> othcovar(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 1))+4*pyridx);
              Map<Vector2d> othmu2(mxGetPr(mxGetFieldByNumber(othcorresp2, 0, 0))+2*pyridx);
              Map<Matrix2d> othcovar2(mxGetPr(mxGetFieldByNumber(othcorresp2, 0, 1))+4*pyridx);
              xsigma=xsigma+(othcovar.inverse()+othcovar2.inverse());
              xb=xb+(othcovar.inverse()+othcovar2.inverse())*(othmu-othmu2);
            }
            if(edgey>-2&&edgex+xpos>=0&&edgex+xpos<hogcols&&edgey+ypos-1>=0&&edgey+ypos<hogrows &&
                  inferred[edgey+ypos-1+hogrows*(edgex+xpos)]&&inferred[edgey+ypos+hogrows*(edgex+xpos)]){
              mxArray* othcorresp=mxGetCell(corresp,edgey+ypos+(edgex+xpos)*hogrows);
              mxArray* othcorresp2=mxGetCell(corresp,edgey+ypos-1+(edgex+xpos)*hogrows);
              Map<Vector2d> othmu(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 0))+2*pyridx);
              Map<Matrix2d> othcovar(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 1))+4*pyridx);
              Map<Vector2d> othmu2(mxGetPr(mxGetFieldByNumber(othcorresp2, 0, 0))+2*pyridx);
              Map<Matrix2d> othcovar2(mxGetPr(mxGetFieldByNumber(othcorresp2, 0, 1))+4*pyridx);
              ysigma=ysigma+(othcovar.inverse()+othcovar2.inverse());
              yb=yb+(othcovar.inverse()+othcovar2.inverse())*(othmu-othmu2);
            }
          }
        }
        transfs[ypos+xpos*hogrows].leftCols(1)=(xsigma).colPivHouseholderQr().solve(xb);
        transfs[ypos+xpos*hogrows].rightCols(1)=(ysigma).colPivHouseholderQr().solve(yb);
        if(DEBUG){cout<<"xsig\n"<<xsigma<<"\nxb\n"<<xb<<"\ntransfs\n"<<transfs[ypos+xpos*hogrows]<<"\n";}
      }
      mustcomptransf.clear();

      if(DEBUG){cout<<"start computing sigma's\n";}
      mxArray *mypyr=getfield(mxGetCell(prhs[2],pyridx),"features");
      // Get the linear index of the next mu/sigma pair to optimize, and store the estimated
      // change.
      double heapdist=cellheap.minKey();
      int infidx=cellheap.deleteMin();
      mxArray* mycorresp=mxGetCell(corresp,infidx);
      int edgex=1;
      int edgey=0;
      int xpos=infidx/hogrows;
      int ypos=infidx-xpos*hogrows;
      if(DEBUG){mexPrintf("get predictor feats\n");mexEvalString("drawnow;");}
      if(DEBUG){mexPrintf("%f\n",mxGetPr(mxGetFieldByNumber(mycorresp,0,2))[pyridx]);}
      mxArray* othhogarr=mxGetCell(mypyr,mxGetPr(mxGetFieldByNumber(mycorresp,0,2))[pyridx]-1);
      double* othhog=mxGetPr(othhogarr);
      const int* othhogdims=mxGetDimensions(othhogarr);
      int othrows=othhogdims[1];
      int othcols=othhogdims[2];
      // relsigma aggregates both Sigma in equation 21 and the outer product in equation 23.
      Matrix2d relsigma = Matrix2d::Zero();
      // relsigmainv aggregates inv(Sigma) in equation 21.
      Matrix2d relsigmainv = Matrix2d::Zero();
      if(DEBUG){mexPrintf("iterate over neighbors\n");mexEvalString("drawnow;");}
      Map<Vector2d> mymu(mxGetPr(mxGetFieldByNumber(mycorresp, 0, 0))+2*pyridx);
      Map<Matrix2d> mycovar(mxGetPr(mxGetFieldByNumber(mycorresp, 0, 1))+4*pyridx);
      // We first iterate over edges
      for(int edgeidx=0; edgeidx<4; ++edgeidx){
        if(DEBUG){mexPrintf("%d %d %d %d %d %d\n",edgex, edgey, xpos, ypos, hogrows, hogcols);}
        if(edgex+xpos>=0&&edgex+xpos<hogcols&&edgey+ypos>=0&&edgey+ypos<hogrows &&
          inferred[edgey+ypos+hogrows*(edgex+xpos)]){
          mxArray* othcorresp=mxGetCell(corresp,edgey+ypos+(edgex+xpos)*hogrows);
          Map<Vector2d> othmu(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 0))+2*pyridx);
          Map<Matrix2d> othcovar(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 1))+4*pyridx);
          Vector2d edge;
          edge << edgex, edgey;
          Matrix2d relsigmainv2=Matrix2d::Zero();
          Matrix2d relsigma2=Matrix2d::Zero();
          double n=0.0;
          // Then we iterate over the \alpha's that give us constraints over these edges
          for(int transfx=max(0,max(xpos,xpos+edgex)-2);transfx<=min(hogcols-1,min(xpos,xpos+edgex)+2); ++transfx){
            for(int transfy=max(0,max(ypos,ypos+edgey)-2);transfy<=min(hogrows-1,min(ypos,ypos+edgey)+2); ++transfy){
                if(DEBUG){cout << "tx " << transfx << " ty " <<transfy << "\n";}
              if(inferred[transfy+hogrows*(transfx)]){
                Vector2d mudiff=othmu-mymu-transfs[transfy+(transfx)*hogrows]*edge;
                if(DEBUG){cout << "othmu\n" << othmu << "\n" << mymu << "\n" << edge << "\ntransf\n"<<transfs[transfy+(transfx)*hogrows]<<"\n";}
                relsigma2=relsigma2+(othcovar+mudiff*mudiff.transpose())*lambdaprime;
                relsigmainv2=relsigmainv2+othcovar.inverse()*lambdaprime;
                n+=1.0;
              }
            }
          }
          // n is very nearly a constant and so in the paper I just rolled into lambdaprime.  However,
          // near the edge n will get smaller, and so dividing by n will give the warping
          // a little boost near the edge where there are fewer alpha's affecting each
          // edge.
          relsigma=relsigma+relsigma2/n;
          relsigmainv=relsigmainv+relsigmainv2/n;
          Vector2d mudiff=othmu-mymu-edge;
          relsigma=relsigma+(othcovar+mudiff*mudiff.transpose())*lambda;
          relsigmainv=relsigmainv+othcovar.inverse()*lambda;
        }
        // We rotate the edge to get the next edge in the sequence.
        int tmp=edgey;
        edgey=edgex;
        edgex=-tmp;
      }

      // Next we need to aggregate the unary terms.  We actually do an e-step (equation 13) as we
      // go along; the values computed on the e-step are not stored.
      //
      // First we need to compute a small region over which we should aggregate our statistics.
      // we use the same major/minor axis trick that we used in contextpredict to get
      // the window.
      SelfAdjointEigenSolver<Matrix2d> eigensolver(mycovar);
      Matrix2d V=eigensolver.eigenvectors().cwiseAbs();
      Vector2d D=eigensolver.eigenvalues();
      double mycovardet=mycovar.determinant();
      if(DEBUG){mexPrintf("computing eigenvalues 1\n");mexEvalString("drawnow;");}
      Matrix2d mycovarinv=mycovar.inverse();
      double dist=sqrt(fabs(1/(((1/(log(.0001)+log(mycovardet)/2))*(V.block(0,0,2,1).transpose()*mycovarinv*V.block(0,0,2,1)/2))(0))));
      double dist2=sqrt(fabs(1/(((1/(log(.0001)+log(mycovardet)/2))*(V.block(0,1,2,1).transpose()*mycovarinv*V.block(0,1,2,1)/2))(0))));
      if(DEBUG){mexPrintf("%f %f\n",dist,dist2);}
      Vector2d distvec = dist*V.block(0,0,2,1).cwiseMax(dist2*V.block(0,1,2,1));
      Map<VectorXd> mydata(hogdata+ndims*ypos+ndims*hogrows*xpos,ndims-1);
      double myconst = *(hogdata+ndims*ypos+ndims*hogrows*xpos+ndims-1);
      VectorXd mydatatransf=mydata;
      double sumprob=0;
      Matrix2d sumwt = Matrix2d::Zero();
      if(DEBUG){mexPrintf("5\n");mexEvalString("drawnow;");}
      //NOTE: window is in 1-indexed coordinates
      if(DEBUG){cout << distvec << "\n";}
      int xmin=floor(mymu(0)-min(distvec(0),30.0));
      int xmax=ceil(mymu(0)+min(distvec(0),30.0));
      int ymin=floor(mymu(1)-min(distvec(1),30.0));
      int ymax=ceil(mymu(1)+min(distvec(1),30.0));
      if(DEBUG){mexPrintf("5.1\n");mexEvalString("drawnow;");}

      // Now that we have the window, check to see if we have all the dot
      // products in that window cached.  We make sure that no more than 1/5
      // of the region that we need to compute is hanging outside of the
      // region stored in the cache on each side.
      CacheCell& mycache=cache[pyridx][hogrows*xpos+ypos];
      bool createcache=false;
      double* cachedata;
      if(DEBUG){mexPrintf("5.2\n");mexEvalString("drawnow;");}
      if(mycache.data!=NULL&&mycache.xmin<xmin+(xmax-xmin)/5&&mycache.ymin<ymin+(ymax-ymin)/5&&
         mycache.xmax>xmin+((xmax-xmin)*4+4)/5&&mycache.ymax>ymin+((ymax-ymin)*4+4)/5){
         ++cachehit;
      }else{
        ++cachemiss;
        createcache=true;
        mycache.xmin=xmin;
        mycache.ymin=ymin;
        mycache.xmax=xmax;
        mycache.ymax=ymax;
        if(mycache.data!=NULL){
          delete mycache.data;
        }
        mycache.data=new double[(ymax-ymin+1)*(xmax-xmin+1)];
      }
      if(DEBUG){mexPrintf("5.3\n");mexEvalString("drawnow;");}
      cachedata=mycache.data;
      if(DEBUG){mexPrintf("5.4\n");mexEvalString("drawnow;");}

      // Loop over the cache or the window, computing the inner products
      // and summing the total probability as required in equation 19.
      for(int windowx = max(xmin,mycache.xmin); windowx<=min(xmax,mycache.xmax); ++windowx){
        for(int windowy = max(ymin,mycache.ymin); windowy<=min(ymax,mycache.ymax); ++windowy){
          Vector2d window;
          window << windowx,windowy;
          Vector2d pt=window-mymu;
          double prob=1/(2*M_PI*sqrt(mycovardet))*exp((-pt.transpose()*mycovarinv*pt)(0)/2);
          if(prob!=prob){
            if(DEBUG){mexPrintf("%d\n",mycovardet);}
            errmsg="nan prob";
            if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
            error=1;
            continue;
          }
          double tmpprob;
          if(createcache){
            int idx=ndims*(min(othrows-1,max(0,windowy-1))+min(othcols-1,max(0,windowx-1))*othrows);
            Map<VectorXd> othdata(othhog+idx,ndims-1);
            tmpprob = exp(-(mydatatransf.dot(othdata)+myconst)/2);
            tmpprob=tmpprob/(tmpprob+.01*(*(othhog+idx+ndims-1)));
            cachedata[(windowx-xmin)*(ymax-ymin+1)+windowy-ymin]=tmpprob;
            if(prob!=prob){
              if(DEBUG){mexPrintf("%f %f %f %f\n",tmpprob,myconst,mydatatransf.dot(othdata),*(othhog+idx+ndims-1));}
              if(DEBUG){cout<<"othdata\n"<<othdata<<"\nmydata\n"<<mydata<<"\n";}
              errmsg="nan prob v2";
              if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
              error=1;
              continue;
            }
          }else{
            tmpprob=cachedata[(windowx-mycache.xmin)*(mycache.ymax-mycache.ymin+1)+windowy-mycache.ymin];
          }

          prob=prob*tmpprob;
          sumprob+=prob;
          sumwt=sumwt+prob*pt*(pt.transpose());
        }
      }
      if(DEBUG){mexPrintf("6\n");mexEvalString("drawnow;");}
      if(DEBUG){mexPrintf("sumwt\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << sumwt << "\n" << sumprob << "\n";}
      if(DEBUG){mexPrintf("relsigma\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << relsigma << "\n";}
      if(DEBUG){mexPrintf("sumwt\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << sumwt << "\n";}
      if(DEBUG){mexPrintf("relsigmainv\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << relsigmainv << "\n";}
      if(DEBUG){mexPrintf("mycovarinv\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << mycovarinv << "\n";}
      double myconfidence = confidences[ypos+hogrows*(xpos)];
      if(DEBUG){mexPrintf("myconfidence\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << myconfidence << "\n";}

      // Finally, put it all together
      Matrix2d nablasigma=(mycovarinv*(relsigma+sumwt*myconfidence)*mycovarinv-relsigmainv-mycovarinv*sumprob*myconfidence)/2;
      double covarstepsz=.1;
      if(nablasigma(0)!=nablasigma(0)){
        errmsg="nans in nablasigma";
        if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
        error=1;
        continue;
      }
      // As is often the case with EM algorithms, we can end up with a covariance matrix shrinking to zero and
      // causing problems.  The right thing to do is to put a constraint on the algorithm that the determinant
      // of the inverse covariance must be less than 1, and correctly project onto the constraint set.  The wrong
      // thing to do is to not take gradient steps when the determinant of the covariance matrix ends up less
      // than 1.  However, out of sheer laziness I've done the latter.  In practice this happens very rarely.
      while(-nablasigma(0)*covarstepsz>mycovar(0)||-nablasigma(3)*covarstepsz>mycovar(3)||(mycovar+nablasigma*covarstepsz).determinant()<1){
        covarstepsz=covarstepsz*.5;
      }
      if(DEBUG){mexPrintf("nablasigma\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << nablasigma << "\n";}
      Matrix2d newcovar=mycovar+nablasigma*covarstepsz;
      if(newcovar(0)>1e4||newcovar(4)>1e4){
        errmsg="exploding sigma";
        if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
        error=1;
        continue;
      }
      Matrix2d newcovarinv=newcovar.inverse();
      Vector2d constr=Vector2d::Zero();
      Matrix2d syst=Matrix2d::Zero();

      if(DEBUG){mexPrintf("starting mu update\n");mexEvalString("drawnow;");}
      // Just like before, we start off by iterating over edges, this time
      // aggregating system of linear equations that's defined by taking the
      // derivative of equation 17 (note that this system also needs to have
      // some terms from  equation 16 added in, but
      // we'll add those terms to the system later) and setting it to 0.
      //
      // We ultimately end up with an equation of the form syst*mu=constr,
      // where each term in the sums of equation 17 adds to both syst and constr.
      edgex=1;
      edgey=0;
      for(int edgeidx=0; edgeidx<4; ++edgeidx){
        if(DEBUG){mexPrintf("%d %d %d %d %d %d\n",edgex, edgey, xpos, ypos, hogrows, hogcols);mexEvalString("drawnow;");}
        if(edgex+xpos>=0&&edgex+xpos<hogcols&&edgey+ypos>=0&&edgey+ypos<hogrows &&
           inferred[edgey+ypos+hogrows*(edgex+xpos)]){
          mxArray* othcorresp=mxGetCell(corresp,edgey+ypos+(edgex+xpos)*hogrows);
          Map<Vector2d> othmu(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 0))+2*pyridx);
          Map<Matrix2d> othcovar(mxGetPr(mxGetFieldByNumber(othcorresp, 0, 1))+4*pyridx);
          Vector2d edge;
          edge << edgex, edgey;
          if(DEBUG){mexPrintf("17\n");mexEvalString("drawnow;");}
          if(DEBUG){cout << othcovar << "\n";}
          if(DEBUG){cout << othmu << "\n";}
          Matrix2d othinvcovar=othcovar.inverse();
          Matrix2d syst2=Matrix2d::Zero();
          Vector2d constr2=Vector2d::Zero();
          double n=0.0;
          for(int transfx=max(0,max(xpos,xpos+edgex)-2);transfx<=min(hogcols-1,min(xpos,xpos+edgex)+2); ++transfx){
            for(int transfy=max(0,max(ypos,ypos+edgey)-2);transfy<=min(hogrows-1,min(ypos,ypos+edgey)+2); ++transfy){
              if(inferred[transfy+hogrows*(transfx)]){
                Vector2d mudiff=othmu-transfs[transfy+(transfx)*hogrows]*edge;
                constr2=constr2+(othinvcovar+newcovarinv)*(mudiff)*lambdaprime;
                syst2=syst2+(othinvcovar+newcovarinv)*lambdaprime;
                n+=1.0;
              }
            }
          }
          // n is very nearly a constant and so in the paper I just rolled it into lambdaprime.  However,
          // near the edge n will get smaller, and so dividing by n will give the warping
          // a little boost near the edge where there are fewer alpha's affecting each
          // edge.
          constr=constr+constr2/n;
          syst=syst+syst2/n;

          Vector2d mudiff=othmu-edge;
          constr=constr+(othinvcovar+newcovarinv)*(mudiff)*lambda;
          syst=syst+(othinvcovar+newcovarinv)*lambda;
        }
        int tmp=edgey;
        edgey=edgex;
        edgex=-tmp;
      }
      if(DEBUG){mexPrintf("11\n");mexEvalString("drawnow;");}
      if(DEBUG){cout << constr << "\n";}
      if(DEBUG){cout << syst << "\n";}

      double newcovardet=newcovar.determinant();
      Vector2d mn = Vector2d::Zero();
      sumprob=0;

      // Now aggregate the contribution from the system from equation 16.  Computing
      // all the stuff for the e-step (including the window, the cache, etc.) is the
      // same as above.
      xmin=floor(mymu(0)-min(distvec(0),30.0));
      xmax=ceil(mymu(0)+min(distvec(0),30.0));
      ymin=floor(mymu(1)-min(distvec(1),30.0));
      ymax=ceil(mymu(1)+min(distvec(1),30.0));
      mycache=cache[pyridx][hogrows*xpos+ypos];
      createcache=false;
      if(mycache.xmin<xmin+(xmax-xmin)/5&&mycache.ymin<ymin+(ymax-ymin)/5&&
         mycache.xmax>xmin+((xmax-xmin)*4+4)/5&&mycache.ymax>ymin+((ymax-ymin)*4+4)/5){
         ++cachehit;
      }else{
        ++cachemiss;
        createcache=true;
        mycache.xmin=xmin;
        mycache.ymin=ymin;
        mycache.xmax=xmax;
        mycache.ymax=ymax;
        if(mycache.data!=NULL){
          delete mycache.data;
        }
        mycache.data=new double[(ymax-ymin+1)*(xmax-xmin+1)];
      }
      cachedata=mycache.data;

      for(int windowx = max(xmin,mycache.xmin); windowx<=min(xmax,mycache.xmax); ++windowx){
        for(int windowy = max(ymin,mycache.ymin); windowy<=min(ymax,mycache.ymax); ++windowy){
          Vector2d window;
          window << windowx,windowy;
          Vector2d pt=window-mymu;
          double prob=1/(2*M_PI*sqrt(newcovardet))*exp((-pt.transpose()*newcovarinv*pt)(0)/2);
          double tmpprob;
          if(createcache){
            int idx=ndims*(min(othrows-1,max(0,windowy-1))+min(othcols-1,max(0,windowx-1))*othrows);
            Map<VectorXd> othdata(othhog+idx,ndims-1);
            tmpprob = exp(-(mydatatransf.dot(othdata)+myconst)/2);
            tmpprob=tmpprob/(tmpprob+.01*(*(othhog+idx+ndims-1)));
            cachedata[(windowx-xmin)*(ymax-ymin+1)+windowy-ymin]=tmpprob;
            if(prob!=prob){
              if(DEBUG){mexPrintf("%f %f %f %f\n",tmpprob,myconst,mydatatransf.dot(othdata),*(othhog+idx+ndims-1));}
              if(DEBUG){cout<<"othdata\n"<<othdata<<"\nmydata\n"<<mydata<<"\n";}
              errmsg="nan prob v3";
              if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
              error=1;
              continue;
            }
          }else{
            tmpprob=cachedata[(windowx-mycache.xmin)*(mycache.ymax-mycache.ymin+1)+windowy-mycache.ymin];
          }
          prob=prob*tmpprob;
          sumprob+=prob;
          mn=mn+prob*pt;
        }
      }
      if(DEBUG){cout << mn << "\n" << mymu << "\n";}
      if(DEBUG){cout << sumprob << "\n";}
      constr=constr+newcovarinv*(mn+mymu*sumprob)*myconfidence;
      syst=syst+newcovarinv*sumprob*myconfidence;
      // Now put it all together.
      Vector2d newmu=syst.inverse()*constr;
      if(DEBUG){mexPrintf("Computed newmu, saving\n");mexEvalString("drawnow;");}
      if(DEBUG){mexPrintf("%f\n",infidx);mexEvalString("drawnow;");}

      mxArray* newcorresp=mxGetCell(corresp,infidx);
      Map<Vector2d> infmu(mxGetPr(mxGetFieldByNumber(newcorresp, 0, 0))+2*pyridx);
      Map<Matrix2d> infcovar(mxGetPr(mxGetFieldByNumber(newcorresp, 0, 1))+4*pyridx);
      if(DEBUG){cout << newmu << "\n";}
      if(DEBUG){cout << newcovar << "\n";}
      if(DEBUG){mexPrintf("%d\n",ntransfs);mexEvalString("drawnow;");}
      if(newmu(0)!=newmu(0)){
        errmsg="nans in newmu";
        if(DEBUG){mexErrMsgTxt(errmsg.c_str());}
        error=1;
        continue;
      }

      // At this point, actually updating the mu and sigma is finished. All that's left is to
      // put this cell back in the queue, and update all the other nodes that it's connected
      // to by an edge (since when this node moves, it will probably make its neighbors move).
      // In general, we aim to overestimate. so we don't have to be afraid of thinking we're
      // converged when we're not.
      double diff=(infmu-newmu).norm()*myconfidence;
      infmu=newmu;
      infcovar=newcovar;
      // We guess that, next time, the current node will move as much as it did this time.
      cellheap.insert(infidx,-diff);
      // Now, for each neighbor, we increment our estimate of how much it will move by the
      // amount that this node moved, divided by the number of neighbors that node has.
      // This is in general a pretty bad estimate, but it's far better than just sweeping
      // over everything.
      for(int edge=0; edge<4; ++edge){
        if(DEBUG){mexPrintf("%d %d %d %d %d %d\n",edgex, edgey, xpos, ypos, hogrows, hogcols);mexEvalString("drawnow;");}
        if(edgex+xpos>=0&&edgex+xpos<hogcols&&edgey+ypos>=0&&edgey+ypos<hogrows &&
           inferred[edgey+ypos+hogrows*(edgex+xpos)]){
             cellheap.decreaseKey(edgey+ypos+hogrows*(edgex+xpos),cellheap.keyOf(edgey+ypos+hogrows*(edgex+xpos))-diff/numneighbors[edgey+ypos+hogrows*(edgex+xpos)]);
        }
        int tmp=edgey;
        edgey=edgex;
        edgex=-tmp;
      }
      // On the next round, re-infer some alphas.  Specifically, only re-infer the alpha
      // associated with the current node.  There's probably a better way to do this, but
      // in practice this seems to work well enough.
      mustcomptransf.push_back(infidx);

      // If we've run out of computation budget or we don't seem to be
      // moving very much anymore, return.  Copy the set of computed alpha's
      // from the stack to the heap so we can return them.
      if(diff>=.1){
        nsincebigmove=0;
      }
      if((nupdates>=maxupdates||nsincebigmove==100)&&heapdist>-100000){
        if(nlhs>=1){
          transf_out[pyridx]=new double[hogrows*hogcols*4];
          ++ntransfs;
          for(int i=0;i<hogrows*hogcols; ++i){
            Map<Matrix2d> transf_out_i(transf_out[pyridx]+4*i);
            transf_out_i=transfs[i];
          }
        }
        break;
      }
    }
  }
  if(error){
    mexErrMsgTxt(errmsg.c_str());
  }

  // Copy the alpha's from the heap to Matlab.
  plhs[0]=corresp;
  if(nlhs>=1){
    mxArray* tout=mxCreateCellMatrix(npyrs,1);
    for(int i=0; i<npyrs; ++i){
      int dims[4];
      dims[0]=2;
      dims[1]=2;
      dims[2]=hogrows;
      dims[3]=hogcols;
      mxArray* data=mxCreateNumericArray(4, dims, mxDOUBLE_CLASS, mxREAL);
      double* transf_out2=mxGetPr(data);
      for(int j=0; j<dims[2]*dims[3]*4; ++j){
        transf_out2[j]=transf_out[i][j];
      }
      mxSetCell(tout,i,data);
      delete transf_out[i];
    }
    plhs[1]=tout;
  }
  // Useful for debugging/performance analysis.
  mexPrintf("cache hit %d miss %d\n",cachehit,cachemiss);
  delete transf_out;
}


#define BOOST_TEST_MODULE ideas

#include "adaptive_units.hpp"
#include "gd.hpp"
#include "graph.hpp"
#include "units.hpp"
#include "utils.hpp"

#include <boost/test/unit_test.hpp>

#include <optional>
#include <queue>
#include <utility>
#include <tuple>

using namespace std;

// -------------------------------------------------------------------
//
//     e1      e2
//    --1--   --2--
//   /     \ /     \
// (0)     (1)     (2)
//   \     / \     /
//    --1--   --1--
//     e3      e4
//
BOOST_AUTO_TEST_CASE(ideas_1)
{
  // Make sure we've got the modulation model right.
  using au = adaptive_units<COST>;
  // Set the maximal reach of the spectrally-worse (m = 1) modulation.
  au::set_reach_1(8);
  assert(au::units(1, 0) == 1);
  assert(au::units(1, 1) == 1);
  assert(au::units(1, 2) == 2);
  assert(au::units(1, 3) == 3);
  assert(au::units(1, 4) == 3);
  assert(au::units(1, 5) == 4);
  assert(au::units(1, 6) == 4);
  assert(au::units(1, 7) == 4);
  assert(au::units(1, 8) == 4);
  assert(au::units(1, 9) == std::numeric_limits<int>::max());

  graph g(3);
  auto p1 = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 2, g);
  auto p3 = boost::add_edge(0, 1, g);
  auto p4 = boost::add_edge(1, 2, g);

  assert(p1.second);
  assert(p2.second);
  assert(p3.second);
  assert(p4.second);

  const edge &e1 = p1.first;
  const edge &e2 = p2.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;

  boost::get(boost::edge_weight, g, e1) = 1;
  boost::get(boost::edge_weight, g, e2) = 2;
  boost::get(boost::edge_weight, g, e3) = 1;
  boost::get(boost::edge_weight, g, e4) = 1;

  // The SU.
  boost::get(boost::edge_su, g, e1) = SU{CU{0, 2}};
  boost::get(boost::edge_su, g, e2) = SU{CU{1, 4}};
  boost::get(boost::edge_su, g, e3) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e4) = SU{CU{0, 4}};

  // Search for edge-disjoint paths of minimal weight.
  demand d(npair(0, 2), 1);
  auto opaths = get<4>(gd(g, d, CU{0, 4}));
  assert(opaths);
  auto paths = opaths.value();

  // The first path should have two edges.
  BOOST_CHECK(paths.first.second.size() == 2);
  // The length of the first path should be 2.
  BOOST_CHECK(get_path_length(g, paths.first.second) == 2);
  // The CU of the first path should be CU(0, 2);
  BOOST_CHECK(paths.first.first == CU(0, 2));

  // The first path should have edges e1, and e4.
  {
    auto i = paths.first.second.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(i == paths.first.second.end());
  }

  // The second path should have two edges.
  BOOST_CHECK(paths.second.second.size() == 2);
  // The length of the second path should be 3.
  BOOST_CHECK(get_path_length(g, paths.second.second) == 3);
  // The CU of the second path should be CU(1, 4);
  BOOST_CHECK(paths.second.first == CU(1, 4));

  // The second path should have edges e3, and e2.
  {
    auto i = paths.second.second.begin();
    BOOST_CHECK(*i == e3);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.second.second.end());
  }
}

// -------------------------------------------------------------------
//
//     e1      e2
//    --1--   --1--
//   /     \ /     \
// (0)     (1)     (2)
//   \     / \     /
//    --2--   --1--
//     e3      e4
//
BOOST_AUTO_TEST_CASE(ideas_1a)
{
  // Make sure we've got the modulation model right.
  using au = adaptive_units<COST>;
  // Set the maximal reach of the spectrally-worse (m = 1) modulation.
  au::set_reach_1(8);
  assert(au::units(1, 0) == 1);
  assert(au::units(1, 1) == 1);
  assert(au::units(1, 2) == 2);
  assert(au::units(1, 3) == 3);
  assert(au::units(1, 4) == 3);
  assert(au::units(1, 5) == 4);
  assert(au::units(1, 6) == 4);
  assert(au::units(1, 7) == 4);
  assert(au::units(1, 8) == 4);
  assert(au::units(1, 9) == std::numeric_limits<int>::max());

  graph g(3);
  auto p1 = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 2, g);
  auto p3 = boost::add_edge(0, 1, g);
  auto p4 = boost::add_edge(1, 2, g);

  assert(p1.second);
  assert(p2.second);
  assert(p3.second);
  assert(p4.second);

  const edge &e1 = p1.first;
  const edge &e2 = p2.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;

  boost::get(boost::edge_weight, g, e1) = 1;
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_weight, g, e3) = 2;
  boost::get(boost::edge_weight, g, e4) = 1;

  // The SU.
  boost::get(boost::edge_su, g, e1) = SU{CU{0, 3}};
  boost::get(boost::edge_su, g, e2) = SU{CU{0, 2}};
  boost::get(boost::edge_su, g, e3) = SU{CU{0, 3}};
  boost::get(boost::edge_su, g, e4) = SU{CU{0, 3}};

  // Search for edge-disjoint paths of minimal weight.
  demand d(npair(0, 2), 1);
  auto opaths = get<4>(gd(g, d, CU{0, 4}));
  assert(opaths);
  auto paths = opaths.value();

  // The first path should have two edges.
  BOOST_CHECK(paths.first.second.size() == 2);
  // The length of the first path should be 2.
  BOOST_CHECK(get_path_length(g, paths.first.second) == 2);
  // The CU of the first path should be CU(0, 2);
  BOOST_CHECK(paths.first.first == CU(0, 2));

  // The first path should have edges e1, and e4.
  {
    auto i = paths.first.second.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.first.second.end());
  }

  // The second path should have two edges.
  BOOST_CHECK(paths.second.second.size() == 2);
  // The length of the second path should be 3.
  BOOST_CHECK(get_path_length(g, paths.second.second) == 3);
  // The CU of the second path should be CU(0, 3);
  BOOST_CHECK(paths.second.first == CU(0, 3));

  // The second path should have edges e3, and e4.
  {
    auto i = paths.second.second.begin();
    BOOST_CHECK(*i == e3);
    ++i;
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(i == paths.second.second.end());
  }
}

// -------------------------------------------------------------------
//
//     (1)
//     /|\
// e1 4 | 1 e2
//   /e3|  \
// (0)  1  (3)
//   \  |  /
// e4 1 | 5 e5
//     \|/
//     (2)
//
BOOST_AUTO_TEST_CASE(ideas_2)
{
  // Make sure we've got the modulation model right.
  using au = adaptive_units<COST>;
  // Set the maximal reach of the spectrally-worse (m = 1) modulation.
  au::set_reach_1(8);
  assert(au::units(1, 0) == 1);
  assert(au::units(1, 1) == 1);
  assert(au::units(1, 2) == 2);
  assert(au::units(1, 3) == 3);
  assert(au::units(1, 4) == 3);
  assert(au::units(1, 5) == 4);
  assert(au::units(1, 6) == 4);
  assert(au::units(1, 7) == 4);
  assert(au::units(1, 8) == 4);
  assert(au::units(1, 9) == std::numeric_limits<int>::max());

  graph g(4);
  auto p1 = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 3, g);
  auto p3 = boost::add_edge(1, 2, g);
  auto p4 = boost::add_edge(0, 2, g);
  auto p5 = boost::add_edge(2, 3, g);

  assert(p1.second);
  assert(p2.second);
  assert(p3.second);
  assert(p4.second);
  assert(p5.second);

  const edge &e1 = p1.first;
  const edge &e2 = p2.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;
  const edge &e5 = p5.first;

  // The lengths.
  boost::get(boost::edge_weight, g, e1) = 3;
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_weight, g, e3) = 1;
  boost::get(boost::edge_weight, g, e4) = 1;
  boost::get(boost::edge_weight, g, e5) = 4;

  // The SU.
  boost::get(boost::edge_su, g, e1) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e2) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e3) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e4) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e5) = SU{CU{0, 4}};

  // Search for edge-disjoint paths of minimal weight.
  demand d(npair(0, 3), 1);
  auto opaths = get<4>(gd(g, d, CU{0, 4}));
  assert(opaths);
  auto paths = opaths.value();

  // The size of the first path should be two edges.
  BOOST_CHECK(paths.first.second.size() == 2);
  // The CU of the first path should be CU(0, 3);
  BOOST_CHECK(paths.first.first == CU(0, 3));
  // The size of the second path should be two edges.
  BOOST_CHECK(paths.second.second.size() == 2);
  // The CU of the second path should be CU(0, 4);
  BOOST_CHECK(paths.first.first == CU(0, 3));

  // The first path should have edges e1, and e2.
  {
    auto i = paths.first.second.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.first.second.end());
  }

  // The second path should have edges e4, and e5.
  {
    auto i = paths.second.second.begin();
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(*i == e5);
    ++i;
    BOOST_CHECK(i == paths.second.second.end());
  }
}

// -------------------------------------------------------------------
//
// This test is similar to the one above, but we add parallel edges.
// We add e1a, which is shortest than e2, but e1a doesn't have enough
// spectrum.  We add e2a, so that the primary path goes through e2a,
// and edge e5 is not used.
//
//      -------(1)-------
//     /       /|\       \
// e1a 5   e1 4 | 1 e2   1 e2a
//     \     /  |  \     /
//      \   /   |   \   /
//       \ /    |    \ /
//       (0)    1 e3 (3)
//         \    |    /
//       e4 1   |   5 e5
//           \  |  /
//            \ | /
//             \|/
//             (2)
//
BOOST_AUTO_TEST_CASE(ideas_3)
{
  // Make sure we've got the modulation model right.
  using au = adaptive_units<COST>;
  // Set the maximal reach of the spectrally-worse (m = 1) modulation.
  au::set_reach_1(8);
  assert(au::units(1, 0) == 1);
  assert(au::units(1, 1) == 1);
  assert(au::units(1, 2) == 2);
  assert(au::units(1, 3) == 3);
  assert(au::units(1, 4) == 3);
  assert(au::units(1, 5) == 4);
  assert(au::units(1, 6) == 4);
  assert(au::units(1, 7) == 4);
  assert(au::units(1, 8) == 4);
  assert(au::units(1, 9) == std::numeric_limits<int>::max());

  graph g(4);
  auto p1 = boost::add_edge(0, 1, g);
  auto p1a = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 3, g);
  auto p2a = boost::add_edge(1, 3, g);
  auto p3 = boost::add_edge(1, 2, g);
  auto p4 = boost::add_edge(0, 2, g);
  auto p5 = boost::add_edge(2, 3, g);

  assert(p1.second);
  assert(p1a.second);
  assert(p2.second);
  assert(p2a.second);
  assert(p3.second);
  assert(p4.second);
  assert(p5.second);

  const edge &e1 = p1.first;
  const edge &e1a = p1a.first;
  const edge &e2 = p2.first;
  const edge &e2a = p2a.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;
  const edge &e5 = p5.first;

  // The lengths.
  boost::get(boost::edge_weight, g, e1) = 4;
  boost::get(boost::edge_weight, g, e1a) = 5;
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_weight, g, e2a) = 1;
  boost::get(boost::edge_weight, g, e3) = 1;
  boost::get(boost::edge_weight, g, e4) = 1;
  boost::get(boost::edge_weight, g, e5) = 5;

  // The SU.
  boost::get(boost::edge_su, g, e1) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e1a) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e2) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e2a) = SU{CU{1, 4}};
  boost::get(boost::edge_su, g, e3) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e4) = SU{CU{0, 4}};
  boost::get(boost::edge_su, g, e5) = SU{CU{0, 4}};

  // Search for edge-disjoint paths of minimal weight.
  demand d(npair(0, 3), 1);
  auto opaths = get<4>(gd(g, d, CU{0, 4}));
  assert(opaths);
  auto paths = opaths.value();

  // The first path should have three edges.
  BOOST_CHECK(paths.first.second.size() == 3);
  // The length of the first path should be 3.
  BOOST_CHECK(get_path_length(g, paths.first.second) == 3);
  // The CU of the first path should be CU(1, 4);
  BOOST_CHECK(paths.first.first == CU(1, 4));

  // The first path should have edges e4, e3, and e2a.
  {
    auto i = paths.first.second.begin();
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(*i == e3);
    ++i;
    BOOST_CHECK(*i == e2a);
    ++i;
    BOOST_CHECK(i == paths.first.second.end());
  }

  // The second path should have two edges.
  BOOST_CHECK(paths.second.second.size() == 2);
  // The length of the second path should be 5.
  BOOST_CHECK(get_path_length(g, paths.second.second) == 5);
  // The CU of the second path should be CU(0, 4);
  BOOST_CHECK(paths.second.first == CU(0, 4));

  // The second path should have edges e1, and e2.
  {
    auto i = paths.second.second.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.second.second.end());
  }
}