#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <igl/readDMAT.h>
#include <igl/writeDMAT.h>
#include <unordered_map>
#include <array>
#include <vector>
#include <queue>

void sparse_continuation(const Eigen::RowVector3d p0, const std::vector<Eigen::RowVector3i> init_voxels, const std::vector<double> t0, const std::function<double(const Eigen::RowVector3d &, double &, std::vector<std::vector<double>> &, std::vector<std::vector<double>> &, std::vector<std::vector<double>> &)> scalarFunc, const double eps, const int expected_number_of_cubes, Eigen::VectorXd & CS, Eigen::MatrixXd & CV, Eigen::MatrixXi & CI, Eigen::VectorXd & CV_argmins_vector){

    struct IndexRowVectorHash  {
        std::size_t operator()(const Eigen::RowVector3i& key) const {
            std::size_t seed = 0;
            std::hash<int> hasher;
            for (int i = 0; i < 3; i++) {
                seed ^= hasher(key[i]) + 0x9e3779b9 + (seed<<6) + (seed>>2); // Copied from boost::hash_combine
            }
            return seed;
        }
    };

    auto sgn = [](double val) -> int {
        return (double(0) < val) - (val < double(0));
    };

    double half_eps = 0.5 * eps;

    std::vector<Eigen::Matrix<int,1,8>> CI_vector;
    std::vector<Eigen::RowVector3d> CV_vector;
    std::vector<std::vector<std::vector<double>>> CV_intervals;
    std::vector<std::vector<std::vector<double>>> CV_values;
    std::vector<std::vector<std::vector<double>>> CV_minima;
    std::vector<double> CS_vector;
    CI_vector.reserve(expected_number_of_cubes);
    CV_vector.reserve(8 * expected_number_of_cubes);
    CS_vector.reserve(8 * expected_number_of_cubes);
    std::vector<std::vector<double>> argmins;
    std::vector<double> CV_argmins;

    argmins.reserve(32 * expected_number_of_cubes);
    int counter = 0;

    // Track visisted neighbors
    std::unordered_map<Eigen::RowVector3i, int, IndexRowVectorHash> visited;
    visited.reserve(6 * expected_number_of_cubes);
    visited.max_load_factor(0.5);

    // BFS Queue
    auto cmp = [](std::tuple<Eigen::RowVector3i, double, int, double> left, std::tuple<Eigen::RowVector3i, double, int, double> right) {
        double pleft, pright;
        pleft = std::get<3>(left);
        pright = std::get<3>(right);
        return pleft > pright;
    };

    std::priority_queue<std::tuple<Eigen::RowVector3i, double, int, double>, std::vector<std::tuple<Eigen::RowVector3i, double, int, double>>, decltype(cmp)> p_queue(cmp);

    std::vector<Eigen::RowVector3i> queue;
    queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> time_queue;
    time_queue.reserve(expected_number_of_cubes * 8);
    std::vector<int> correspondence_queue;
    correspondence_queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> intervals_turn, values_turn, minima_turn;

    for (int seed_ind = 0; seed_ind < init_voxels.size(); seed_ind++) {

        double min_turn = 1000.0;
        double final_seed = t0[seed_ind];
//        for (double tt = 0; tt <=1.0; tt = tt + 0.1) {
//            intervals_turn.resize(0);
//            values_turn.resize(0);
//            minima_turn.resize(0);
//            double seed_turn = tt;
//            Eigen::RowVector3i pi_turn = init_voxels[seed_ind];
//            Eigen::RowVector3d ctr_turn = p0 + eps*pi_turn.cast<double>();
//            double val_turn = scalarFunc(ctr_turn,seed_turn,intervals_turn,values_turn,minima_turn);
//            if (val_turn < min_turn) {
//                final_seed = seed_turn;
//                min_turn = val_turn;
//            }
//        }
        queue.push_back(init_voxels[seed_ind]);
        time_queue.push_back(final_seed);
        correspondence_queue.push_back(-1);
        auto bar = std::make_tuple(init_voxels[seed_ind], final_seed, -1, 0.0);
        p_queue.push(bar);
    }
    //queue.push_back(Eigen::RowVector3i(0, 0, 0));
    //time_queue.push_back(t0);
    //std::cout << "test" << std::endl;

    int additions_normal, additions_corrections, additions_self;
    additions_normal = 0;
    additions_corrections = 0;
    additions_self = 0;
    while (queue.size() > 0)
    {
        Eigen::RowVector3i pi = queue.back();
        queue.pop_back();
        double time_seed = time_queue.back();
        time_queue.pop_back();
        int correspondence = correspondence_queue.back();
        correspondence_queue.pop_back();

//        std::tuple<Eigen::RowVector3i, double, int, double> val;
//        val = p_queue.top();
//        p_queue.pop();
//        pi = std::get<0>(val);
//        time_seed = std::get<1>(val);
//        correspondence = std::get<2>(val);

        Eigen::RowVector3d ctr = p0 + eps*pi.cast<double>(); // R^3 center of this cube

        // X, Y, Z basis vectors, and array of neighbor offsets used to construct cubes
        const Eigen::RowVector3i bx(1, 0, 0), by(0, 1, 0), bz(0, 0, -1);
        const std::array<Eigen::RowVector3i, 30> neighbors = {
            bx, -bx, by, -by, bz, -bz,
            by-bz, -by+bz, // 1-2 4-7
            bx+by, -bx-by, // 0-1 7-6
            by+bz, -by-bz,  // 0-3 6-5
            by-bx, -by+bx,  // 2-3 5-4
            bx-bz, -bx+bz, // 1-5 3-7
            bx+bz, -bx-bz, // 0-4 2-6
            -bx+by+bz, bx-by-bz, // 3 5
            bx+by+bz, -bx-by-bz, // 0 6
            bx+by-bz, -bx-by+bz, //1 7
            -bx+by-bz, bx-by+bz, // 2 4,
            bx-bx, bx-bx,
            bx-bx, bx-bx
        };

        // Compute the position of the cube corners and the scalar values at those corners
        std::array<Eigen::RowVector3d, 8> cubeCorners = {
            ctr+half_eps*(bx+by+bz).cast<double>(), ctr+half_eps*(bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by+bz).cast<double>(),
            ctr+half_eps*(bx-by+bz).cast<double>(), ctr+half_eps*(bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by+bz).cast<double>()
        };
        std::array<double, 8> cubeScalars;
        //double time_seed = 0.0;
        //std::cout << time_seed << std::endl;
        double time_test;
        argmins[CI_vector.size()].resize(8);
        std::vector<double> argmins_cube;

        // Add the cube vertices and indices to the output arrays if they are not there already

        uint8_t vertexAlreadyAdded = 0; // This is a bimask. If a bit is 1, it has been visited already by the BFS
        constexpr std::array<uint8_t, 30> zv = {
            (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5),
            (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3) | (1 << 4) | (1 << 7),
            (1 << 1) | (1 << 2) | (1 << 5) | (1 << 6),
            (1 << 1) | (1 << 2),
            (1 << 4) | (1 << 7),
            (1 << 0) | (1 << 1),
            (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3),
            (1 << 5) | (1 << 6),
            (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5),
            (1 << 1) | (1 << 5),
            (1 << 3) | (1 << 7),
            (1 << 0) | (1 << 4),
            (1 << 2) | (1 << 6),
            (1 << 3), (1 << 5), // diagonals
            (1 << 0), (1 << 6),
            (1 << 1), (1 << 7),
            (1 << 2), (1 << 4),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
        };
        constexpr std::array<std::array<int, 4>, 30> zvv {{
            {{0, 1, 4, 5}}, {{3, 2, 7, 6}}, {{0, 1, 2, 3}},
            {{4, 5, 6, 7}}, {{0, 3, 4, 7}}, {{1, 2, 5, 6}},
            {{-1,-1,1,2}}, {{-1,-1,4,7}}, {{-1,-1,0,1}},{{-1,-1,7,6}},
            {{-1,-1,0,3}}, {{-1,-1,5,6}}, {{-1,-1,2,3}}, {{-1,-1,5,4}},
            {{-1,-1,1,5}}, {{-1,-1,3,7}}, {{-1,-1,0,4}}, {{-1,-1,2,6}},
            {{-1,-1,-1,3}}, {{-1,-1,-1,5}}, {{-1,-1,-1,0}}, {{-1,-1,-1,6}},
            {{-1,-1,-1,1}}, {{-1,-1,-1,7}}, {{-1,-1,-1,2}}, {{-1,-1,-1,4}},
            {{0,1,2,3}}, {{0,1,2,3}}, {{4,5,6,7}}, {{4,5,6,7}}
        }};
        bool flag = false;

        Eigen::Matrix<int,1,8> cube;
        cube << -1, -1, -1, -1, -1, -1, -1, -1;
        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            if (nbr != visited.end()) {
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                    }
                }
            }
        }

        // do we already know we're inside?
        bool we_in = true;
        for (int i = 0; i<8; i++) {
            if(cube[i]==-1){
                we_in = false;
                break;
            }
            if (CS_vector[cube[i]] > 0.0) {
                we_in = false;
                break;
            }
        }

        if (we_in) {
            continue;
        }

        std::vector<std::vector<std::vector<double>>> intervals;
        intervals.resize(8);
        std::vector<std::vector<std::vector<double>>> values;
        values.resize(8);
        std::vector<std::vector<std::vector<double>>> minima;
        minima.resize(8);

        bool debug_flag = false;
        bool in_existing_interval = false;
        bool intersecting_interval = false;
        time_test = time_seed;
        double running_argmin = 0.0;
        for (int i = 0; i < 8; i++){
            time_test = time_seed;
            //            cubeScalars[i] = scalarFunc(cubeCorners[i],time_test);
            //            argmins[CI_vector.size()][i] = time_test;
            //   running_argmin = running_argmin + (time_test/8.0);

            if (cube[i] >= 0) {
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                argmins[CI_vector.size()][i] = time_test;

                if (correspondence==-1) {
                double temp = cubeScalars[i];
                    //std::cout << "We got " << temp << " at " << time_seed << "...";
                int temp_i = -1;
                    int temp_s = -1;
                    for (int s = 0; s < CV_intervals[cube[i]].size(); s++) {
                for (int mm = 0; mm < (CV_intervals[cube[i]][s].size()/2); mm++){
                    if ( (CV_values[cube[i]][s][mm]+1e-3) < temp) {
                        temp = CV_values[cube[i]][s][mm];
                        temp_i = mm;
                        temp_s = s;
                    }
                }}
                if (temp_i > -1) {
                     //JAN 24 change this
                    queue.push_back(pi);
                    //std::cout << " but we found " << temp << " at " << CV_minima[cube[i]][temp_s][temp_i] << " (correspondence " << correspondence << std::endl;
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(CV_minima[cube[i]][temp_s][temp_i]);
                    correspondence_queue.push_back(1);
                    auto bar = std::make_tuple(pi, CV_minima[cube[i]][temp_s][temp_i], 1, temp);
                    p_queue.push(bar);
                    additions_self++;

                    //time_seed = CV_minima[cube[i]][temp_s][temp_i];
//                    cubeScalars[i] = temp;
//                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][temp_s][temp_i];
                }else{
                    //std::cout << std::endl;
                }
                }
                // DEBUGGING
                //                if (cube[i]==10) {
//                std::cout << "______________";
//                std::cout << "Time seed: " << time_seed << std::endl;
//                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
//                    std::cout << "Existing interval: " << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
//                }
                //                }
                //                            in_existing_interval = false;
                //                            for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                    cubeScalars[i] = CV_values[cube[i]][mm];
                //                                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][mm];
                //                                    in_existing_interval = true;
                //                                }
                //                            }
                //                            if (!in_existing_interval) {
                //                                intersecting_interval = false;
                //                                time_test = time_seed;
                //                                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                //                                argmins[CI_vector.size()][i] = time_test;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                    if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                        intersecting_interval = true;
                //                                        CV_intervals[cube[i]][2*mm] = std::min(CV_intervals[cube[i]][2*mm],time_seed);
                //                                        CV_intervals[cube[i]][2*mm+1] = std::max(CV_intervals[cube[i]][2*mm+1],time_seed);
                //                                        CV_values[cube[i]][mm] = std::min(CV_values[cube[i]][mm],cubeScalars[i]); //  ??
                //                                    }
                //                                }
                //                            }
                //                            if (!in_existing_interval && !intersecting_interval) {
                //                                std::vector<double> interval_i;
                //                                interval_i.push_back(time_test);
                //                                interval_i.push_back(time_seed);
                //                                std::sort(interval_i.begin(), interval_i.end());
                //                                CV_intervals[cube[i]].push_back(interval_i[0]);
                //                                CV_intervals[cube[i]].push_back(interval_i[1]);
                //                                CV_values[cube[i]].push_back(cubeScalars[i]);
                //                                CV_minima[cube[i]].push_back(time_test);
                //                            }
                //                time_test = time_seed;
                //                double debug = scalarFunc(cubeCorners[i],time_test);
                //                if (fabs(debug - cubeScalars[i])>1e-3 ) {
                //                                std::cout << "______________";
                ////                                std::cout << "Seed: " << time_seed << " value " << debug << " at time " << time_test << std::endl;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
                //                                    std::cout << "Existing interval :" << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
                //                                }
                //                                std::cout << "Point: " << CV_vector[cube[i]] << std::endl;
                //                }

            }else{
                time_test = time_seed;
                intervals[i].resize(0);
                values[i].resize(0);
                minima[i].resize(0);
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,intervals[i],values[i],minima[i]);
                argmins[CI_vector.size()][i] = time_test;
            }

            running_argmin = running_argmin + (argmins[CI_vector.size()][i]/8.0);
            //            if (cubeScalars[i]>0.3) {
            //                debug_flag = true;
            //            }

        }
        //std::cout << time_test << std::endl;
        // If this cube doesn't intersect the surface, disregard it
        bool validCube = false;
        int sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
                break;
            }
        }

        //    if (!validCube) {
        //continue;
        //  }

        // Debugging
        //        if (debug_flag) {
        //            std::cout << "voxel" << std::endl;
        //            for (int i = 0; i<8; i++) {
        //                std::cout << "value: " << cubeScalars[i] << " at time" << argmins[CI_vector.size()][i] << " with seed" << time_seed << std::endl;
        //
        //            }
        //        }

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                vertexAlreadyAdded |= zv[n];
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        //              if( CS_vector[cube[zvv[n][i]]] < cubeScalars[zvv[n][i]] ){
                        //                  // It was better before
                        //                  cubeScalars[zvv[n][i]] = CS_vector[cube[zvv[n][i]]];
                        //              }else
                        if((CS_vector[cube[zvv[n][i]]]>cubeScalars[zvv[n][i]] && (CS_vector[cube[zvv[n][i]]]*cubeScalars[zvv[n][i]])<0) || CS_vector[cube[zvv[n][i]]]>(cubeScalars[zvv[n][i]] + 1e-3)){
                            if (!flag) {
                                queue.push_back(nkey);
                                //time_queue.push_back(time_test);
                                // JAN 24 CHANGE THIS
                                time_queue.push_back(running_argmin);
                                correspondence_queue.push_back(nbr->second);
                                additions_corrections++;
                                flag = true;
                                auto bar = std::make_tuple(nkey, running_argmin, nbr->second, cubeScalars[zvv[n][i]]);
                                p_queue.push(bar);
                            }
                        }

                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        //cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        //CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }
        }

        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }
        bool validCube_before = validCube;

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }else{
                //                validCube = false;
                //                sign = sgn(cubeScalars[0]);
                //                for (int i = 1; i < 8; i++) {
                //                    if (sign != sgn(cubeScalars[i])) {
                //                        validCube = true;
                //                    }
                //                }
                if (validCube) {
                    //                    if (debug_flag) {
                    //                        std::cout << "BAD THING HAPPENED" << std::endl;
                    //                    }
                    //                    queue.push_back(nkey);
                    //                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    //                   // time_queue.push_back(time_test);
                    //                    time_queue.push_back(running_argmin);
                    //                    correspondence_queue.push_back(-1);
                }

            }
        }
  // WARNING THIS SHOULDNT BE COMMENTED!!
        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }

        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr == visited.end()) {
                if(validCube && validCube_before){
                    queue.push_back(nkey);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);

                    time_queue.push_back(running_argmin);
                    correspondence_queue.push_back(-1);
                    auto bar = std::make_tuple(nkey, running_argmin, -1, 0.0);
                    p_queue.push(bar);
                    additions_normal++;
                }
            }
        }

        auto did_we_visit_this_one = visited.find(pi);
        if (correspondence==-1 && did_we_visit_this_one==visited.end()) {
            for (int i = 0; i < 8; i++) { // Add new, non-visited,2 vertices to the arrays
                //if (0 == ((1 << i) & vertexAlreadyAdded)) {
                if (0 == ((1 << i) & vertexAlreadyAdded)) {
//                    std::vector<double> interval;
//                    std::vector<double> interval_values;
//                    std::vector<double> interval_minima;
//                    std::vector<double> interval_big;
//                    std::vector<double> interval_values_big;
//                    std::vector<double> interval_minima_big;
//                    interval_minima.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(time_seed);
//                    std::sort(interval.begin(), interval.end());
//                    interval_values.push_back(cubeScalars[i]);

                    CV_intervals.push_back(intervals[i]);
                    CV_values.push_back(values[i]);
                    CV_minima.push_back(minima[i]);
                    cube[i] = CS_vector.size();
                    CV_vector.push_back(cubeCorners[i]);
                    CS_vector.push_back(cubeScalars[i]);
                    CV_argmins.push_back(argmins[CI_vector.size()][i]);
                }
            }

            visited[pi] = CI_vector.size();
            CI_vector.push_back(cube);
        }

        //std::cout << queue.size() << std::endl;

        bool debug = false;
        if (debug) {
            CV.resize(CV_vector.size(), 3);
            CV_argmins_vector.resize(CV_vector.size(), 1);
            CS.resize(CS_vector.size(), 1);
            CI.resize(CI_vector.size(), 8);
            Eigen::MatrixXi Q;
            Q.resize(queue.size(),3);
            for (int i = 0; i < queue.size(); i++) {
                Q.row(i) = queue[i];
            }
            // If you pass in column-major matrices, this is going to be slooooowwwww
            for (int i = 0; i < CV_vector.size(); i++) {
                CV.row(i) = CV_vector[i];
            }
            for (int i = 0; i < CS_vector.size(); i++) {
                CS(i) = CS_vector[i];
            }
            for (int i = 0; i < CI_vector.size(); i++) {
                CI.row(i) = CI_vector[i];
            }
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CS" + std::to_string(counter) + ".dmat",CS);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CV" + std::to_string(counter) + ".dmat",CV);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CI" + std::to_string(counter) + ".dmat",CI);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/Q" + std::to_string(counter) + ".dmat",Q);
            counter = counter + 1;
        }

    }
    //std::cout << "test" << std::endl;
//    std::cout << " Normal: " << additions_normal << std::endl;
//    std::cout << " Corrections: " << additions_corrections << std::endl;
//    std::cout << " Self: " << additions_self << std::endl;

    CV.conservativeResize(CV_vector.size(), 3);
    CV_argmins_vector.conservativeResize(CV_vector.size(), 1);
    CS.conservativeResize(CS_vector.size(), 1);
    CI.conservativeResize(CI_vector.size(), 8);
//    // If you pass in column-major matrices, this is going to be slooooowwwww
//    for (int j = 0; j < CV_vector.size(); j++) {
//        std::cout << "______________" << std::endl;
//        for (int mm = 0; mm < (CV_intervals[j].size()/2); mm++){
//            std::cout << "Existing interval: " << CV_intervals[j][2*mm] << " " <<  CV_intervals[j][2*mm + 1] << " value: " << CV_values[j][mm] << std::endl;
//        }
//    }

    for (int i = 0; i < CV_vector.size(); i++) {
        CV.row(i) = CV_vector[i];
    }
    for (int i = 0; i < CS_vector.size(); i++) {
        CS(i) = CS_vector[i];
        //CS(i) = std::min_element(CV_values[i].begin(), CV_values[i].end());
    }
    for (int i = 0; i < CI_vector.size(); i++) {
        CI.row(i) = CI_vector[i];
    }
    for (int i = 0; i < CV_argmins.size(); i++) {
        double val = 100.0;
        double argmin = 0.0;
        for(int s = 0; s<CV_values[i].size(); s++){
        for (int mm = 0; mm<CV_values[i][s].size(); mm++) {
            if (CV_values[i][s][mm]<val) {
                val = CV_values[i][s][mm];
                argmin = CV_minima[i][s][mm];
            }
        }
        }
        CV_argmins_vector(i) = argmin;
    }
}

void sparse_continuation(const Eigen::RowVector3d p0, const std::vector<Eigen::RowVector3i> init_voxels, const std::vector<Eigen::RowVectorXd> t0, const  std::function<double(const Eigen::RowVector3d &, Eigen::RowVectorXd &, std::vector<std::vector<Eigen::RowVectorXd>> &, std::vector<std::vector<double>> &, std::vector<std::vector<Eigen::RowVectorXd>> &)> scalarFunc, const double eps, const int expected_number_of_cubes, Eigen::VectorXd & CS, Eigen::MatrixXd & CV, Eigen::MatrixXi & CI, Eigen::MatrixXd & CV_argmins_vector){

    struct IndexRowVectorHash  {
        std::size_t operator()(const Eigen::RowVector3i& key) const {
            std::size_t seed = 0;
            std::hash<int> hasher;
            for (int i = 0; i < 3; i++) {
            seed ^= hasher(key[i]) + 0x9e3779b9 + (seed<<6) + (seed>>2); // Copied from boost::hash_combine
            }
            return seed;
        }
    };

    auto sgn = [](double val) -> int {
        return (double(0) < val) - (val < double(0));
    };

    double half_eps = 0.5 * eps;

    std::vector<Eigen::Matrix<int,1,8>> CI_vector;
    std::vector<Eigen::RowVector3d> CV_vector;
    std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> CV_intervals;
    std::vector<std::vector<std::vector<double>>> CV_values;
    std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> CV_minima;
    std::vector<double> CS_vector;
    CI_vector.reserve(expected_number_of_cubes);
    CV_vector.reserve(8 * expected_number_of_cubes);
    CS_vector.reserve(8 * expected_number_of_cubes);
    std::vector<std::vector<Eigen::RowVectorXd>> argmins;
    std::vector<Eigen::RowVectorXd> CV_argmins;

    argmins.reserve(32 * expected_number_of_cubes);
    int counter = 0;

    // Track visisted neighbors
    std::unordered_map<Eigen::RowVector3i, int, IndexRowVectorHash> visited;
    visited.reserve(6 * expected_number_of_cubes);
    visited.max_load_factor(0.5);

    // BFS Queue
    std::vector<Eigen::RowVector3i> queue;
    queue.reserve(expected_number_of_cubes * 8);
    std::vector<Eigen::RowVectorXd> time_queue;
    time_queue.reserve(expected_number_of_cubes * 8);
    std::vector<int> correspondence_queue;
    correspondence_queue.reserve(expected_number_of_cubes * 8);
    std::vector<double> intervals_turn, values_turn, minima_turn;

    for (int seed_ind = 0; seed_ind < init_voxels.size(); seed_ind++) {

        double min_turn = 1000.0;
        Eigen::VectorXd final_seed = t0[seed_ind];

        queue.push_back(init_voxels[seed_ind]);
        time_queue.push_back(final_seed);
        correspondence_queue.push_back(-1);
    }

    while (queue.size() > 0)
    {
        Eigen::RowVector3i pi = queue.back();
        queue.pop_back();
        Eigen::VectorXd time_seed = time_queue.back();
        time_queue.pop_back();
        int correspondence = correspondence_queue.back();
        correspondence_queue.pop_back();

        Eigen::RowVector3d ctr = p0 + eps*pi.cast<double>(); // R^3 center of this cube

        // X, Y, Z basis vectors, and array of neighbor offsets used to construct cubes
        const Eigen::RowVector3i bx(1, 0, 0), by(0, 1, 0), bz(0, 0, -1);
        const std::array<Eigen::RowVector3i, 30> neighbors = {
            bx, -bx, by, -by, bz, -bz,
            by-bz, -by+bz, // 1-2 4-7
            bx+by, -bx-by, // 0-1 7-6
            by+bz, -by-bz,  // 0-3 6-5
            by-bx, -by+bx,  // 2-3 5-4
            bx-bz, -bx+bz, // 1-5 3-7
            bx+bz, -bx-bz, // 0-4 2-6
            -bx+by+bz, bx-by-bz, // 3 5
            bx+by+bz, -bx-by-bz, // 0 6
            bx+by-bz, -bx-by+bz, //1 7
            -bx+by-bz, bx-by+bz, // 2 4,
            bx-bx, bx-bx,
            bx-bx, bx-bx
        };

        // Compute the position of the cube corners and the scalar values at those corners
        std::array<Eigen::RowVector3d, 8> cubeCorners = {
            ctr+half_eps*(bx+by+bz).cast<double>(), ctr+half_eps*(bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by-bz).cast<double>(), ctr+half_eps*(-bx+by+bz).cast<double>(),
            ctr+half_eps*(bx-by+bz).cast<double>(), ctr+half_eps*(bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by-bz).cast<double>(), ctr+half_eps*(-bx-by+bz).cast<double>()
        };
        std::array<double, 8> cubeScalars;
        //double time_seed = 0.0;
        //std::cout << time_seed << std::endl;
        Eigen::RowVectorXd time_test;
        argmins[CI_vector.size()].resize(8);
        std::vector<Eigen::RowVectorXd> argmins_cube;

        // Add the cube vertices and indices to the output arrays if they are not there already

        uint8_t vertexAlreadyAdded = 0; // This is a bimask. If a bit is 1, it has been visited already by the BFS
        constexpr std::array<uint8_t, 30> zv = {
            (1 << 0) | (1 << 1) | (1 << 4) | (1 << 5),
            (1 << 2) | (1 << 3) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3) | (1 << 4) | (1 << 7),
            (1 << 1) | (1 << 2) | (1 << 5) | (1 << 6),
            (1 << 1) | (1 << 2),
            (1 << 4) | (1 << 7),
            (1 << 0) | (1 << 1),
            (1 << 6) | (1 << 7),
            (1 << 0) | (1 << 3),
            (1 << 5) | (1 << 6),
            (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5),
            (1 << 1) | (1 << 5),
            (1 << 3) | (1 << 7),
            (1 << 0) | (1 << 4),
            (1 << 2) | (1 << 6),
            (1 << 3), (1 << 5), // diagonals
            (1 << 0), (1 << 6),
            (1 << 1), (1 << 7),
            (1 << 2), (1 << 4),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
            (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7),
        };
        constexpr std::array<std::array<int, 4>, 30> zvv {{
            {{0, 1, 4, 5}}, {{3, 2, 7, 6}}, {{0, 1, 2, 3}},
            {{4, 5, 6, 7}}, {{0, 3, 4, 7}}, {{1, 2, 5, 6}},
            {{-1,-1,1,2}}, {{-1,-1,4,7}}, {{-1,-1,0,1}},{{-1,-1,7,6}},
            {{-1,-1,0,3}}, {{-1,-1,5,6}}, {{-1,-1,2,3}}, {{-1,-1,5,4}},
            {{-1,-1,1,5}}, {{-1,-1,3,7}}, {{-1,-1,0,4}}, {{-1,-1,2,6}},
            {{-1,-1,-1,3}}, {{-1,-1,-1,5}}, {{-1,-1,-1,0}}, {{-1,-1,-1,6}},
            {{-1,-1,-1,1}}, {{-1,-1,-1,7}}, {{-1,-1,-1,2}}, {{-1,-1,-1,4}},
            {{0,1,2,3}}, {{0,1,2,3}}, {{4,5,6,7}}, {{4,5,6,7}}
        }};
        bool flag = false;

        Eigen::Matrix<int,1,8> cube;
        cube << -1, -1, -1, -1, -1, -1, -1, -1;
        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            if (nbr != visited.end()) {
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                    }
                }
            }
        }

        std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> intervals;
        intervals.resize(8);
        std::vector<std::vector<std::vector<double>>> values;
        values.resize(8);
        std::vector<std::vector<std::vector<Eigen::RowVectorXd>>> minima;
        minima.resize(8);

        bool debug_flag = false;
        bool in_existing_interval = false;
        bool intersecting_interval = false;
        time_test = time_seed;
        Eigen::RowVectorXd running_argmin;
        running_argmin.resize(time_seed.size());
        running_argmin.setZero();
        for (int i = 0; i < 8; i++){
            time_test = time_seed;
            //            cubeScalars[i] = scalarFunc(cubeCorners[i],time_test);
            //            argmins[CI_vector.size()][i] = time_test;
            //   running_argmin = running_argmin + (time_test/8.0);

            if (cube[i] >= 0) {
               // std::cout << " CALL FUNC " << std::endl;
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
               // std::cout << " end CALL FUNC " << std::endl;
                argmins[CI_vector.size()][i] = time_test;
                minima[i] = CV_minima[cube[i]];
                values[i] = CV_values[cube[i]];
                intervals[i] = CV_intervals[cube[i]];

                if (correspondence==-1) {
                double temp = cubeScalars[i];
                int temp_i = -1;
                    int temp_s = -1;
                    for (int s = 0; s < CV_intervals[cube[i]].size(); s++) {
                for (int mm = 0; mm < (CV_intervals[cube[i]][s].size()/2); mm++){
                    if (CV_values[cube[i]][s][mm]+1e-3 < temp) {
                        temp = CV_values[cube[i]][s][mm];
                        temp_i = mm;
                        temp_s = s;
                    }
                }}
                if (temp_i > -1) {
                    queue.push_back(pi);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(CV_minima[cube[i]][temp_s][temp_i]);
                    correspondence_queue.push_back(1);
                }
                }
                // DEBUGGING
                //                if (cube[i]==10) {
//                std::cout << "______________";
//                std::cout << "Time seed: " << time_seed << std::endl;
//                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
//                    std::cout << "Existing interval: " << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
//                }
                //                }
                //                            in_existing_interval = false;
                //                            for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                    cubeScalars[i] = CV_values[cube[i]][mm];
                //                                    argmins[CI_vector.size()][i] = CV_minima[cube[i]][mm];
                //                                    in_existing_interval = true;
                //                                }
                //                            }
                //                            if (!in_existing_interval) {
                //                                intersecting_interval = false;
                //                                time_test = time_seed;
                //                                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,CV_intervals[cube[i]], CV_values[cube[i]], CV_minima[cube[i]]);
                //                                argmins[CI_vector.size()][i] = time_test;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++) {
                //                                    if ((time_test >= CV_intervals[cube[i]][2*mm]) && (time_test <= CV_intervals[cube[i]][2*mm+1]) ){
                //                                        intersecting_interval = true;
                //                                        CV_intervals[cube[i]][2*mm] = std::min(CV_intervals[cube[i]][2*mm],time_seed);
                //                                        CV_intervals[cube[i]][2*mm+1] = std::max(CV_intervals[cube[i]][2*mm+1],time_seed);
                //                                        CV_values[cube[i]][mm] = std::min(CV_values[cube[i]][mm],cubeScalars[i]); //  ??
                //                                    }
                //                                }
                //                            }
                //                            if (!in_existing_interval && !intersecting_interval) {
                //                                std::vector<double> interval_i;
                //                                interval_i.push_back(time_test);
                //                                interval_i.push_back(time_seed);
                //                                std::sort(interval_i.begin(), interval_i.end());
                //                                CV_intervals[cube[i]].push_back(interval_i[0]);
                //                                CV_intervals[cube[i]].push_back(interval_i[1]);
                //                                CV_values[cube[i]].push_back(cubeScalars[i]);
                //                                CV_minima[cube[i]].push_back(time_test);
                //                            }
                //                time_test = time_seed;
                //                double debug = scalarFunc(cubeCorners[i],time_test);
                //                if (fabs(debug - cubeScalars[i])>1e-3 ) {
                //                                std::cout << "______________";
                ////                                std::cout << "Seed: " << time_seed << " value " << debug << " at time " << time_test << std::endl;
                //                                for (int mm = 0; mm < (CV_intervals[cube[i]].size()/2); mm++){
                //                                    std::cout << "Existing interval :" << CV_intervals[cube[i]][2*mm] << " " <<  CV_intervals[cube[i]][2*mm + 1] << " value: " << CV_values[cube[i]][mm] << std::endl;
                //                                }
                //                                std::cout << "Point: " << CV_vector[cube[i]] << std::endl;
                //                }

            }else{
                time_test = time_seed;
                intervals[i].resize(0);
                values[i].resize(0);
                minima[i].resize(0);
          //      std::cout << " CALL FUNC " << std::endl;
                cubeScalars[i] = scalarFunc(cubeCorners[i],time_test,intervals[i],values[i],minima[i]);
          //      std::cout << " end CALL FUNC " << std::endl;
                argmins[CI_vector.size()][i] = time_test;
            }

            running_argmin = running_argmin + (argmins[CI_vector.size()][i]/8.0);
            //            if (cubeScalars[i]>0.3) {
            //                debug_flag = true;
            //            }

        }
        //std::cout << time_test << std::endl;
        // If this cube doesn't intersect the surface, disregard it
        bool validCube = false;
        int sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
                break;
            }
        }

        //    if (!validCube) {
        //continue;
        //  }

        // Debugging
        //        if (debug_flag) {
        //            std::cout << "voxel" << std::endl;
        //            for (int i = 0; i<8; i++) {
        //                std::cout << "value: " << cubeScalars[i] << " at time" << argmins[CI_vector.size()][i] << " with seed" << time_seed << std::endl;
        //
        //            }
        //        }

        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                vertexAlreadyAdded |= zv[n];
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        //              if( CS_vector[cube[zvv[n][i]]] < cubeScalars[zvv[n][i]] ){
                        //                  // It was better before
                        //                  cubeScalars[zvv[n][i]] = CS_vector[cube[zvv[n][i]]];
                        //              }else
                        if((CS_vector[cube[zvv[n][i]]]>cubeScalars[zvv[n][i]] && (CS_vector[cube[zvv[n][i]]]*cubeScalars[zvv[n][i]])<0) || CS_vector[cube[zvv[n][i]]]>(cubeScalars[zvv[n][i]] + 1e-3)){
                            if (!flag) {
                                queue.push_back(nkey);
                                //time_queue.push_back(time_test);
                                time_queue.push_back(running_argmin);
                                correspondence_queue.push_back(nbr->second);
                                flag = true;
                            }
                        }

                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        //cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        //CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }
        }

        validCube = false;
        sign = sgn(cubeScalars[0]);
        for (int i = 1; i < 8; i++) {
            if (sign != sgn(cubeScalars[i])) {
                validCube = true;
            }
        }

        for (int n = 0; n < 30; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr != visited.end()) { // We've already visited this neighbor, use references to its vertices instead of duplicating them
                for (int i = 0; i < 4; i++) {
                    if (zvv[n][i]!=-1) {
                        cube[zvv[n][i]] = CI_vector[nbr->second][zvv[n % 2 == 0 ? n + 1 : n - 1][i]];
                        // WARNING: THIS BELOW IS WRONG, HAVE TO CHANGE
                        cubeScalars[zvv[n][i]] = std::min(CS_vector[cube[zvv[n][i]]],cubeScalars[zvv[n][i]]);
                        CS_vector[cube[zvv[n][i]]] = cubeScalars[zvv[n][i]];
                    }
                }
                //} else if(correspondence==-1) {
            }else{
                //                validCube = false;
                //                sign = sgn(cubeScalars[0]);
                //                for (int i = 1; i < 8; i++) {
                //                    if (sign != sgn(cubeScalars[i])) {
                //                        validCube = true;
                //                    }
                //                }
                if (validCube) {
                    //                    if (debug_flag) {
                    //                        std::cout << "BAD THING HAPPENED" << std::endl;
                    //                    }
                    //                    queue.push_back(nkey);
                    //                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    //                   // time_queue.push_back(time_test);
                    //                    time_queue.push_back(running_argmin);
                    //                    correspondence_queue.push_back(-1);
                }

            }
        }
  // WARNING THIS SHOULDNT BE COMMENTED!!
//        validCube = false;
//        sign = sgn(cubeScalars[0]);
//        for (int i = 1; i < 8; i++) {
//            if (sign != sgn(cubeScalars[i])) {
//                validCube = true;
//            }
//        }
        for (int n = 0; n < 6; n++) { // For each neighbor, check the hash table to see if its been added before
            Eigen::RowVector3i nkey = pi + neighbors[n];
            auto nbr = visited.find(nkey);
            flag = false;
            if (nbr == visited.end()) {
                if(validCube){
                    queue.push_back(nkey);
                    // Otherwise, we have not visited the neighbor, put it in the BFS queue
                    // time_queue.push_back(time_test);
                    time_queue.push_back(running_argmin);
                    correspondence_queue.push_back(-1);
                }
            }
        }

        auto did_we_visit_this_one = visited.find(pi);
        if (correspondence==-1 && did_we_visit_this_one==visited.end()) {
            for (int i = 0; i < 8; i++) { // Add new, non-visited,2 vertices to the arrays
                //if (0 == ((1 << i) & vertexAlreadyAdded)) {
                if (0 == ((1 << i) & vertexAlreadyAdded)) {
//                    std::vector<double> interval;
//                    std::vector<double> interval_values;
//                    std::vector<double> interval_minima;
//                    std::vector<double> interval_big;
//                    std::vector<double> interval_values_big;
//                    std::vector<double> interval_minima_big;
//                    interval_minima.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(argmins[CI_vector.size()][i]);
//                    interval.push_back(time_seed);
//                    std::sort(interval.begin(), interval.end());
//                    interval_values.push_back(cubeScalars[i]);

                    CV_intervals.push_back(intervals[i]);
                    CV_values.push_back(values[i]);
                    CV_minima.push_back(minima[i]);
                    cube[i] = CS_vector.size();
                    CV_vector.push_back(cubeCorners[i]);
                    CS_vector.push_back(cubeScalars[i]);
                    CV_argmins.push_back(argmins[CI_vector.size()][i]);
                }
            }

            visited[pi] = CI_vector.size();
            CI_vector.push_back(cube);
        }

        //std::cout << queue.size() << std::endl;

        bool debug = false;
        if (debug) {
            CV.resize(CV_vector.size(), 3);
            CV_argmins_vector.resize(CV_vector.size(), 1);
            CS.resize(CS_vector.size(), 1);
            CI.resize(CI_vector.size(), 8);
            Eigen::MatrixXi Q;
            Q.resize(queue.size(),3);
            for (int i = 0; i < queue.size(); i++) {
                Q.row(i) = queue[i];
            }
            // If you pass in column-major matrices, this is going to be slooooowwwww
            for (int i = 0; i < CV_vector.size(); i++) {
                CV.row(i) = CV_vector[i];
            }
            for (int i = 0; i < CS_vector.size(); i++) {
                CS(i) = CS_vector[i];
            }
            for (int i = 0; i < CI_vector.size(); i++) {
                CI.row(i) = CI_vector[i];
            }
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CS" + std::to_string(counter) + ".dmat",CS);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CV" + std::to_string(counter) + ".dmat",CV);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/CI" + std::to_string(counter) + ".dmat",CI);
            igl::writeDMAT("/Volumes/Seagate Hard Drive/debug/Q" + std::to_string(counter) + ".dmat",Q);
            counter = counter + 1;
        }

    }
    //std::cout << "test" << std::endl;

    CV.conservativeResize(CV_vector.size(), 3);
    CV_argmins_vector.conservativeResize(CV_vector.size(), 2);
    CS.conservativeResize(CS_vector.size(), 1);
    CI.conservativeResize(CI_vector.size(), 8);
//    // If you pass in column-major matrices, this is going to be slooooowwwww
//    for (int j = 0; j < CV_vector.size(); j++) {
//        std::cout << "______________" << std::endl;
//        for (int mm = 0; mm < (CV_intervals[j].size()/2); mm++){
//            std::cout << "Existing interval: " << CV_intervals[j][2*mm] << " " <<  CV_intervals[j][2*mm + 1] << " value: " << CV_values[j][mm] << std::endl;
//        }
//    }

    for (int i = 0; i < CV_vector.size(); i++) {
        CV.row(i) = CV_vector[i];
    }
    for (int i = 0; i < CS_vector.size(); i++) {
        CS(i) = CS_vector[i];
        //CS(i) = std::min_element(CV_values[i].begin(), CV_values[i].end());
    }
    for (int i = 0; i < CI_vector.size(); i++) {
        CI.row(i) = CI_vector[i];
    }
    for (int i = 0; i < CV_vector.size(); i++) {
        std::cout << "1" << std::endl;
        double val = CV_values[i][0][0];
        Eigen::RowVectorXd argmin;
        argmin = CV_minima[i][0][0];
        std::cout << "2" << std::endl;
        for(int s = 0; s<CV_values[i].size(); s++){
        for (int mm = 0; mm<CV_values[i][s].size(); mm++) {
            if (CV_values[i][s][mm]<val) {
                std::cout << "3" << std::endl;
                val = CV_values[i][s][mm];
                std::cout << "4" << std::endl;
                argmin = CV_minima[i][s][mm];
            }
        }
        }
        std::cout << "5" << std::endl;
        std::cout << argmin << std::endl;
        CV_argmins_vector.row(i) = argmin;
        std::cout << "5b" << std::endl;
    }
    std::cout << "6" << std::endl;
}


// Copyright 2010-2013 The Trustees of Indiana University.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_SPLITTABLE_ECUYER1988_HPP
#define BOOST_SPLITTABLE_ECUYER1988_HPP

#include <boost/cstdint.hpp>
#include <boost/iterator.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <utility>

namespace boost {
  namespace random {

    template <typename Base> // To get around incomplete class issues
    class generic_split_iterator: public boost::iterator_facade<generic_split_iterator<Base>, Base, std::input_iterator_tag, Base> {
      public:
      Base base;
      uint32_t new_multiplier1, new_multiplier2;

      generic_split_iterator(Base base, uint32_t new_multiplier1, uint32_t new_multiplier2)
	: base(base), new_multiplier1(new_multiplier1), new_multiplier2(new_multiplier2) {}

      generic_split_iterator(): base(), new_multiplier1(0), new_multiplier2(0) {}

      Base dereference() const {
	Base result = base;
	result.multiplier1 = new_multiplier1;
	result.multiplier2 = new_multiplier2;
	return result;
      }

      bool equal(const generic_split_iterator& it) const {
	return base.state1 == it.base.state1 && base.state2 == it.base.state2;
      }

      void increment() {
	base(); // Advance seeds
      }
    };

    class splittable_ecuyer1988 {
      uint32_t state1, state2, multiplier1, multiplier2;

      static const uint64_t modulus1 = 2147483563;
      static const uint64_t modulus2 = 2147483399;

      template <uint64_t Modulus>
      static uint32_t mul_mod(uint32_t a, uint32_t b) {
	return uint32_t((uint64_t(a) * b) % Modulus);
      }

      template <uint64_t Modulus>
      static uint32_t exp_mod(uint32_t a, uint32_t b) {
	if (b == 0) {
	  return 1;
	} else if (b == 1) {
	  return a;
	} else {
	  uint32_t temp = exp_mod<Modulus>(a, b>>1);
	  temp = mul_mod<Modulus>(temp, temp);
	  if (b % 2) temp = mul_mod<Modulus>(a, temp);
	  return temp;
	}
      }

      public:
      splittable_ecuyer1988(): state1(1), state2(1), multiplier1(40014), multiplier2(40692) {}
      splittable_ecuyer1988(uint32_t seed1, uint32_t seed2): state1(seed1), state2(seed2), multiplier1(40014), multiplier2(40692) {}

      void seed(uint32_t seed1, uint32_t seed2) {
	state1 = seed1;
	state2 = seed2;
      }

      void split(splittable_ecuyer1988& a, splittable_ecuyer1988& b) const {
	a = *this;
	a.multiplier1 = mul_mod<modulus1>(a.multiplier1, a.multiplier1);
	a.multiplier2 = mul_mod<modulus2>(a.multiplier2, a.multiplier2);
	b = *this;
	b();
	b.multiplier1 = a.multiplier1;
	b.multiplier2 = a.multiplier2;
      }

      typedef generic_split_iterator<splittable_ecuyer1988> split_iterator;
      typedef std::pair<split_iterator, split_iterator> split_iterator_pair;
      friend class generic_split_iterator<splittable_ecuyer1988>;

      split_iterator_pair split_n(uint32_t n) const { // This RNG is not usable after the split
	split_iterator begin(*this, exp_mod<modulus1>(multiplier1, n), exp_mod<modulus2>(multiplier2, n));
	split_iterator end(*this, begin.new_multiplier1, begin.new_multiplier2);
	end.base.state1 = mul_mod<modulus1>(end.base.state1, end.new_multiplier1);
	end.base.state2 = mul_mod<modulus2>(end.base.state2, end.new_multiplier2);
	return std::make_pair(begin, end);
      }

      split_iterator_pair split_off_n(uint32_t n) { // Allows this RNG to keep being used
	split_iterator_pair p = split_n(n + 1);
	*this = *p.first;
	++p.first;
	return p;
      }

      typedef uint32_t result_type;

      uint32_t operator()() {
	state1 = mul_mod<modulus1>(state1, multiplier1);
	state2 = mul_mod<modulus2>(state2, multiplier2);
	return (state1 + modulus1 - 1 - state2) % (modulus1 - 1);
      }

      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      BOOST_STATIC_CONSTANT(uint32_t, min_value = 0);
      BOOST_STATIC_CONSTANT(uint32_t, max_value = modulus1 - 1);
      uint32_t min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      uint32_t max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}

      static bool validation(uint32_t x) {
	return x == 831582319;
      }
    };

    template <typename Gen>
    class uniform_01_wrapper {
      Gen& gen;

      public:
      uniform_01_wrapper(Gen& gen): gen(gen) {}

      double operator()() const {return boost::uniform_01<double>()(gen);}

      typedef double result_type;
      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      static const double min_value, max_value;
      double min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      double max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}
    };

    template <typename Gen>
    const double uniform_01_wrapper<Gen>::min_value = 0.;
    template <typename Gen>
    const double uniform_01_wrapper<Gen>::max_value = 1.;
  }
}

#endif // BOOST_SPLITTABLE_ECUYER1988_HPP


#include <frovedis.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>

using namespace frovedis;
using namespace std;

int sum(int a, int b){return a + b;}

BOOST_AUTO_TEST_CASE( frovedis_test )
{
  int argc = 1;
  char** argv = NULL;
  use_frovedis use(argc, argv);

  // filling sample input vector
  std::vector<int> v, ref;
  for(size_t i = 1; i <= 8; i++) v.push_back(i);
  for(size_t i = 1; i <= 8; i++) ref.push_back(i*2);

  auto d1 = frovedis::make_dvector_scatter(v);
  auto d2 = d1;
  auto d3 = frovedis::zip(d1, d2).map(sum);
  auto r = d3.gather();
  //for(auto i: r) std::cout << i << std::endl;

  BOOST_CHECK(r == ref);
}


#include "ni/layers/prunesmallsegments.h"

#include <boost/assign/list_of.hpp>

#include "elm/core/cv/mat_utils.h"
#include "elm/core/cv/mat_vector_utils.h"
#include "elm/core/exception.h"
#include "elm/core/graph/graphattr.h"
#include "elm/core/layerconfig.h"
#include "elm/core/signal.h"
#include "elm/ts/layerattr_.h"

#include "ni/core/graph/vertexopsegmentsize.h"

using namespace cv;
using namespace elm;
using namespace ni;

const std::string PruneSmallSegments::PARAM_MIN_SIZE = "min";

template <>
elm::MapIONames LayerAttr_<PruneSmallSegments>::io_pairs = boost::assign::map_list_of
        ELM_ADD_INPUT_PAIR(detail::BASE_SINGLE_INPUT_FEATURE_LAYER__KEY_INPUT_STIMULUS)
        ELM_ADD_OUTPUT_PAIR(detail::BASE_MATOUTPUT_LAYER__KEY_OUTPUT_RESPONSE)
        ;

PruneSmallSegments::PruneSmallSegments()
    : base_FeatureTransformationLayer()
{
}

void PruneSmallSegments::Clear()
{
    m_ = Mat1f();
}

void PruneSmallSegments::Reconfigure(const LayerConfig &config)
{
    min_ = config.Params().get<float>(PARAM_MIN_SIZE);
}

void PruneSmallSegments::Reset(const LayerConfig &config)
{
    Clear();
    Reconfigure(config);
}

cv::Mat1f mask_vertex2(const cv::Mat1i& img, const cv::Mat &mask)
{
    Mat mask_inverted;
    cv::bitwise_not(mask, mask_inverted);
    return img.clone().setTo(0, mask_inverted);
}

void PruneSmallSegments::Activate(const Signal &signal)
{
    Mat1f map = signal.MostRecentMat1f(name_input_); // weighted gradient after thresholding

    GraphAttr seg_graph(map.clone(), map > 0);

    VecI seg_ids = seg_graph.VerticesIds();
    Mat1f seg_sizes = elm::Reshape(seg_graph.applyVerticesToMap(VertexOpSegmentSize::calcSize));

    // assign size to vector attributes
    for(size_t i=0; i<seg_sizes.total(); i++) {

        if(seg_sizes(i) < min_) {

            seg_graph.removeVertex(seg_ids[i]);
        }
    }

    // replace with getter to Graph's underlying map image
    VecMat1f masked_maps = seg_graph.applyVerticesToMap(mask_vertex2);
    m_ = Mat1f::zeros(map.size());
    for(size_t i=0; i<masked_maps.size(); i++) {

        m_ += masked_maps[i];
    }
}


/**
 * Copyright (c) 2013-2015, Damian Vicino
 * Carleton University, Universite de Nice-Sophia Antipolis
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
#include <chrono>
#include <algorithm>
#include <boost/simulation.hpp>
#include <boost/simulation/pdevs/basic_models/generator.hpp>
#include <boost/rational.hpp>

using namespace boost::simulation;
using namespace boost::simulation::pdevs;
using namespace boost::simulation::pdevs::basic_models;
using namespace std;

using hclock=chrono::high_resolution_clock;

//This example is the simulation of a clock with 3 needles (H,M,S)

int main(){
    cout << "Creating the atomic models for the 3 needles" << endl;

    auto second = make_atomic_ptr<generator<boost::rational<int>, string>, boost::rational<int>, string>(boost::rational<int>{1}, string("second"));
    auto minute = make_atomic_ptr<generator<boost::rational<int>, string>, boost::rational<int>, string>(boost::rational<int>{60}, string("minute"));
    auto hour = make_atomic_ptr<generator<boost::rational<int>, string>, boost::rational<int>, string>(boost::rational<int>{3600}, string("hour"));

    cout << "Coupling the models into a clock model, the 3 needles make output" << endl;

    shared_ptr<coupled<boost::rational<int>, string>> clock( new coupled<boost::rational<int>, string>{{second, minute, hour}, {}, {}, {second, minute, hour}});

    cout << "Preparing runner" << endl;
    boost::rational<int> initial_time{0};
    runner<boost::rational<int>, string> r(clock, initial_time, cout, [](ostream& os, string m){ os << m;});
    boost::rational<int> end_time{7200}; //2 hours

    std::cout << "Starting simulation until time:" << end_time << "seconds" << std::endl;

    auto start = hclock::now(); //to measure simulation execution time

    end_time = r.runUntil(end_time);

    auto elapsed = std::chrono::duration_cast<std::chrono::duration<double, std::ratio<1>>>
                                                                                (hclock::now() - start).count();

    cout << "Finished simulation with time: " << end_time << "sec" << endl;
    cout << "Simulation took:" << elapsed << "sec" << endl;
    return 0;
}


#ifndef MAIA_GAMEENGINE_LOCALPOSITION_H_INCLUDED
#define MAIA_GAMEENGINE_LOCALPOSITION_H_INCLUDED

#include <Eigen/Core>

namespace Maia::GameEngine::Components
{
	struct Local_position
	{
		Eigen::Vector3f value{ 0.0f, 0.0f, 0.0f };
	};
}

#endif


#include <algorithm>
#include <bitset>
// #include <boost/algorithm/string/replace.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <map>
#include <set>
#include <string>
#include <vector>

using namespace std;

#define FOR(i, j, n) for (int i = j; i < n; i++)
#define OUT(s) cout << s << endl;
#define IN(a) cin >> a;
#define PACK(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cin >> a[i];
#define LIST(a, n)                                                             \
  for (int i = 0; i < n; i++)                                                  \
    cout << i << ": " << a[i] << endl;
#define PP(label, obj) cout << "[" << label << "] " << obj << endl;

int main() {
  int N;
  IN(N);

  string S[N];
  PACK(S, N);

  int sum = 0;
  int s1[26];
  int s2[26];
  FOR(i, 0, N) {
    FOR(j, i, N) {
      if (i == j)
        continue;

      FOR(x, 0, 26) s1[x] = 0;
      FOR(x, 0, 26) s2[x] = 0;
      FOR(k, 0, 10) {
        s1[int(S[i][k]) - 97] += 1;
      }
      FOR(k, 0, 10) {
        s2[int(S[j][k]) - 97] += 1;
      }
      bool found = true;
      FOR(k, 0, 26) {
        if (s1[k] != s2[k]) {
          found = false;
          break;
        }
      }
      if (found)
        sum += 1;
    }
  }
  OUT(sum);
}


#pragma once

#include <vector>
#include <Eigen/Dense>

namespace cilantro {
    template <typename ClusterIndexT = size_t, typename PointIndexT = size_t>
    std::vector<ClusterIndexT> getPointToClusterIndexMap(const std::vector<std::vector<PointIndexT>> &cluster_to_point,
                                                         size_t num_points)
    {
        // cluster_to_point.size() signifies unlabeled point
        std::vector<ClusterIndexT> point_to_segment(num_points, cluster_to_point.size());
        for (size_t i = 0; i < cluster_to_point.size(); i++) {
            for (size_t j = 0; j < cluster_to_point[i].size(); j++) {
                point_to_segment[cluster_to_point[i][j]] = static_cast<ClusterIndexT>(i);
            }
        }
        return point_to_segment;
    }

    // Cluster indices in point_to_cluster are in [0, num_clusters - 1];
    // a cluster index >= num_clusters signifies unlabeled point
    template <typename PointIndexT = size_t, typename ClusterIndexT = size_t>
    std::vector<std::vector<PointIndexT>> getClusterToPointIndicesMap(const std::vector<ClusterIndexT> &point_to_cluster,
                                                                      size_t num_clusters)
    {
        const ClusterIndexT first_invalid_cluster_idx = static_cast<ClusterIndexT>(num_clusters);
        std::vector<std::vector<PointIndexT>> cluster_to_point(num_clusters);
        for (size_t i = 0; i < point_to_cluster.size(); i++) {
            if (point_to_cluster[i] < first_invalid_cluster_idx) cluster_to_point[point_to_cluster[i]].emplace_back(i);
        }
        return cluster_to_point;
    }

    // Cluster indices in point_to_cluster are in [0, num_clusters - 1];
    // a cluster index >= num_clusters signifies unlabeled point
    template <typename PointIndexT, typename ClusterIndexT>
    std::vector<PointIndexT> getLabeledPointIndices(const std::vector<ClusterIndexT> &point_to_cluster,
                                                    size_t num_clusters)
    {
        const ClusterIndexT first_invalid_cluster_idx = static_cast<ClusterIndexT>(num_clusters);
        std::vector<PointIndexT> res;
        for (size_t i = 0; i < point_to_cluster.size(); i++) {
            if (point_to_cluster[i] < first_invalid_cluster_idx) res.emplace_back(i);
        }
        return res;
    }

    // Cluster indices in point_to_cluster are in [0, num_clusters - 1];
    // a cluster index >= num_clusters signifies unlabeled point
    template <typename PointIndexT, typename ClusterIndexT>
    std::vector<PointIndexT> getUnlabeledPointIndices(const std::vector<ClusterIndexT> &point_to_cluster,
                                                      size_t num_clusters)
    {
        const ClusterIndexT first_invalid_cluster_idx = static_cast<ClusterIndexT>(num_clusters);
        std::vector<PointIndexT> res;
        for (size_t i = 0; i < point_to_cluster.size(); i++) {
            if (point_to_cluster[i] >= first_invalid_cluster_idx) res.emplace_back(i);
        }
        return res;
    }

    // CRTP base class that holds clustering results and accessors
    template <typename Derived, typename PointIndexT = size_t, typename ClusterIndexT = size_t>
    class ClusteringBase {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef PointIndexT PointIndex;
        typedef ClusterIndexT ClusterIndex;

        typedef std::vector<std::vector<PointIndexT>> ClusterToPointIndicesMap;
        typedef std::vector<ClusterIndexT> PointToClusterIndexMap;

        inline const ClusterToPointIndicesMap& getClusterToPointIndicesMap() const { return cluster_to_point_indices_map_; }

        inline const PointToClusterIndexMap& getPointToClusterIndexMap() const { return point_to_cluster_index_map_; }

        inline size_t getNumberOfClusters() const { return cluster_to_point_indices_map_.size(); }

        inline size_t getNumberOfPoints() const { return point_to_cluster_index_map_.size(); }

        template <typename IndexT = PointIndexT>
        inline std::vector<IndexT> getLabeledPointIndices() const {
            return cilantro::getLabeledPointIndices<IndexT>(point_to_cluster_index_map_, cluster_to_point_indices_map_.size());
        }

        template <typename IndexT = PointIndexT>
        inline std::vector<IndexT> getUnlabeledPointIndices() const {
            return cilantro::getUnlabeledPointIndices<IndexT>(point_to_cluster_index_map_, cluster_to_point_indices_map_.size());
        }

    protected:
        ClusterToPointIndicesMap cluster_to_point_indices_map_;
        PointToClusterIndexMap point_to_cluster_index_map_;
    };
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file ErrorTermWheelTest.cpp
    \brief This file tests the ErrorTermWheel class.
  */

#include <Eigen/Core>

#include <boost/make_shared.hpp>

#include <gtest/gtest.h>

#include <aslam/backend/test/ErrorTermTestHarness.hpp>
#include <aslam/backend/EuclideanPoint.hpp>
#include <aslam/backend/EuclideanExpression.hpp>
#include <aslam/backend/Scalar.hpp>
#include <aslam/backend/ScalarExpression.hpp>

#include "aslam/calibration/car/error-terms/ErrorTermWheel.h"

using namespace aslam::backend;
using namespace aslam::calibration;

TEST(AslamCalibrationTestSuite, testErrorTermWheel) {

  // linear velocity
  const Eigen::Matrix<double, 3, 1> v(1.5, 1.7, 2.8);
  auto v_dv = boost::make_shared<EuclideanPoint>(v);
  EuclideanExpression v_exp(v_dv);

  // scaling factor
  auto k_vd = boost::make_shared<Scalar>(1.6);
  ScalarExpression k_exp(k_vd);

  // error term rear
  ErrorTermWheel e_r(v_exp, k_exp, 1.5, Eigen::Matrix3d::Identity());

  // error term front
  ErrorTermWheel e_f(v_exp, k_exp, 1.5, Eigen::Matrix3d::Identity(), true);

  // test the error terms
  try {
    ErrorTermTestHarness<3> harness(&e_r);
    harness.testAll();
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }
  try {
    ErrorTermTestHarness<3> harness(&e_f);
    harness.testAll();
  }
  catch (const std::exception& e) {
    FAIL() << e.what();
  }

  // test accessors
  e_r.setCovariance(Eigen::Matrix3d::Identity());
  ASSERT_EQ(e_r.getCovariance(), Eigen::Matrix3d::Identity());
  e_r.setMeasurement(10);
  ASSERT_EQ(e_r.getMeasurement(), 10);
  ASSERT_TRUE(e_f.getFrontEnabled());
  ASSERT_FALSE(e_r.getFrontEnabled());
  e_r.setFrontEnabled();
  ASSERT_TRUE(e_r.getFrontEnabled());
}


/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "galois/Reduction.h"
#include "galois/Timer.h"
#include "galois/Galois.h"
#include "galois/UserContext.h"
#include "galois/Graph/LCGraph.h"
#include "galois/worklists/WorkList.h"

#include "llvm/Support/CommandLine.h"
#include "Lonestar/BoilerPlate.h"

#include <boost/iterator/filter_iterator.hpp>

#include <algorithm>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <deque>
#include <cstdlib>

#define SHOULD_PRODUCE_CERTIFICATE 0

static const char* name = "Betweenness Centrality";
static const char* desc =
    "Computes the betweenness centrality of all nodes in a graph";
static const char* url = "betweenness_centrality";

static llvm::cl::opt<std::string> filename(llvm::cl::Positional,
                                           llvm::cl::desc("<input file>"),
                                           llvm::cl::Required);
static llvm::cl::opt<int> iterLimit(
    "limit",
    llvm::cl::desc("Limit number of iterations to value (0 is all nodes)"),
    llvm::cl::init(0));
static llvm::cl::opt<unsigned int>
    startNode("startNode", llvm::cl::desc("Node to start search from"),
              llvm::cl::init(0));
static llvm::cl::opt<bool> forceVerify(
    "forceVerify",
    llvm::cl::desc("Abort if not verified, only makes sense for torus graphs"));

typedef galois::graphs::LC_CSR_Graph<void, void>::with_no_lockable<
    true>::type ::with_numa_alloc<true>::type Graph;
typedef Graph::GraphNode GNode;

Graph* G;
int NumNodes;

galois::runtime::PerThreadStorage<double*> CB;

template <typename T>
struct PerIt {
  typedef typename galois::PerIterAllocTy::rebind<T>::other Ty;
};

struct process {
  typedef int tt_does_not_need_aborts;
  typedef int tt_needs_per_iter_alloc;
  typedef int tt_does_not_need_push;

  void operator()(GNode& _req, galois::UserContext<GNode>& lwl) {
    typedef std::deque<GNode, PerIt<GNode>::Ty> GNdeque;
    GNdeque SQ(lwl.getPerIterAlloc());
    std::deque<double, PerIt<double>::Ty> sigma(NumNodes, 0.0,
                                                lwl.getPerIterAlloc());
    std::deque<int, PerIt<int>::Ty> d(NumNodes, 0, lwl.getPerIterAlloc());
    std::deque<double, PerIt<double>::Ty> delta(NumNodes, 0.0,
                                                lwl.getPerIterAlloc());
    std::deque<GNdeque, PerIt<GNdeque>::Ty> succ(
        NumNodes, GNdeque(lwl.getPerIterAlloc()), lwl.getPerIterAlloc());
    unsigned int QAt = 0;

    int req = _req;

    sigma[req] = 1;
    d[req]     = 1;

    SQ.push_back(_req);

    while (QAt != SQ.size()) {
      GNode _v = SQ[QAt++];
      int v    = _v;
      for (Graph::edge_iterator
               ii = G->edge_begin(_v, galois::MethodFlag::NONE),
               ee = G->edge_end(_v, galois::MethodFlag::NONE);
           ii != ee; ++ii) {
        GNode _w = G->getEdgeDst(ii);
        int w    = _w;
        if (!d[w]) {
          SQ.push_back(_w);
          d[w] = d[v] + 1;
        }
        if (d[w] == d[v] + 1) {
          sigma[w] = sigma[w] + sigma[v];
          succ[v].push_back(w);
        }
      }
    }

    while (SQ.size()) {
      int w = SQ.back();
      SQ.pop_back();

      double sigma_w = sigma[w];
      double delta_w = delta[w];
      auto slist     = succ[w];
      for (auto ii = slist.begin(), ee = slist.end(); ii != ee; ++ii) {
        // std::cerr << "Processing node " << w << std::endl;
        GNode v = *ii;
        delta_w += (sigma_w / sigma[v]) * (1.0 + delta[v]);
      }
      delta[w] = delta_w;
    }
    double* Vec = *CB.getLocal();
    for (unsigned int i = 0; i < delta.size(); ++i) {
      Vec[i] += delta[i];
    }
  }
};

// Verification for reference torus graph inputs.
// All nodes should have the same betweenness value.
void verify() {
  double sampleBC = 0.0;
  bool firstTime  = true;
  for (int i = 0; i < NumNodes; ++i) {
    double bc = (*CB.getRemote(0))[i];
    for (unsigned int j = 1; j < galois::getActiveThreads(); ++j)
      bc += (*CB.getRemote(j))[i];
    if (firstTime) {
      sampleBC = bc;
      std::cerr << "BC: " << sampleBC << std::endl;
      firstTime = false;
    } else {
      if (!((bc - sampleBC) <= 0.0001)) {
        std::cerr << "If torus graph, verification failed " << (bc - sampleBC)
                  << "\n";
        if (forceVerify)
          abort();
        assert((bc - sampleBC) <= 0.0001);
        return;
      }
    }
  }
}

void printBCcertificate() {
  std::stringstream foutname;
  foutname << "outer_certificate_" << numThreads;
  std::ofstream outf(foutname.str().c_str());
  std::cerr << "Writing certificate..." << std::endl;

  for (int i = 0; i < NumNodes; ++i) {
    double bc = (*CB.getRemote(0))[i];
    for (unsigned int j = 1; j < galois::getActiveThreads(); ++j)
      bc += (*CB.getRemote(j))[i];
    outf << i << ": " << std::setiosflags(std::ios::fixed)
         << std::setprecision(9) << bc << std::endl;
  }
  outf.close();
}

struct HasOut : public std::unary_function<GNode, bool> {
  Graph* graph;
  HasOut(Graph* g) : graph(g) {}
  bool operator()(const GNode& n) const {
    return graph->edge_begin(n) != graph->edge_end(n);
  }
};

struct InitializeLocal {
  void operator()(unsigned, unsigned) {
    *CB.getLocal() = (double*)galois::runtime::MM::pageAlloc();
    std::fill(&(*CB.getLocal())[0], &(*CB.getLocal())[NumNodes], 0.0);
  }
};

int main(int argc, char** argv) {
  galois::StatManager M;
  LonestarStart(argc, argv, name, desc, url);

  Graph g;
  G = &g;
  galois::graphs::readGraph(*G, filename);

  NumNodes = G->size();

  // CB.resize(NumNodes);
  // FIXME
  assert(galois::runtime::MM::pageSize >= NumNodes * sizeof(double));
  galois::on_each(InitializeLocal());

  galois::reportPageAlloc("MeminfoPre");
  galois::preAlloc(numThreads * galois::runtime::MM::numPageAllocTotal() / 3);
  galois::reportPageAlloc("MeminfoMid");

  boost::filter_iterator<HasOut, Graph::iterator>
      begin = boost::make_filter_iterator(HasOut(G), g.begin(), g.end()),
      end   = boost::make_filter_iterator(HasOut(G), g.end(), g.end());

  boost::filter_iterator<HasOut, Graph::iterator> begin2 =
      iterLimit ? galois::safe_advance(begin, end, (int)iterLimit) : end;

  size_t iterations = std::distance(begin, begin2);

  std::vector<GNode> v(begin, begin2);

  std::cout << "NumNodes: " << NumNodes << " Start Node: " << startNode
            << " Iterations: " << iterations << "\n";

  typedef galois::worklists::StableIterator<true> WLL;
  galois::StatTimer T;
  T.start();
  galois::for_each<WLL>(v.begin(), v.end(), process());
  T.stop();

  if (!skipVerify) {
    for (int i = 0; i < 10; ++i) {
      double bc = (*CB.getRemote(0))[i];
      for (unsigned int j = 1; j < galois::getActiveThreads(); ++j)
        bc += (*CB.getRemote(j))[i];
      std::cout << i << ": " << std::setiosflags(std::ios::fixed)
                << std::setprecision(6) << bc << "\n";
    }
#if SHOULD_PRODUCE_CERTIFICATE
    printBCcertificate();
#endif
  }

  galois::reportPageAlloc("MeminfoPost");

  if (forceVerify || !skipVerify) {
    verify();
  }
  std::cerr << "Application done...\n";

  return 0;
}