#include "inf_pers2cyc.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <tuple>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/graph_utility.hpp>
#include "utils.h"
#include "mesh_writer.h"
#include "tests.h"
#include "flow_graph.hpp"

const size_t MAX_N_INTERVALS = 1000;

struct CompPersPairsByFiltIndex {
    CompPersPairsByFiltIndex(Bitmap_cubical_complex* _complex) : complex(_complex) {}

    bool operator()(const std::pair<int,int>& p1, const std::pair<int,int>& p2) {
        return complex->key(p1.second) - complex->key(p1.first) >
               complex->key(p2.second) - complex->key(p2.first);
    }

    Bitmap_cubical_complex* complex;
};

struct CompPersPairsByFuncVal {
    CompPersPairsByFuncVal(Bitmap_cubical_complex* _complex) : complex(_complex) {}

    bool operator()(const std::pair<int,int>& p1, const std::pair<int,int>& p2) {
        return complex->filtration(p1.second) - complex->filtration(p1.first) >
               complex->filtration(p2.second) - complex->filtration(p2.first);
    }

    Bitmap_cubical_complex* complex;
};

void computePersistenceCube(
    const std::string& perseus_fname,
    const IntervSortType interv_sort_type,
    const ExecOptions& ops,
    std::string* filt_fname) {

    Bitmap_cubical_complex bm_cube_cmplx(perseus_fname.c_str());
    cout << endl << "bitmap complex load done" << endl;

    Persistent_cohomology pcoh(bm_cube_cmplx);
    pcoh.init_coefficients(2);
    pcoh.compute_persistent_cohomology(0.0);
    auto pers_pairs = pcoh.get_persistent_pairs();

    vector<std::pair<int,int>> pers_pairs_dim2;
    for (auto p : pers_pairs) {
        if (bm_cube_cmplx.dimension(get<0>(p)) == 2) {
            pers_pairs_dim2.emplace_back(get<0>(p), get<1>(p));
        }
    }

    if (interv_sort_type == BY_FILT_INDEX) {
        CompPersPairsByFiltIndex comp(&bm_cube_cmplx);
        std::sort(pers_pairs_dim2.begin(), pers_pairs_dim2.end(), comp);
    } else {
        CompPersPairsByFuncVal comp(&bm_cube_cmplx);
        std::sort(pers_pairs_dim2.begin(), pers_pairs_dim2.end(), comp);
    }

    cout << endl << "computing persistence done, total intervals: "
        << pers_pairs_dim2.size() << endl;

    int dim, x_res, y_res, z_res;

    std::ifstream fin(perseus_fname.c_str());
    fin >> dim;
    if (dim != 3) {
        cout << "FATAL: perseus file dim not 3" << endl;
        exit(-1);
    }

    fin >> x_res >> y_res >> z_res;

    std::string purename;
    getFilePurename(perseus_fname, &purename);

    *filt_fname = purename
        + (interv_sort_type == BY_FILT_INDEX ? "_IX" : "_FV")
        + ".filt";

    cout << endl << "writing filt file '"
        << *filt_fname << "'" << endl;

    writeFiltCube(
        &bm_cube_cmplx, x_res, y_res, z_res,
        pers_pairs_dim2, *filt_fname);
}

void writeFiltCube(Bitmap_cubical_complex* bm_cube_cmplx,
    const int x_res, const int y_res, const int z_res,
    vector<std::pair<int,int>> pers_pairs_dim2,
    const std::string& filename) {

    int num_intv = std::min(pers_pairs_dim2.size(), MAX_N_INTERVALS);

    // cell count of every dimension for each pair.
    // indexed the same as 'pers_pairs_dim2'.
    vector<array<int,4>> dim_cell_count_all(num_intv, {-1, -1, -1, -1});
    vector<array<int,2>> pairs_key_index(num_intv);

    for (auto i = 0; i < num_intv; i ++) {
        // i-th pair in 'pers_pairs_dim2'
        auto p = pers_pairs_dim2[i];
        pairs_key_index[i][0] = bm_cube_cmplx->key(p.first);
        pairs_key_index[i][1] = i;
    }

    struct  {
        bool operator()(const array<int,2>& ki1, const array<int,2>& ki2) {
            return ki1[0] < ki2[0];
        }
    } comp;

    std::sort(pairs_key_index.begin(), pairs_key_index.end(), comp);
    // for (auto ki : pairs_key_index) {
    //     cout << ki[0] << " " << ki[1] << endl;
    // }

    // calculate the cell count of every dimension for all pairs
    array<int,4> dim_cell_count {0, 0, 0, 0};
    int cur_ind = 0;

    for (auto key = 0;
        key < bm_cube_cmplx->num_simplices() &&
        cur_ind < num_intv;
        key ++) {

        auto cell_id = bm_cube_cmplx->simplex(key);
        dim_cell_count[bm_cube_cmplx->dimension(cell_id)] ++;

        if (key == pairs_key_index.at(cur_ind).at(0)) {
            auto intv_ind = pairs_key_index.at(cur_ind).at(1);
            dim_cell_count_all.at(intv_ind) = dim_cell_count;
            cur_ind ++;
        }
    }

    if (cur_ind != num_intv) {
        cout << "FATAL: cur_ind != num_intv in writeFiltCube" << endl;
        exit(-1);
    }

    std::ofstream fout(filename);

    fout << num_intv << endl;

    for (auto i = 0; i < num_intv; i ++) {
        auto p = pers_pairs_dim2[i];

        fout << i << " "                              // 0: seq of interval
            << dim_cell_count_all.at(i).at(2)         // 1: count of 2 and 3-cells
            + dim_cell_count_all.at(i).at(3) << " "
            << bm_cube_cmplx->key(p.first) << " "     // 2: start key
            << bm_cube_cmplx->key(p.second) << " "    // 3: end key
            << p.first << " "                         // 4: start bm id
            << bm_cube_cmplx->key(p.second)           // 5: index length
            - bm_cube_cmplx->key(p.first) << " "
            << bm_cube_cmplx->filtration(p.second)    // 6: function value length
            - bm_cube_cmplx->filtration(p.first) << " "
            << dim_cell_count_all.at(i).at(1) << " "  // 7: 1-cell tount
            << dim_cell_count_all.at(i).at(2) << " "  // 8: 2-cell tount
            << dim_cell_count_all.at(i).at(3) << endl;// 9: 3-cell tount
    }

    fout << endl;

    // the given resolution is in terms of 3-d cells (cubes), while
    // the resolution for the filt file as well as for CubeComplex
    // is in terms of 0-d cells (vertices).
    fout << x_res+1 << " " << y_res+1 << " " << z_res+1 << endl << endl;

    vector<int> verts;
    for (auto key = 0; key <= bm_cube_cmplx->num_simplices(); key ++) {
        auto cell_id = bm_cube_cmplx->simplex(key);
        auto dim = bm_cube_cmplx->dimension(cell_id);

        if (dim == 2 || dim == 3) {
            getBitmapComplexCellVerts(cell_id, dim,
                x_res, y_res, z_res, &verts);

            fout << verts[0];
            for (auto i = 1; i < verts.size(); i ++) {
                fout << ' ' << verts[i];
            }
            fout << endl;
        }
    }
}

void infPers2CycsFromFile(
    const std::string& filt_fname,
    const ComplexType cmplx_type,
    const int start_interval,
    const int num_intervals,
    const ExecOptions& ops) {

    int num_all_intervals;

    {
        std::ifstream fin(filt_fname);
        fin >> num_all_intervals;
    }

    cout << endl << "filt file num of intervals: " << num_all_intervals << endl;

    for (int i = start_interval;
        i < num_all_intervals && i < start_interval + num_intervals;
        i ++) {

        cout << endl << endl << "---- " << i << "-th interval ----" << endl;

        // this complex will be deleted in infPers2Cyc
        CellComplex* complex;
        if (cmplx_type == CUBE_CMPLX) {
            complex = new CubeComplex();
        }
        complex->init(1,3);

        vector<int> pos_cell_2d_verts;
        int start_key = -1, start_id = -1;

        // loadToyCubeComplex((CubeComplex*)complex, &pos_cell_2d_verts);
        // std::string purename = "hardcode_toy";

        loadComplex(filt_fname, cmplx_type, complex,
            i, &start_key, &start_id, &pos_cell_2d_verts);

        char buf[50];
        snprintf(buf, 50, "%03d_", i);

        std::string purename;
        getFilePurename(filt_fname, &purename);
        purename = purename + buf + std::to_string(start_key)
            + "_" + std::to_string(start_id);

        if (ops.verbose) {
            cout << endl << "positive cell: " << pos_cell_2d_verts << endl;
        }
        cout << endl << "load complex done" << endl;

        if (ops.write_intem) {
            complex->writeMesh(purename + "_orig.off");
            // ((CubeComplex*)complex)->writeEdges(purename + "_orig_edges.ply");
        }

        infPers2Cyc(cmplx_type, complex, pos_cell_2d_verts, ops, purename);
    }
}

void loadComplex(
    const std::string& filt_fname,
    const ComplexType cmplx_type,
    CellComplex* complex,
    const int interval_id,
    int* interv_start_key,
    int* interv_start_id,
    vector<int>* pos_cell_2d_verts) {

    std::ifstream fin(filt_fname);

    int num_all_intervals, interv_cell_count;
    array<int,4> interv_dim_cell_count;

    fin >> num_all_intervals;

    for (int i = 0; i < num_all_intervals; i ++) {
        int interv_seq, cell_count, start_key, end_key, len_idx, start_id;
        double len_fv;
        array<int,4> dim_cell_count;

        fin >> interv_seq >> cell_count >> start_key >> end_key >> start_id
            >> len_idx >> len_fv
            >> dim_cell_count[1] >> dim_cell_count[2] >> dim_cell_count[3];

        if (i == interval_id) {
            interv_cell_count = cell_count;
            interv_dim_cell_count = dim_cell_count;
            *interv_start_key = start_key;
            *interv_start_id = start_id;
        }
    }

    if (cmplx_type == CUBE_CMPLX) {
        int x_res, y_res, z_res;
        fin >> x_res >> y_res >> z_res;
        ((CubeComplex*)complex)->setResolution(x_res, y_res, z_res);
    }

    complex->reserveCellSize(1, interv_dim_cell_count[1]);
    complex->reserveCellSize(2, interv_dim_cell_count[2]);
    complex->reserveCellSize(3, interv_dim_cell_count[3]);

    std::string line, s;
    vector<int> verts;

    for (int k = 0; k < interv_cell_count;) {
        if (!std::getline(fin, line)) {
            cout << "FATAL: reach end of file but not finish reading in loadComplex" << endl;
            exit(-1);
        }

        if (line.compare("") == 0) {
            continue;
        }

        std::istringstream iss(line);
        verts.clear();
        while (getline(iss, s, ' ')) {
            verts.push_back(std::stoi(s));
        }

        // cout << verts << endl;

        if (verts.size() == complex->getVertCnt(2)) {
            complex->toCanonVerts(2, &verts);
            complex->addCellNoConvCanon(2, verts);
        } else if (verts.size() == complex->getVertCnt(3)) {
            complex->toCanonVerts(3, &verts);
            complex->addCellNoConvCanon(3, verts);
        } else {
            cout << "FATAL: filt file has cells other than 2d or 3d" << endl;
        }

        k ++;
    }

    if (verts.size() != complex->getVertCnt(2)) {
        cout << "FATAL: positive cell vert count not right in loadComplex" << endl;
        exit(-1);
    }

    *pos_cell_2d_verts = verts;
}

void infPers2Cyc(
    const ComplexType cmplx_type, CellComplex* orig_complex,
    const vector<int>& pos_cell_2d_verts, const ExecOptions& ops,
    const std::string& file_prefix) {

    /* pruning */

    pruneComplex(orig_complex);

    // orig_complex->print();
    cout << endl << "prune done" << endl;

    if (ops.verbose) {
        cout << endl << "orig_complex map load:" << endl;
        orig_complex->printLoadStat();
    }

    if (ops.write_intem) {
        orig_complex->writeMesh(file_prefix + "_pruned.off");
    }

    /* get 2-connected component */

    auto start_cell_2d_id = orig_complex->getCellIdNoConvCanon(2, pos_cell_2d_verts);
    if (start_cell_2d_id < 0) {
        cout << "FATAL: start_cell_2d_id < 0 in infPers2Cyc" << endl;
        exit(-1);
    }

    auto orig_complex_cell_1d_cnt = orig_complex->getCellCount(1);
    auto orig_complex_cell_2d_cnt = orig_complex->getCellCount(2);
    auto orig_complex_cell_3d_cnt = orig_complex->getCellCount(3);

    orig_complex->deleteDimCofaces(2);
    orig_complex->deleteDimVertQueryMap(2);

    CellComplex* conn_complex;

    if (cmplx_type == CUBE_CMPLX) {
        conn_complex = new CubeComplex();
        ((CubeComplex*)conn_complex)->setResolution(
            ((CubeComplex*)orig_complex)->getXRes(),
            ((CubeComplex*)orig_complex)->getYRes(),
            ((CubeComplex*)orig_complex)->getZRes());
    }

    conn_complex->init(1,3);
    conn_complex->setCofaceCountHint(2, 2);

    // just some hints
    conn_complex->reserveCellSize(1, orig_complex_cell_1d_cnt/4+1);
    conn_complex->reserveCellSize(2, orig_complex_cell_2d_cnt/4+1);
    conn_complex->reserveCellSize(3, orig_complex_cell_3d_cnt/4+1);

    cellConnectedComponent(orig_complex, start_cell_2d_id, conn_complex);

    int conn_compnt_cell_2d_cnt = conn_complex->getCellCount(2);

    if (ops.write_intem) {
        conn_complex->writeMesh(file_prefix + "_conn.off");
    }

    /* add necessary 3-cells */

    vector<int> cell_3d_verts;
    vector<int> f_2d_verts;

    for (auto cell_3d_id = 0;
        cell_3d_id < orig_complex->getCellSize(3);
        cell_3d_id ++) {

        if (orig_complex->isCellValid(3, cell_3d_id)) {
            orig_complex->getCellVerts(3, cell_3d_id, &cell_3d_verts);
            orig_complex->getFaceVerts(3, cell_3d_verts, 0, &f_2d_verts);

            if (conn_complex->getCellId(2, f_2d_verts) >= 0) {
                conn_complex->addCellNoConvCanon(3, cell_3d_verts);
            }
        }
    }

    if (conn_compnt_cell_2d_cnt != conn_complex->getCellCount(2)) {
        cout << "FATAL: 2-cell count not equal after 3-cells adding" << endl;
        exit(-1);
    }

    cout << endl << "2-conn component done" << endl;

    if (ops.verbose) {
        cout << endl <<  "conn_complex map load:" << endl;
        conn_complex->printLoadStat();
    }

    delete orig_complex;
    orig_complex = NULL;

    /* reconstruct void boundaries */

    vector<array<int,2>>* cell_2d_void_map =
        new vector<array<int,2>>(conn_complex->getCellSize(2), {-1, -1});
    int void_cnt;

    reconVoidBound(cmplx_type, conn_complex, cell_2d_void_map, ops, file_prefix, &void_cnt);

    cout << endl << "void boundary reconstruction done" << endl;

    // free some memory space

    int src, sink;
    {
        auto pos_cell_2d_id = conn_complex->
            getCellIdNoConvCanon(2, pos_cell_2d_verts);

        vector<int> void_ids;
        getCell2dVoidIds(pos_cell_2d_id, conn_complex, cell_2d_void_map, &void_ids);
        src = void_ids.at(0);
        sink = void_ids.at(1);

        // auto cofaces = conn_complex->getCellCofaces(2, pos_cell_2d_id);
        // src = cofaces->at(0);
        // sink = cofaces->at(1);
    }

    if (src == sink) {
        cout << "FATAL: src sink the same in infPers2Cyc" << endl;
        exit(-1);
    }

    conn_complex->deleteDimCellData(1);
    conn_complex->deleteDimCellData(3);
    conn_complex->deleteDimVertQueryMap(2);

    /* record the corresponding set of 2-cells for each graph edge */

    GEdgeCellMap* gedge_cell_map = new GEdgeCellMap(conn_complex->getCellCount(2)/4+1);

    std::pair<int,int> gedge;
    const vector<int> empty_vector;
    for (auto cell_2d_id = 0;
        cell_2d_id < conn_complex->getCellSize(2);
        cell_2d_id ++) {

        if (conn_complex->isCellValid(2, cell_2d_id)) {
            vector<int> void_ids;
            getCell2dVoidIds(cell_2d_id, conn_complex, cell_2d_void_map, &void_ids);

            if (void_ids.at(0) != void_ids.at(1)) {
                gedge.first = void_ids.at(0);
                gedge.second = void_ids.at(1);
                toCanonEdge(&gedge);

                auto iter = gedge_cell_map->find(gedge);
                if (iter == gedge_cell_map->end()) {
                    std::tie(iter, std::ignore) =
                        gedge_cell_map->insert({gedge, empty_vector});
                    iter->second.reserve(1);
                    iter->second.push_back(cell_2d_id);
                } else {
                    iter->second.push_back(cell_2d_id);
                }
            }
        }
    }

    if (ops.verbose) {
        cout << endl << "gedge_cell_map: ";
        printHashMapLoad(*gedge_cell_map);
    }
    // cout << endl;
    // printGEdgeCellMap(gedge_cell_map, conn_complex);

    /* compute the min cyc by flow network */

    delete cell_2d_void_map;
    conn_complex->deleteDimCofaces(2);

    if (cmplx_type == CUBE_CMPLX) {
        long max_w = 0;

        for (auto iter = gedge_cell_map->begin();
            iter != gedge_cell_map->end(); iter ++) {

            if (iter->second.size() > max_w) {
                max_w = iter->second.size();
            }
        }

        if (ops.verbose) {
            cout << endl << "max_w: " << max_w << endl;
        }

        // TODO: use cpp limits instead
        if (max_w <= 127/2) {
            if (ops.verbose) {
                cout << "use 'char' as weight type" << endl;
            }

            computeMinCyc<char>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        } else if (max_w <= 32767/2) {
            if (ops.verbose) {
                cout << "use 'short' as weight type" << endl;
            }

            computeMinCyc<short>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        } else {
            if (ops.verbose) {
                cout << "use 'int' as weight type" << endl;
            }

            computeMinCyc<int>(
                conn_complex, gedge_cell_map, void_cnt,
                src, sink, ops, file_prefix);
        }
    }

    cout << endl << "compute min cycle done" << endl;

    delete gedge_cell_map;
    delete conn_complex;
}

void cellConnectedComponent(
    CellComplex* in_complex, const int start_cell_2d_id, CellComplex* out_complex) {

    vector<bool> cell_2d_visited(in_complex->getCellSize(2), false);

    cell_2d_visited[start_cell_2d_id] = true;
    vector<int> cell_stack { start_cell_2d_id };

    // vertices container for the 2-cell being deleted
    vector<int> cell_2d_verts;
    // vertices container for 1-faces of the 2-cell being deleted
    vector<int> f_verts;

    while (!cell_stack.empty()) {
        auto cell_2d_id = cell_stack[cell_stack.size() - 1];
        cell_stack.pop_back();
        in_complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

#ifdef DEBUG_OPT
        if (out_complex->getCellIdNoConvCanon(2, cell_2d_verts) >= 0) {
            cout << "FATAL: cell in out_complex" << endl;
            exit(-1);
        }
#endif

        out_complex->addCellNoConvCanon(2, cell_2d_verts);

        for (auto i = 0; i < in_complex->getFaceCnt(2); i ++) {
            in_complex->getFaceVerts(2, cell_2d_verts, i, &f_verts);
            auto face_id = in_complex->getCellId(1, f_verts);
            const vector<int>* cofaces = in_complex->getCellCofaces(1, face_id);

            for (auto j = 0; j < cofaces->size(); j ++) {
#ifdef DEBUG_OPT
                if ( !in_complex->isCellValid( 2, cofaces->at(j) ) ) {
                    cout << "FATAL: coface invalid in cellConnectedComponent" << endl;
                    exit(-1);
                }
#endif

                if (!cell_2d_visited[ cofaces->at(j) ]) {
                    cell_stack.push_back( cofaces->at(j) );
                    cell_2d_visited[ cofaces->at(j) ] = true;
                }
            }
        }
    }
}

void pruneComplex(CellComplex* complex) {

    std::unordered_set<int> one_cells_w1cof;
    for (auto cell_1d_id = 0; cell_1d_id < complex->getCellSize(1); cell_1d_id ++) {
        if (complex->isCellValid(1, cell_1d_id)) {
            if (complex->getCellCofaces(1, cell_1d_id)->size() == 1) {
                one_cells_w1cof.insert(cell_1d_id);
            }
        }
    }

    // vertices container for the 2-cell being deleted
    vector<int> cell_2d_verts;
    // vertices container for 1-faces of the 2-cell being deleted
    vector<int> f_verts;

    while (!one_cells_w1cof.empty()) {
        auto cell_1d_id = *(one_cells_w1cof.begin());
        // the 2-cell being deleted
        auto cell_2d_id = complex->getCellCofaces(1, cell_1d_id)->at(0);

#ifdef DEBUG_OPT
        if (complex->getCellCofaces(1, cell_1d_id)->size() != 1) {
            cout << "FATAL: 1d cell has cofaces not 1" << endl;
            exit(-1);
        }
        if (complex->getCellCofaces(2, cell_2d_id)->size() != 0) {
            cout << "FATAL: 2d cell has coface in prune" << endl;
            exit(-1);
        }
#endif

        complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);
        complex->deleteCell(2, cell_2d_id);

        // enumerate all faces of the 2-cell being deleted
        for (auto i = 0; i < complex->getFaceCnt(2); i ++) {
            complex->getFaceVerts(2, cell_2d_verts, i, &f_verts);
            auto face_id = complex->getCellId(1, f_verts);

            complex->deleteCoface(1, face_id, cell_2d_id);

            if (complex->getCellCofaces(1, face_id)->size() == 0) {
                one_cells_w1cof.erase(face_id);
            } else if (complex->getCellCofaces(1, face_id)->size() == 1) {
                one_cells_w1cof.insert(face_id);
            }
        }
    }
}

void get2DCellPtCubic(
    CellComplex* complex, const int cell_2d_id,
    const vector<int>& cell_1d_verts, Vector3d* pt) {

    vector<int> cell_2d_verts;
    complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

    int verts[2];
    int j = 0;
    for (auto v : cell_2d_verts) {
        if (v != cell_1d_verts[0] && v != cell_1d_verts[1]) {
            verts[j] = v;
            j ++;
        }
    }

#ifdef DEBUG_OPT
    if (j != 2) {
        cout << "FATAL: j!=2 in get2DCellPtCubic" << endl;
        exit(-1);
    }
#endif

    Vector3d pt1, pt2;
    complex->getVertPos(verts[0], &pt1);
    complex->getVertPos(verts[1], &pt2);
    *pt = (pt1+pt2) / 2;
}

// get the canonical oriented 2d cell vertex sequence where the orientation is
// from the from_vert to the to_vert. the canonical vertex sequence then starts
// from the smallest vertex following the orientation.
// the input 'verts' is assumed to be in canon order.
void getCanonOrien2DCellVertsCubic(const vector<int>& verts,
    const int& from_vert, const int& to_vert, vector<int>* canon_verts) {

    canon_verts->clear();

    int inc;
    for (auto i = 0; i < verts.size(); i ++) {
        if (verts[i] == from_vert) {
            if (verts[ mod(i+1, verts.size()) ] == to_vert) {
                inc = 1;
            } else {
                inc = -1;
            }

            break;
        }
    }

    // int min_ind = 0;
    // int min_vert = verts[0];

    // for (auto i = 1; i < verts.size(); i ++) {
    //     if (verts[i] < min_vert) {
    //         min_ind = i;
    //         min_vert = verts[i];
    //     }
    // }

    for (auto i = 0; i < verts.size(); i ++) {
        // canon_verts->push_back( verts[ mod(min_ind+i*inc, verts.size()) ] );
        canon_verts->push_back( verts[ mod(i*inc, verts.size()) ] );
    }
}

void addOrien2DCellPairs(
    const ComplexType cmplx_type,
    CellComplex* complex,
    const int cell_1d_id,
    const vector<int>& cell_1d_verts,
    VertArrIdMap* orien_cell_2d_id_map,
    Graph<int>* orien_cell_2d_graph) {

    // if (cell_1d->cofaces.size() == 2) {
    //     int dosomething = 0;
    // }

    const vector<int>* cell_1d_cofaces = complex->getCellCofaces(1, cell_1d_id);

    Vector3d cell_1d_pt0, cell_1d_pt1;
    complex->getVertPos(cell_1d_verts[0], &cell_1d_pt0);
    complex->getVertPos(cell_1d_verts[1], &cell_1d_pt1);

    // cout << endl << cell_1d_verts << endl;
    // cout << cell_1d_pt0 << endl;
    // cout << cell_1d_pt1 << endl;

    Vector3d cell_1d_mid_pt = (cell_1d_pt0+cell_1d_pt1) / 2;
    // cout << cell_1d_mid_pt << endl;

    /* get the plane equation of the first coface */

    auto first_cell_2d_id = cell_1d_cofaces->at(0);
    vector<int> first_cell_2d_verts;
    complex->getCellVerts(2, first_cell_2d_id, &first_cell_2d_verts);

    Vector3d first_cell_2d_pt0, first_cell_2d_pt1, first_cell_2d_pt2;
    complex->getVertPos(first_cell_2d_verts[0], &first_cell_2d_pt0);
    complex->getVertPos(first_cell_2d_verts[1], &first_cell_2d_pt1);
    complex->getVertPos(first_cell_2d_verts[2], &first_cell_2d_pt2);

    Vector4d first_cell_plane_eq;
    getPlaneEquation(first_cell_2d_pt0, first_cell_2d_pt1, first_cell_2d_pt2, &first_cell_plane_eq);
    // first_cell_plane_eq = -first_cell_plane_eq;
    // cout << first_cell_plane_eq << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt0) << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt1) << endl;
    // cout << evalPlaneEquation(first_cell_plane_eq, first_cell_2d_pt2) << endl;

    /* get the angles of other cofaces w.r.t the first coface */

    // TODO: this vector seems useless
    vector<Vector3d> cell_2d_pts(cell_1d_cofaces->size());
    vector<std::pair<Decimal,int>> cell_2d_order(cell_1d_cofaces->size());

    Vector3d first_cell_2d_vec;
    for (auto i = 0; i < cell_1d_cofaces->size(); i ++) {
        if (cmplx_type == CUBE_CMPLX) {
            get2DCellPtCubic(complex, cell_1d_cofaces->at(i), cell_1d_verts, &cell_2d_pts[i]);
        } else if (cmplx_type == SIMP_CMPLX) {
            int dosomething = 0;
        }

        if (i == 0) {
            cell_2d_order[i].first = 0;
            first_cell_2d_vec = cell_2d_pts[i] - cell_1d_mid_pt;
            first_cell_2d_vec.normalize();
        } else {
            Vector3d cell_2d_vec = cell_2d_pts[i] - cell_1d_mid_pt;
            cell_2d_vec.normalize();

            cell_2d_order[i].first = acos(first_cell_2d_vec.dot(cell_2d_vec));
            if (evalPlaneEquation(first_cell_plane_eq, cell_2d_pts[i]) < 0) {
                cell_2d_order[i].first = 2*M_PI - cell_2d_order[i].first;
            }
        }

        cell_2d_order[i].second = i;
    }

    struct {
        bool operator()(const std::pair<Decimal,int> &a,
            const std::pair<Decimal,int> &b) const {
            return a.first < b.first;
        }
    } comp;

    std::sort(cell_2d_order.begin(), cell_2d_order.end(), comp);

#ifdef DEBUG_OPT
    if (cell_2d_order[0].second != 0) {
        cout << "FATAL: cell_2d_order[0].second != 0 in addOrien2DCellPairs" << endl;
        exit(-1);
    }
#endif

    // for (auto pair : cell_2d_order) {
    //     cout << radian2Degree(pair.first) << ' ';
    // }
    // cout << endl;

    Vector3d cell_1d_vec = cell_1d_pt1 - cell_1d_pt0;
    cell_1d_vec.normalize();
    Vector3d cell_1d_ortho_pt = cell_1d_mid_pt + cell_1d_vec.cross(first_cell_2d_vec);

    bool cell_1d_0_to_1 = true;
    if (evalPlaneEquation(first_cell_plane_eq, cell_1d_ortho_pt) < 0) {
        cell_1d_0_to_1 = false;
    }

    // cout << "cell_1d_0_to_1: " << cell_1d_0_to_1 << endl;

    vector<int> cell_2d_verts1, cell_2d_verts2, canon_verts1, canon_verts2;
    canon_verts1.reserve(complex->getVertCnt(2));
    canon_verts2.reserve(complex->getVertCnt(2));

    for (auto i = 0; i < cell_2d_order.size(); i ++) {
        if (cmplx_type == CUBE_CMPLX) {
            auto cell_2d_order1 = cell_2d_order[i].second;
            auto cell_2d_order2 = cell_2d_order[ mod(i+1, cell_2d_order.size()) ].second;

            auto cell_2d_id1 = cell_1d_cofaces->at(cell_2d_order1);
            auto cell_2d_id2 = cell_1d_cofaces->at(cell_2d_order2);

            auto cell_2d_cofaces1 = complex->getCellCofaces(2, cell_2d_id1);
            auto cell_2d_cofaces2 = complex->getCellCofaces(2, cell_2d_id2);

            // check whether the two oriented 2-cells enclose a 3-cell
            if (vecIntersect(*cell_2d_cofaces1, *cell_2d_cofaces2)) {
                if (cell_1d_cofaces->size() == 2) {
                    auto degree1 = cell_2d_order[i].first;
                    auto degree2 = cell_2d_order[ mod(i+1, cell_2d_order.size()) ].first;

                    // cout << "deg:" << radian2Degree(degree1) << " " << radian2Degree(degree2)
                        // << " " << radian2Degree(radianSub(degree2, degree1)) << endl;

                    if (radianSub(degree2, degree1) <= M_PI) {
                        continue;
                    }
                } else {
                    continue;
                }
            }

            complex->getCellVerts(2, cell_2d_id1, &cell_2d_verts1);
            complex->getCellVerts(2, cell_2d_id2, &cell_2d_verts2);

            if (cell_1d_0_to_1) {
                // CAUTION: should guarantee the input vertices in canon order
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts1, cell_1d_verts[0], cell_1d_verts[1], &canon_verts1);
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts2, cell_1d_verts[1], cell_1d_verts[0], &canon_verts2);
            } else {
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts1, cell_1d_verts[1], cell_1d_verts[0], &canon_verts1);
                getCanonOrien2DCellVertsCubic(
                    cell_2d_verts2, cell_1d_verts[0], cell_1d_verts[1], &canon_verts2);
            }

            // cout << cell_2d_verts1 << endl;
            // cout << "paired 2-cells: " << canon_verts1 << ' ' << canon_verts2 << endl;

            auto id1 = orien_cell_2d_id_map->getId(canon_verts1);
            if (id1 < 0) {
                id1 = orien_cell_2d_id_map->addArrayLabel(canon_verts1);
            }

            auto id2 = orien_cell_2d_id_map->getId(canon_verts2);
            if (id2 < 0) {
                id2 = orien_cell_2d_id_map->addArrayLabel(canon_verts2);
            }

            orien_cell_2d_graph->addEdge(id1, id2);
        }
    }
}

void reconVoidBound(
    const ComplexType cmplx_type,
    CellComplex* complex,
    vector<array<int,2>>* cell_2d_void_map,
    const ExecOptions& ops,
    const std::string& file_prefix,
    int* void_cnt) {

    VertArrIdMap orien_cell_2d_id_map;
    orien_cell_2d_id_map.init(complex->getVertCnt(2));
    // cout << "complex->getVertCnt(2): " << complex->getVertCnt(2) << endl;

    Graph<int> orien_cell_2d_graph;

    /* get the number of boundary oriented 2-cells
       and reserve the size for the graph and the id map */

    int orien_cell_2d_cnt = 0;
    for (auto cell_2d_id = 0; cell_2d_id < complex->getCellSize(2); cell_2d_id ++) {
        if (complex->isCellValid(2, cell_2d_id)) {
            orien_cell_2d_cnt += 2 - complex->getCellCofaces(2, cell_2d_id)->size();
        }
    }

    orien_cell_2d_id_map.reserve(orien_cell_2d_cnt);
    orien_cell_2d_graph.reserve(orien_cell_2d_cnt);

    /* traverse all boundary 2-cells and their 1-faces
       to add all pairs of orented 2-cells */
    // TODO: traverse all 1-cells instead

    std::unordered_set<int>* visited_cells_1d = new std::unordered_set<int>;
    vector<int> cell_1d_verts, cell_2d_verts;

    for (auto cell_2d_id = 0; cell_2d_id < complex->getCellSize(2); cell_2d_id ++) {
        if (complex->isCellValid(2, cell_2d_id)) {
            if (complex->getCellCofaces(2, cell_2d_id)->size() < 2) {
                complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);

                for (auto i = 0; i < complex->getFaceCnt(2); i ++) {
                    complex->getFaceVerts(2, cell_2d_verts, i, &cell_1d_verts);
                    complex->toCanonVerts(1, &cell_1d_verts);
                    // cout << "  " << cell_1d_verts << endl;
                    auto cell_1d_id = complex->getCellIdNoConvCanon(1, cell_1d_verts);

                    if (visited_cells_1d->find(cell_1d_id) == visited_cells_1d->end()) {
                        visited_cells_1d->insert(cell_1d_id);
                        addOrien2DCellPairs(cmplx_type, complex, cell_1d_id, cell_1d_verts,
                            &orien_cell_2d_id_map, &orien_cell_2d_graph);
                    }
                }
            }
        }
    }

    delete visited_cells_1d;
    visited_cells_1d = NULL;

    if (ops.verbose) {
        cout << endl << "orien_cell_2d_id_map size stat:" << endl;
        orien_cell_2d_id_map.printLoadStat();
        cout << endl << "orien_cell_2d_graph: capacity="
            << orien_cell_2d_graph.adj_nodes_.capacity()
            << ",size=" << orien_cell_2d_graph.adj_nodes_.size() << endl;
    }

    // TODO: maybe can add a degree check for the graph: 4 for cubical complex?
    // orien_cell_2d_graph.print();
    // cout << endl;

    /* get connected components of the graph of oriented 2-cells by DFS */

    int void_id = 0;
    vector<bool> orien_cell_2d_visited;
    orien_cell_2d_visited.resize(orien_cell_2d_graph.size(), false);
    vector<int> node_stack;
    vector<int> orien_cell_2d;

    for (auto i = 0; i < orien_cell_2d_graph.size(); i ++) {
        if (orien_cell_2d_visited[i] == false) {
            node_stack = { i };
            orien_cell_2d_visited[i] = true;

            MeshWriter* mesh_writer;
            if (ops.write_intem) {
                mesh_writer = new MeshWriter(complex, complex->getVertCnt(2));
            }

            // cout << endl << "node_stack.capacity(): " << node_stack.capacity() << endl << endl;
            // cout << "void " << void_id << ": (";
            // cout << "void " << void_id << ":" << endl;

            while (!node_stack.empty()) {
                auto node = node_stack[node_stack.size() - 1];
                node_stack.pop_back();

                // cout << node << ",";
                orien_cell_2d_id_map.getArrayLabel(node, &orien_cell_2d);
                // cout << orien_cell_2d << endl;

                if (ops.write_intem) {
                    mesh_writer->addFace(orien_cell_2d);
                }

                complex->toCanonVerts(2, &orien_cell_2d);
                auto cell_2d_id = complex->getCellIdNoConvCanon(2, orien_cell_2d);

                if (cell_2d_void_map->at(cell_2d_id).at(0) < 0) {
                    cell_2d_void_map->at(cell_2d_id).at(0) = void_id + complex->getCellSize(3);
                } else if (cell_2d_void_map->at(cell_2d_id).at(1) < 0) {
                    cell_2d_void_map->at(cell_2d_id).at(1) = void_id + complex->getCellSize(3);
                } else {
                    cout << "FATAL cell_2d_void_map[cell_2d_id] both >= 0 in reconVoidBound" << endl;
                    exit(-1);
                }

                // auto cofaces = const_cast<vector<int>*>(complex->getCellCofaces(2, cell_2d_id));
                // cofaces->push_back(void_id + complex->getCellSize(3));

                for (auto j = 0; j < orien_cell_2d_graph.adj_nodes_[node].size(); j ++) {
                    auto adj_node = orien_cell_2d_graph.adj_nodes_[node][j];

                    if (orien_cell_2d_visited[adj_node] == false) {
                        node_stack.push_back(adj_node);
                        orien_cell_2d_visited[adj_node] = true;
                    }
                }
            }

            if (ops.write_intem) {
                mesh_writer->write(file_prefix + "_void" + std::to_string(void_id) + ".off");
                delete mesh_writer;
            }

            void_id ++;
            // cout << ")" << endl;
            // cout << endl;
        }
    }

    *void_cnt = complex->getCellSize(3) + void_id;

    if (ops.verbose) {
        cout << endl << "void count: " << void_id << endl;
    }
}

template<typename WeightType>
void computeMinCyc(
    CellComplex* complex,
    const GEdgeCellMap* gedge_cell_map,
    const int void_cnt,
    const int src,
    const int sink,
    const ExecOptions& ops,
    const std::string& file_prefix) {

    // if (ops.verbose) {
    //     cout << "vertex size:" << void_cnt + gedge_cell_map->size() << endl;
    // }

    // FlowGraph<int,WeightType> _graph(void_cnt + gedge_cell_map->size());

    // int edge_id = 0;
    // for (auto iter = gedge_cell_map->begin();
    //     iter != gedge_cell_map->end(); iter ++) {

    //     const std::pair<int,int>& edge = iter->first;

    //     WeightType w = 0;
    //     for (auto cell_2d_id : iter->second) {
    //         w += (WeightType)(complex->getWeight(2, cell_2d_id));
    //     }

    //     _graph.addEdge(edge.first, edge.second, w);
    //     _graph.addEdge(edge.second, void_cnt + edge_id, w);
    //     _graph.addEdge(void_cnt + edge_id, edge.first, w);

    //     edge_id ++;
    // }

    // return;

    /* compute the maximal flow */

    typedef boost::adjacency_list_traits < boost::vecS, boost::vecS, boost::directedS > Traits;

    typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::directedS,
    // boost::property < boost::vertex_name_t, std::string,
    boost::property < boost::vertex_index_t, int, // originally it's 'long'
    boost::property < boost::vertex_color_t, boost::default_color_type,
    boost::property < boost::vertex_distance_t, int, // originally it's 'long'
    boost::property < boost::vertex_predecessor_t, Traits::edge_descriptor > > > >,

    boost::property < boost::edge_capacity_t, WeightType,
    boost::property < boost::edge_residual_capacity_t, WeightType,
    boost::property < boost::edge_reverse_t, Traits::edge_descriptor > > > > FlowGraphBoost;

    if (ops.verbose) {
        cout << endl << "graph elem sizes: "
            << sizeof(Traits::vertex_descriptor) << " "
            << sizeof(Traits::edge_descriptor) << endl;
    }

    FlowGraphBoost* graph = new FlowGraphBoost;
    typename boost::property_map<FlowGraphBoost, boost::edge_capacity_t>::type
        capacity = get(boost::edge_capacity, *graph);
    typename boost::property_map<FlowGraphBoost, boost::edge_residual_capacity_t>::type
        residual_capacity = get(boost::edge_residual_capacity, *graph);
    typename boost::property_map<FlowGraphBoost, boost::edge_reverse_t>::type
        reverse_edge = get(boost::edge_reverse, *graph);

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        const std::pair<int,int>& edge = iter->first;
        Traits::edge_descriptor e1, e2;

        boost::tie(e1, boost::tuples::ignore)
            = add_edge(edge.first, edge.second, *graph);
        boost::tie(e2, boost::tuples::ignore)
            = add_edge(edge.second, edge.first, *graph);

        WeightType w = 0;
        for (auto cell_2d_id : iter->second) {
            w += (WeightType)(complex->getWeight(2, cell_2d_id));
        }

        // cout << edge << " w=" << w << endl;

        capacity[e1] = w;
        capacity[e2] = w;
        reverse_edge[e1] = e2;
        reverse_edge[e2] = e1;
    }

    cout << endl << "build flow network done" << endl;

    auto max_flow = boykov_kolmogorov_max_flow(*graph ,src, sink);
    if (ops.verbose) {
        cout << endl << "max_flow: " << (double)max_flow << endl;
    }

    /* do a DFS to get the min-cut */

    if (ops.verbose) {
        cout << "graph num_vertices: " << num_vertices(*graph) << endl;
    }

    vector<bool> visited;

    {
        visited.resize(num_vertices(*graph), false);
        visited[src] = true;
        vector<int> vert_stack = { src };

        while (!vert_stack.empty()) {
            auto v = vert_stack.back();
            vert_stack.pop_back();

            typename boost::graph_traits<FlowGraphBoost>::out_edge_iterator e_iter, e_end;
            boost::tie(e_iter, e_end) = out_edges(v, *graph);
            for (; e_iter != e_end; e_iter ++) {
                if (residual_capacity[*e_iter] > 0) {
                    int adj_v = target(*e_iter, *graph);
                    if (!visited[adj_v]) {
                        vert_stack.push_back(adj_v);
                        visited[adj_v] = true;
                    }
                }
            }
        }
    }

    delete graph;

    /* collect edges across the min-cut */

    MeshWriter mesh_writer(complex, complex->getVertCnt(2));
    vector<int> cell_2d_verts;

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        const std::pair<int,int>& edge = iter->first;
        if (visited[edge.first] != visited[edge.second]) {
            for (auto cell_2d_id : iter->second) {
                complex->getCellVerts(2, cell_2d_id, &cell_2d_verts);
                mesh_writer.addFace(cell_2d_verts);
            }
        }
    }

    mesh_writer.write(file_prefix + "_mincyc.off");
}

void getBitmapComplexCellVerts(const int cell_handle, const int cell_dim,
    const int x_res, const int y_res, const int z_res, vector<int>* verts) {

    const int x_cnt = 2*x_res + 1;
    const int y_cnt = 2*y_res + 1;

    const int z_coord = cell_handle / (x_cnt*y_cnt);
    const int left = cell_handle % (x_cnt*y_cnt);
    const int y_coord = left / x_cnt;
    const int x_coord = left % x_cnt;

    int vari_cnt = 0;

    int x_vari = 1;
    if (x_coord % 2 == 1) {
        x_vari = 2;
        vari_cnt ++;
    }

    int y_vari = 1;
    if (y_coord % 2 == 1) {
        y_vari = 2;
        vari_cnt ++;
    }

    int z_vari = 1;
    if (z_coord % 2 == 1) {
        z_vari = 2;
        vari_cnt ++;
    }

#ifdef DEBUG_OPT
    if (vari_cnt != cell_dim) {
        cout << "FATAL: getBitmapCC2dCellVerts dim not equal" << endl;
        exit(-1);
    }
#endif

    vector<int> temp_verts;
    for (auto x = x_coord/2; x < x_coord/2 + x_vari; x ++) {
        for (auto y = y_coord/2; y < y_coord/2 + y_vari; y ++) {
            for (auto z = z_coord/2; z < z_coord/2 + z_vari; z ++) {
                temp_verts.push_back(x + y*(x_res+1) + z*(x_res+1)*(y_res+1));
            }
        }
    }

    if (cell_dim == 2) {
        *verts = {temp_verts[0], temp_verts[1], temp_verts[3], temp_verts[2]};
    } else if (cell_dim == 3) {
        *verts = {temp_verts[0], temp_verts[1], temp_verts[3], temp_verts[2],
            temp_verts[4], temp_verts[5], temp_verts[7], temp_verts[6]};
    }
}

void printGEdgeCellMap(const GEdgeCellMap* gedge_cell_map, CellComplex* complex) {
    cout << "3-cells [" << complex->getCellCount(3) << "]:" << endl;
    for (int cell_3d_id = 0; cell_3d_id < complex->getCellSize(3); cell_3d_id ++) {
        if (complex->isCellValid(3, cell_3d_id)) {
            vector<int> verts;
            complex->getCellVerts(3, cell_3d_id, &verts);
            cout << cell_3d_id << ": " << verts << endl;
        }
    }
    cout << endl;

    for (auto iter = gedge_cell_map->begin();
        iter != gedge_cell_map->end(); iter ++) {

        cout << iter->first << ":" << endl;
        for (auto cell_2d_id : iter->second) {
            vector<int> verts;
            complex->getCellVerts(2, cell_2d_id, &verts);
            cout << "  " << verts << endl;
        }
    }
}

void getCell2dVoidIds(
    const int cell_2d_id, CellComplex* complex,
    vector<array<int,2>>* cell_2d_void_map, vector<int>* void_ids) {

    void_ids->clear();

    auto cofaces = complex->getCellCofaces(2, cell_2d_id);
    for (auto cell_3d_id : *cofaces) {
        void_ids->push_back(cell_3d_id);
    }

    for (auto void_id : cell_2d_void_map->at(cell_2d_id)) {
        if (void_id >= 0) {
            void_ids->push_back(void_id);
        }
    }

    if (void_ids->size() != 2) {
        cout << "FATAL: void_ids.size() != 2 in getCell2dVoidIds" << endl;
        exit(-1);
    }
}


#ifndef _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_
#define _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_

#include <cmath>
#include <vector>

#include <Eigen/Core>

namespace ldaplusplus {
namespace optimization {

/**
 * SecondOrderLogisticRegressionApproximation is a second order taylor
 * approximation to the expectation of the logistic loss function of a random
 * variable.
 *
 * We use this class to approximate the following equation in the lower bound
 * of the likelihood of an LDA model. \f$q\f$ is the variational distribution
 * used and \f$\bar{z}\f$ is a random variable (the mean of the topic
 * assignments). The equation below is for a single document.
 *
 * \f[
 *     \mathbb{E}_q\left[
 *         \eta_{y_n}^T \bar{z} -
 *         \log\left( \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T \bar{z}) \right)
 *         \right] \approx
 *         \eta_{y_n}^T \mathbb{E}_q[\bar{z}] -
 *         \log \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T \mathbb{E}_q[\bar{z})\left(
 *         1 + \frac{1}{2} \eta_{\hat{y}}^T \mathbb{V}_q[\bar{z}] \eta_{\hat{y}}
 *         \right)
 * \f]
 */
template <typename Scalar>
class SecondOrderLogisticRegressionApproximation
{
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;

    public:
        /**
         * @param X     The documents defining the minimization problem (\f$X
         *              \in \mathbb{R}^{D \times N}\f$)
         * @param X_var A vector containing the variance matrix for each
         *              document (\f$X_{\text{var}} \in \mathbb{R}^{N \times D
         *              \times D}\f$)
         * @param y     The class indexes for each document (\f$y \in
         *              \mathbb{N}^N\f$)
         * @param Cy    A different weight for each class in the optimization
         *              problem
         * @param L     The L2 regularization penalty for the weights
         */
        SecondOrderLogisticRegressionApproximation(
            const MatrixX &X,
            const std::vector<MatrixX> &X_var,
            const Eigen::VectorXi &y,
            VectorX Cy,
            Scalar L
        );

        /**
         * @param X     The documents defining the minimization problem (\f$X
         *              \in \mathbb{R}^{D \times N}\f$)
         * @param X_var A vector containing the variance matrix for each
         *              document (\f$X_{\text{var}} \in \mathbb{R}^{N \times D
         *              \times D}\f$)
         * @param y     The class indexes for each document (\f$y \in
         *              \mathbb{N}^N\f$)
         * @param L     The L2 regularization penalty for the weights
         */
        SecondOrderLogisticRegressionApproximation(
            const MatrixX &X,
            const std::vector<MatrixX> &X_var,
            const Eigen::VectorXi &y,
            Scalar L
        );

        /**
         * The value of the objective function to be minimized.
         *
         * \f$N\f$ is the number of documents (different vectors), \f$X_n \in
         * \mathbb{R}^D\f$ is the nth document, \f$\eta_y \in \mathbb{R}^D\f$
         * is the weights vector for the class \f$y\f$ defining the hyperplane
         * that separates class \f$y\f$ from all the other, \f$y_n\f$
         * is the class of the nth document and finally \f$X_n^{\text{var}} \in
         * \mathbb{R}^{D \times D}\f$ is the variance of the nth document (see
         * the class description).
         *
         * \f[
         *     J = - \sum_{n=1}^N C_{y_n} \left(
         *         \eta_{y_n}^T X_n -
         *         \log \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n) \left(
         *         1 +
         *         \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *         \right)
         *         \right) +
         *         \frac{L}{2} \left\| \eta \right\|_F^2
         * \f]
         *
         * @param eta The weights of the linear model (\f$\eta \in
         *            \mathbb{R}^{D \times Y}\f$)
         */
        Scalar value(const MatrixX &eta) const;

        /**
         * The gradient of the objective function implemented in value().
         *
         * We use \f$I(y) \in \mathbb{R}^Y\f$ as the indicator vector of
         * \f$y\f$ (a vector with all the values 0 except at the yth position).
         *
         * \f[
         *     \nabla_{\eta} J = - \sum_{n=1}^N C_{y_n} \left(
         *         X_n I(y_n)^T -
         *         \frac{
         *              \sum_{\hat{y}=1}^Y \left(\left(
         *              X_n \exp(\eta_{\hat{y}}^T X_n) \left(
         *              1 +
         *              \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *              \right)\right) + \left(
         *              \frac{1}{2}
         *              \exp(\eta_{\hat{y}}^T X_n) \eta_{\hat{y}}^T \left(
         *              X_n^{\text{var}} + \left(X_n^{\text{var}}\right)^T
         *              \right)\right)
         *              \right) I(\hat{y})^T
         *              }
         *              {
         *              \sum_{\hat{y}=1}^Y \exp(\eta_{\hat{y}}^T X_n) \left(
         *              1 +
         *              \frac{1}{2} \eta_{\hat{y}}^T X_n^{\text{var}} \eta_{\hat{y}}
         *              \right)
         *              }
         *         \right) +
         *         L \eta
         * \f]
         *
         * @param eta  The weights of the linear model (\f$\eta \in
         *             \mathbb{R}^{D \times Y}\f$)
         * @param grad A matrix of dimensions equal to \f$\eta\f$ that will
         *             hold the result
         */
        void gradient(const MatrixX &eta, Eigen::Ref<MatrixX> grad) const;

    private:
        const MatrixX &X_;
        const std::vector<MatrixX> &X_var_;
        const Eigen::VectorXi &y_;
        Scalar L_;
        VectorX Cy_;
};

}  // namespace optimization
}  // namespace ldaplusplus

#endif // _LDAPLUSPLUS_OPTIMIZATION_SECOND_ORDER_LOGISTIC_REGRESSION_APPROXIMATION_HPP_


#include <iostream>
#include <Eigen/Dense>
#include <cassert>
#include <vector>
#include "Option.h"
#include <cmath>
#include "Regression.h"
#include "FileManagement.h"
#include <chrono>
#include "nonuniform_grid.h"
#include <random>
#include "Simulation.h"
#include <tuple>
#include "HestonOption.h"
#include "OptimalExecution.h"

int main() {

  // Generation of option prices
  // auto o = Option("Call");
  // o.set_tn(1);
  // o.set_s_max(100);
  // o.set_numdiff_t(10);
  // o.set_numdiff_s(10);
  // o.set_volatility(.1);
  // o.set_interest_rate(.15);
  // o.set_strike(50);
  // o.set_stock_boundary_condition("Dirichlet");
  // o.fixed_difference_step();
  //
  // o.compute_solution_grid("Crank-Nicholson");
  // o.print_solution_grid();

  // Heston option Monte-Carlo Simulation
  // auto h = HestonOption("Call", 1, 10, 100, 50);
  // h.set_params_stock_process(45, .1);
  // h.set_prams_variance_process(.2, 1, 1, .25, .5);
  // auto pr = h.compute_price();
  //
  // std::cout << pr << "\n";

  // Generation of optimal execution paths
   // double a_coeff_, double b_coeff_, double sigma_coeff_, double k_coeff_, double phi_coeff_
   // auto o = OptimalExecution(.6,.1, 0.1,.1,.6);
   // // num_diff_t_, int num_diff_q_
   // o.set_num_steps(100,100);
   // // true_impact, trading_speed, impact_nonlinearity
   // o.compute_liquidation("Linear", 1);
   // Eigen::VectorXd h = o.get_value_process();
   // std::cout << "Value Process\n" << h << "\n";
   // Eigen::VectorXd j = o.get_optimal_speed_process();
   // std::cout << "Speed Process\n" << j << "\n";
   // Eigen::VectorXd s = o.get_stock_process();
   // std::cout << "Stock Process\n" << s << "\n";
   // Eigen::VectorXd c = o.get_cash_process();
   // std::cout << "Cash Process\n" << c << "\n";
   // Eigen::VectorXd i = o.get_inventory_process();
   // std::cout << "Inventory Process\n" << i << "\n";
   //
   // std::cout << o.get_value_matrix();

    // Generate 500 optimal execution scenarios and save it in the csv files
    // Eigen::MatrixXd nonlinear_speed_csv_value,nonlinear_speed_csv_optimal_speed, nonlinear_speed_csv_stock, nonlinear_speed_csv_cash, nonlinear_speed_csv_inventory;
    // nonlinear_speed_csv_value.resize(101,100); nonlinear_speed_csv_optimal_speed.resize(101,100); nonlinear_speed_csv_stock.resize(101,100); nonlinear_speed_csv_cash.resize(101,100); nonlinear_speed_csv_inventory.resize(101,100);
    // Eigen::MatrixXd linear_speed_csv_value,linear_speed_csv_optimal_speed, linear_speed_csv_stock, linear_speed_csv_cash, linear_speed_csv_inventory;
    // linear_speed_csv_value.resize(101,100); linear_speed_csv_optimal_speed.resize(101,100); linear_speed_csv_stock.resize(101,100); linear_speed_csv_cash.resize(101,100); linear_speed_csv_inventory.resize(101,100);
    // for (int sim = 0; sim < 100; ++sim) {
        // Trading speed: Nonlinear, true impact: Nonlinear
        // auto o = OptimalExecution(.08,.06, .1,.08,.06);
        // o.set_num_steps(100,100);
        // o.compute_liquidation("Nonlinear", "Nonlinear", .6);
        // Eigen::VectorXd value_process = o.get_value_process();
        // nonlinear_speed_csv_value.col(sim) = value_process;
        // Eigen::VectorXd optimal_speed_process = o.get_optimal_speed_process();
        // nonlinear_speed_csv_optimal_speed.col(sim) = optimal_speed_process;
        // Eigen::VectorXd stock_process = o.get_stock_process();
        // nonlinear_speed_csv_stock.col(sim) = stock_process;
        // Eigen::VectorXd cash_process = o.get_cash_process();
        // nonlinear_speed_csv_cash.col(sim) = cash_process;
        // Eigen::VectorXd inventory_process = o.get_inventory_process();
        // nonlinear_speed_csv_inventory.col(sim) = inventory_process;

        // Trading speed: Linear, true impact: Nonlinear
        // auto ol = OptimalExecution(.6,.1, 0.1,.1,.6);
        // ol.set_num_steps(100,100);
        // ol.compute_liquidation("Linear", 1);
        // Eigen::VectorXd value_process_lin = ol.get_value_process();
        // linear_speed_csv_value.col(sim) = value_process_lin;
        // Eigen::VectorXd optimal_speed_process_lin = ol.get_optimal_speed_process();
        // linear_speed_csv_optimal_speed.col(sim) = optimal_speed_process_lin;
        // Eigen::VectorXd stock_process_lin = ol.get_stock_process();
        // linear_speed_csv_stock.col(sim) = stock_process_lin;
        // Eigen::VectorXd cash_process_lin = ol.get_cash_process();
        // linear_speed_csv_cash.col(sim) = cash_process_lin;
        // Eigen::VectorXd inventory_process_lin = ol.get_inventory_process();
        // linear_speed_csv_inventory.col(sim) = inventory_process_lin;
    // }
    // saveData("data/nonlinear_speed_csv_value.csv", nonlinear_speed_csv_value);
    // saveData("data/nonlinear_speed_csv_optimal_speed.csv", nonlinear_speed_csv_optimal_speed);
    // saveData("data/nonlinear_speed_csv_stock.csv", nonlinear_speed_csv_stock);
    // saveData("data/nonlinear_speed_csv_cash.csv", nonlinear_speed_csv_cash);
    // saveData("data/nonlinear_speed_csv_inventory.csv", nonlinear_speed_csv_inventory);

    // saveData("data/linear_speed_csv_value.csv", linear_speed_csv_value);
    // saveData("data/linear_speed_csv_optimal_speed.csv", linear_speed_csv_optimal_speed);
    // saveData("data/linear_speed_csv_stock.csv", linear_speed_csv_stock);
    // saveData("data/linear_speed_csv_cash.csv", linear_speed_csv_cash);
    // saveData("data/linear_speed_csv_inventory.csv", linear_speed_csv_inventory);

    return 0;
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <limits>

TEST(AgradRev, ibeta_vvv) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a, b, c);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.436993, grad_f[0]);
  EXPECT_FLOAT_EQ(0.7779751, grad_f[1]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b.val(), c.val()), grad_f[2]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a, b, c);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.03737671, grad_f[0]);
  EXPECT_FLOAT_EQ(0.02507405, grad_f[1]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b.val(), c.val()), grad_f[2]);
}
TEST(AgradRev, ibeta_vvd) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  AVAR b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a, b);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.436993, grad_f[0]);
  EXPECT_FLOAT_EQ(0.7779751, grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a, b);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.03737671, grad_f[0]);
  EXPECT_FLOAT_EQ(0.02507405, grad_f[1]);
}
TEST(AgradRev, ibeta_vdv) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  double b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a, c);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.436993, grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b, c.val()), grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a, c);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.03737671, grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a.val(), b, c.val()), grad_f[1]);
}
TEST(AgradRev, ibeta_vdd) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  AVAR a = 0.6;
  double b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(a);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.436993, grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(a);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(-0.03737671, grad_f[0]);
}
TEST(AgradRev, ibeta_dvv) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(b, c);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(0.7779751, grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b.val(), c.val()), grad_f[1]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(b, c);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(0.02507405, grad_f[0]);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b.val(), c.val()), grad_f[1]);
}
TEST(AgradRev, ibeta_dvd) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  AVAR b = 0.3;
  double c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(b);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(0.7779751, grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(b);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(0.02507405, grad_f[0]);
}
TEST(AgradRev, ibeta_ddv) {
  using stan::math::ibeta;
  using stan::math::ibeta;
  using stan::math::var;

  using boost::math::ibeta_derivative;

  double a = 0.6;
  double b = 0.3;
  AVAR c = 0.5;
  AVAR f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.3121373, f.val());

  AVEC x = createAVEC(c);
  VEC grad_f;
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b, c.val()), grad_f[0]);

  a = 3;
  b = 2;
  c = 0.2;
  f = ibeta(a, b, c);
  EXPECT_FLOAT_EQ(0.0272, f.val());
  x = createAVEC(c);
  f.grad(x, grad_f);
  EXPECT_FLOAT_EQ(ibeta_derivative(a, b, c.val()), grad_f[0]);
}

struct ibeta_fun {
  template <typename T0, typename T1, typename T2>
  inline typename stan::return_type<T0, T1, T2>::type operator()(
      const T0& arg1, const T1& arg2, const T2& arg3) const {
    return ibeta(arg1, arg2, arg3);
  }
};

TEST(AgradRev, ibeta_NaN) {
  ibeta_fun ibeta_;
  test_nan(ibeta_, 0.6, 0.3, 0.5, true, false);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 0.6;
  AVAR b = 0.3;
  AVAR c = 0.5;
  test::check_varis_on_stack(stan::math::ibeta(a, b, c));
  test::check_varis_on_stack(stan::math::ibeta(a, b, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(a, 0.3, c));
  test::check_varis_on_stack(stan::math::ibeta(a, 0.3, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(0.6, b, c));
  test::check_varis_on_stack(stan::math::ibeta(0.6, b, 0.5));
  test::check_varis_on_stack(stan::math::ibeta(0.6, 0.3, c));
}


#include <Rcpp.h>
#include <RcppEigen.h>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
using namespace Rcpp;
using namespace Eigen;

// [[Rcpp::depends(RcppEigen)]]
//

typedef Eigen::MappedSparseMatrix<double> MSpMat;
typedef Eigen::SparseMatrix<double> SpMat;
Eigen::SimplicialLLT <Eigen::SparseMatrix<double>, Eigen::Lower, Eigen::NaturalOrdering<int>> cholesky;

// [[Rcpp::export]]
double bym_scale(const SEXP &Q_) {

    //Map SparseMatrix
    const SpMat Q(Rcpp::as<MSpMat>(Q_));

    MatrixXd L = cholesky.compute(Q).matrixL();
    MatrixXd Sigma;
    Sigma.setZero(Q.rows(),Q.cols());
    Sigma.diagonal() = 1 / pow(L.diagonal().array(), 2);
    int n = Sigma.rows();
    for(int i = (n-2); i >= 0; --i){
        for(int j = (n-1); j >= i; --j){
            Sigma(i,j) -= 1.0 / L(i,i) * Sigma.col(j).tail(n-i-1).dot(L.col(i).tail(n-i-1));
            Sigma(j,i) = Sigma(i,j);
        }
    }

    MatrixXd A = Eigen::MatrixXd::Constant(1,n,1);
    MatrixXd W = Sigma * A.transpose();
    Sigma = Sigma - W * (A*W).inverse() * W.transpose();

    return(exp(Sigma.diagonal().array().log().mean()));
}


/**
 * @file GaussianHyperparameters.cpp
 * @author Jan Nguyen
 * @date 11.05.20
 */

#include "GaussianHyperparameters.h"

#include <Eigen/Cholesky>

#include "GaussianProcess.h"

void autopas::GaussianHyperparameters::precalculate(double sigma, const std::vector<Eigen::VectorXd> &inputs,
                                                    const Eigen::VectorXd &outputs) {
  size_t size = outputs.size();
  // mean of output shifted to zero
  Eigen::VectorXd outputCentered = outputs - mean * Eigen::VectorXd::Ones(size);

  Eigen::MatrixXd covMat(size, size);
  // calculate covariance matrix
  for (size_t i = 0; i < size; ++i) {
    covMat(i, i) = GaussianProcess::kernel(inputs[i], inputs[i], theta, dimScales) + sigma;
    for (size_t j = i + 1; j < size; ++j) {
      covMat(i, j) = covMat(j, i) = GaussianProcess::kernel(inputs[i], inputs[j], theta, dimScales);
    }
  }

  // cholesky decomposition
  Eigen::LLT<Eigen::MatrixXd> llt = covMat.llt();
  Eigen::MatrixXd l = llt.matrixL();

  // precalculate inverse of covMat and weights for predictions
  covMatInv = llt.solve(Eigen::MatrixXd::Identity(size, size));
  weights = covMatInv * outputCentered;

  // likelihood of evidence given parameters
  score = std::exp(-0.5 * outputCentered.dot(weights)) / l.diagonal().prod();

  if (std::isnan(score)) {
    // error score calculation failed
    utils::ExceptionHandler::exception("GaussianProcess: invalid score ", score);
  }
}


/**
 * @file  maximumprinciple.cc
 * @brief NPDE homework "MaximumPrinciple" code
 * @author Oliver Rietmann
 * @date 25.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "maximumprinciple.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <vector>

namespace MaximumPrinciple {

/**
 * @brief Assembly on a tensor product mesh
 *
 * Compute the global Galerkin matrix from the local
 * element matrix.
 *
 * @param M Number of interior vertices in x and y direction.
 * @param B_K Local element matrix.
 * @return Global Galerkin matrix of size M^2 times M^2.
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<double> assemble(int M, const Eigen::Matrix3d &B_K) {
  int M2 = M * M;
  Eigen::SparseMatrix<double> A(M2, M2);
  //====================

  // allocate memory
  A.reserve(7);

  // Get |K| - note that each cell has the same shape, so we can
  // get it outside of the loop
  double h = 1.0/(M+1);
  double K = 0.5*h*h;

  // Looper over all nodes
  for(int i=0; i<M2; ++i) {

    A.coeffRef(i,j) = v;
  }

  //====================
  return A;
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<double> computeGalerkinMatrix(int M, double c) {
  Eigen::Matrix3d B_K;
  //====================
  double h = 1./(M+1);
  double K = 0.5*h*h;

  Eigen::Matrix3d A_K;
  A_K << 2.0, -1.0, -1.0,
        -1.0,  2.0,  0.0
        -1.0,  0.0,  2.0;
  A_K *= 0.5;

  Eigen::Matrix3d M_K;
  M_K << 2.0, 1.0, 1.0,
         1.0, 2.0, 1.0,
         1.0, 1.0, 2.0;
  M_K *= K/12.0;

  B_K = (1.0 - c)*A_K + c*M_K;
  //====================
  return assemble(M, B_K);
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_4 */
Eigen::SparseMatrix<double> computeGalerkinMatrixTR(int M, double c) {
  Eigen::Matrix3d B_K;
  //====================
  // Your code goes here
  //====================
  return assemble(M, B_K);
}
/* SAM_LISTING_END_4 */

}  // namespace MaximumPrinciple


//Type:     Source file (Testing)
//User:     n0688119 (Elliot Harding)
//Purpose:  Testing the function route::maxGradient()

/*
                **Documentation**

This test file contains 14 tests for the function route::maxGradient(). The tests were chosen
in order to test the full functionality of maxGradient, ensuring that it is implimented correctly.
A range of data is used within the tests, this includes typical data, atypical data as well as
incorrect data. This way the expected results from all types of data input can be tested.

In terms of atypical data, the extremes of input data can be tested. Once the edges of correct
data is tested, it is safe to assume that input data inbetween the edge cases will also work
correctly.
 */

//Includes:
#include <boost/test/unit_test.hpp>
#include "logs.h"
#include "route.h"
#include "track.h"

//namespaces:
using namespace GPS;

//Public variables:
const bool isFileName = true;

//Test cases:
BOOST_AUTO_TEST_SUITE(Route_maxGradient)

/*
Test 1:
created using :     gpx log made manually
Description   :     This test case tests normal data, with variing elevation changes, the function should return
                    the largest gradient between two points.
GPX contains   :    Typical data, with varing elevation changes
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_ChangesInElevation){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_differentChangesInElevation.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),89.7426543,1);
}

/*
Test 2:
created using :     gpx log made manually
Description   :     Testing if the function finds the smallest negative gradient, given that all the gradients between
                    points are negative.
GPX contains   :    Positions all lower in elevation than the previous point, some to a greater extent
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_SmallestNegativeGradient){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_smallestNegativeGradient.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),-65.93637193,1);
}

/*
Test 3:
created using  :    generated via source function
Description    :    Testing if the function handles no change in latitude or longitude,
                    as this would result in a division by zero as there is not change in
                    distance(from a birds eye view)
GPX contains   :    Assortment of positions, all with the same latitude and longitude
Expected result:    function returns 90 degrees (-90 degrees in the case of a lower elevation)

    !!!!TO NOTE!!!!
    First I was going to do a boost check throw, but the teacher pointed out to me that
    if the latitude and logitude are the same it is not an error but a feature, so instead I
    compare with 90 degrees

*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_noLatOrLongChange){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_noLatOrLongChange.gpx",isFileName);
    BOOST_CHECK_EQUAL(route.maxGradient(),90);
}

/*
Test 4:
created using :     generated via source function
Description   :     Testing the function returns 0 (meaning the greatest gradient change is 0), if there are no changes
                    in elevation of the route
GPX contains   :    Positions all the same elevation
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_NoElevationChange){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_noElevationChange.gpx",isFileName);
    BOOST_CHECK_EQUAL(route.maxGradient(),0);
}

/*
Test 5:
created using  :    generated via source function
Description    :    Testing the function handles a route if successive points have a positive gradient
Expected result:    The largest positive gradient
GPX contains   :    Positions all higher in elevation than the previous point with same increase between each
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_constantUpwards){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_constantUpwards.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),6.39971588,1);
}

/*
Test 6:
created using  :    generated via source function
Description    :    Testing the function handles a route if successive points have a negative gradient
Expected result:    The largest negative gradient
GPX contains   :    Positions all lower in elevation than the previous point with same decrease between each
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_constantDownwards){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_constantDownwards.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),-6.39971588,1);
}

/*
Test 7:
created using  :    generated via source function
Description    :    Testing the function handles a route gradient which goes up then down
Expected result:    The largest positive gradient
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_UpThenDown){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_UpThenDown.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),6.39971588,1);
}

/*
Test 8:
created using  :    generated via source function
Description    :    Testing the function handles a route gradient which goes down then up
Expected result:    The largest positive gradient
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_DownThenUp){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_DownThenUp.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),6.39971588,1);
}

/*
Test 9:
created using  :    generated via source function
Description    :    Testing the function can handle large elevation values
Expected result:    The largest positive gradient
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_largeValues){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_largeValues.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),-6.39971588,0.1);
}

/*
Test 10:
created using  :    generated via source function
Description    :    Testing the function can handle precise decimal elevation values
Expected result:    The largest positive gradient
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_smallValues){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_smallValues.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),-6.39971588,0.1);
}

/*
Test 11:
created using  :    generated via source function
Description    :    Testing the function can handle small decimal elevation values
Expected result:    The largest positive gradient
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_smallElevationChanges){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_smallElevationChange.gpx",isFileName);
    BOOST_CHECK_CLOSE(route.maxGradient(),6.39971588,0.1);
}

/*
Test 12:
created using  :    generated via source function
Description    :    Testing if the function handles only one point, as two are needed to find a gradient
GPX contains   :    Only one position
Expected result:    std::invalid_argument thrown
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_onePoint){
    Route route = Route(LogFiles::GPXRoutesDir + "n0688119_onePoint.gpx",isFileName);
    BOOST_CHECK_THROW(route.maxGradient(),std::invalid_argument);
}

/*
Test 13:
created using  :    gpx log made manually
Description    :    Testing if input that is not numbers is dealt with correctly
GPX contains   :    Letters are input instead of numbers for the elevation
Expected result:    std::invalid_argument thrown
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_incorrectInput){
    BOOST_CHECK_THROW(Route route = Route(LogFiles::GPXRoutesDir + "n0688119_incorrectInput.gpx",isFileName),std::invalid_argument);
}

/*
Test 14:
created using  :    generated via source function
Description    :    Testing if the function handles empty input correctly
GPX contains   :    No data
Expected result:    std::domain_error thrown
*/
BOOST_AUTO_TEST_CASE(n0688119_Route_maxGradient_noInput){
    BOOST_CHECK_THROW(Route route = Route(LogFiles::GPXRoutesDir + "n0688119_noInput.gpx",isFileName),std::domain_error);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/saturated.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i+1) : T(N+i+1);
    b[i] = bs::sqr(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_EQUAL(bs::sqr(aa1), bb);
}

STF_CASE_TPL("Check sqr on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

template <typename T, std::size_t N, typename Env>
void tests(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::saturated_(bs::sqr)(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_EQUAL(bs::saturated_(bs::sqr)(aa1), bb);
}

STF_CASE_TPL("Check saturated sqr on pack" , STF_NUMERIC_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  tests<T, N>(runtime);
  tests<T, N/2>(runtime);
  tests<T, N*2>(runtime);
}

STF_CASE_TPL (" sqr real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sqr;
  using p_t = bs::pack<T>;
  using r_t = decltype(sqr(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(sqr(bs::Inf<p_t>()), bs::Inf<p_t>());
  STF_EQUAL(sqr(bs::Minf<p_t>()), bs::Inf<p_t>());
  STF_IEEE_EQUAL(sqr(bs::Nan<p_t>()), bs::Nan<p_t>());
#endif
  STF_EQUAL(sqr(bs::Mone<p_t>()), bs::One<p_t>());
  STF_EQUAL(sqr(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(sqr(bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for floating_

STF_CASE_TPL (" sqr unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sqr;
  using p_t = bs::pack<T>;
  using r_t = decltype(sqr(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(sqr(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(sqr(bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" sqr signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::sqr;
  using p_t = bs::pack<T>;
  using r_t = decltype(sqr(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(sqr(bs::Mone<p_t>()), bs::One<p_t>());
  STF_EQUAL(sqr(bs::One<p_t>()), bs::One<p_t>());
  STF_EQUAL(sqr(bs::Zero<p_t>()), bs::Zero<p_t>());
} // end of test for signed_int_


/*
 * KolmogorovComputer.cpp
 *
 *  Created on: Jan 28, 2014
 *      Author: jan
 *
 *      This class handles everything that deals with the Kolmogorov distance or Kolmogorov function. In particular this class provides the number of needed
 *      simulations S for each provided tolerance epsilon, and the other way around. It also computes the distance between a computed trajectory and the original data.
 *      The parameters beta  comes from: d(Y_S, X_M) < epsilon ==> P(d(Y, X) > epsilon) <= beta for further details see Lillacci & Khammash 2013
 *      The parameter kappa is the computed tolerance corresponding to the number of samples in the original dataset
 */

#include "KolmogorovComputer.h"

#include <cmath>
#include <iostream>
#include <cstdlib>

#include <Eigen/Dense>
#include <boost/math/tools/roots.hpp>

#include "IllegalArgumentException.h"

namespace INSIGHTv3 {

// Constructor for approximated kappa
KolmogorovComputer::KolmogorovComputer(
		const std::vector<EiVector>& original_data, double beta,
		double tolerance) {

	_kappa = getKappaForMApprox(original_data[0].size(), beta);
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

// Constructor for exact kappa
KolmogorovComputer::KolmogorovComputer(double kappa_tolerance,
		const std::vector<EiVector>& original_data, double beta,
		double tolerance) {

	std::cout
			<< "Exact kappa for the KolmovorovComputer is being computed. M is "
			<< original_data[0].size()
			<< ". The tolerance for the kolmogorov function root solver is "
			<< kappa_tolerance
			<< ". This may take a long time. Consider using one of the other constructors that use a precomputed kappa, or compute kappa only approximately"
			<< std::endl;
	_kappa = getKappaForM(original_data[0].size(), beta, kappa_tolerance);
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

// Constructor for precomputed kappa
KolmogorovComputer::KolmogorovComputer(
		const std::vector<EiVector>& original_data, double beta,
		double tolerance, double kappa) {

	_kappa = kappa;
	_beta = beta;
	_tolerance = tolerance;
	_init(original_data);
}

KolmogorovComputer::~KolmogorovComputer() {
}

void KolmogorovComputer::_init(const std::vector<EiVector>& original_data) {
	for (size_t column = 0; column < original_data.size(); column++) {
		EiVector single_column = original_data[column];
		std::sort(single_column.data(),
				single_column.data() + single_column.size());
		_sorted_Data.push_back(single_column);
	}

}

double KolmogorovComputer::distTwoSample(const EiVector& first_sample,
		const size_t column_of_second_sample) {

	size_t j1 = 0, j2 = 0;
	double distance = 0.0, d1, d2, dt, fn1 = 0.0, n1 =
			(double) first_sample.size(), fn2 = 0.0, n2 =
			(_sorted_Data[column_of_second_sample]).size();

	EiVector first_sample_sorted = EiVector(first_sample);
	std::sort(first_sample_sorted.data(),
			first_sample_sorted.data() + first_sample_sorted.size());

	EiVector* second_sample_sorted = (&_sorted_Data[column_of_second_sample]);

	while (j1 < n1 && j2 < n2) {
		if ((d1 = first_sample_sorted(j1))
				<= (d2 = (*second_sample_sorted)(j2))) {

			EiVectorRef first_sample_tail = first_sample_sorted.tail(
					first_sample_sorted.size() - j1);

			if (d1 == d2) {
				j1 = j1 + _findLargestIndex(d2, first_sample_tail) + 1;
			} else {
				j1 = j1 + _findSmallestPredecesorIndex(d2, first_sample_tail)
						+ 1;
			}
		}
		if (d2 <= d1) {
			EiVectorRef second_sample_tail = second_sample_sorted->tail(
					second_sample_sorted->size() - j2);
			if (d1 == d2) {
				j2 = j2 + _findLargestIndex(d1, second_sample_tail) + 1;
			} else {
				j2 = j2 + _findSmallestPredecesorIndex(d1, second_sample_tail)
						+ 1;
			}
		}
		fn2 = (j2) / n2;
		fn1 = (j1) / n1;
		if ((dt = fabs((double) fn2 - fn1)) > distance)
			distance = dt;
	}

	return distance;
}

size_t KolmogorovComputer::_findSmallestPredecesorIndex(const double value,
		EiVectorRef& _vector) {
	if (_vector.size() == 1) {
		return 0;
	} else {
		size_t median_index = floor(_vector.size() / 2.0);
		double median = _vector(median_index);
		if (value > median) {

			EiVectorRef upper_sub_vector = _vector.tail(
					_vector.size() - median_index);

			return median_index
					+ _findSmallestPredecesorIndex(value, upper_sub_vector);
		} else {
			EiVectorRef lower_sub_vector = _vector.head(median_index);
			return _findSmallestPredecesorIndex(value, lower_sub_vector);
		}
	}

}

size_t KolmogorovComputer::_findLargestIndex(const double value,
		EiVectorRef& _vector) {
	if (_vector.size() == 1) {
		return 0;
	} else {
		size_t median_index = floor(_vector.size() / 2.0);
		double median = _vector(median_index);
		if (value >= median) {
			EiVectorRef upper_sub_vector = _vector.tail(
					_vector.size() - median_index);

			return median_index + _findLargestIndex(value, upper_sub_vector);
		} else {
			EiVectorRef lower_sub_vector = _vector.head(median_index);
			return _findLargestIndex(value, lower_sub_vector);
		}
	}
}

double KolmogorovComputer::getKappaForMApprox(int M, double beta) {
	double alpha = 1 - sqrt(1.0 - beta);
	return sqrt(-(1 / (2.0 * M)) * log(alpha / 2.0));
}

double KolmogorovComputer::getKappaForM(int M, double beta, double tolerance) {
	return kolmogorovCdfInverse(M, 1 - (1 - sqrt(1 - beta)), tolerance);
}

double KolmogorovComputer::getThresholdForSApprox(int s, double beta, int M) {
	double kappa = getKappaForMApprox(M, beta);
	return getThresholdForSApprox(s, beta, kappa);
}

double KolmogorovComputer::getThresholdForSApprox(int s, double beta,
		double kappa) {
	double alpha = 1 - sqrt(1.0 - beta);
	return sqrt(-(1 / (2.0 * s)) * log(alpha / 2.0)) + kappa;
}

double KolmogorovComputer::getThresholdForS(int s, double beta, double kappa,
		double tolerance) {
	return kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance) + kappa;
}

int KolmogorovComputer::getSForThresholdApprox(double threshold, double beta,
		int M) {
	double kappa = getKappaForMApprox(M, beta);
	return getSForThresholdApprox(threshold, beta, kappa);
}

int KolmogorovComputer::getSForThresholdApprox(double threshold, double beta,
		double kappa) {
	double alpha = 1 - sqrt(1.0 - beta);
	int s = ceil(-log(alpha / 2.0) / (2.0 * pow(threshold - kappa, 2.0)));
	return s;
}

int KolmogorovComputer::getSForThreshold(double threshold, double beta,
		double kappa, double tolerance) {
	int s = 1;
	double tol = kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance);
	while (tol < threshold - kappa) {
		s++;
		tol = kolmogorovCdfInverse(s, 1 - (1 - sqrt(beta)), tolerance);
	}
	return s;
}

double KolmogorovComputer::getThresholdForSApprox(int s) {
	double alpha = 1 - sqrt(1.0 - _beta);
	return sqrt(-(1 / (2.0 * s)) * log(alpha / 2.0)) + _kappa;
}

double KolmogorovComputer::getThresholdForS(int s) {
	return kolmogorovCdfInverse(s) + _kappa;
}

int KolmogorovComputer::getSForThresholdApprox(double threshold) {

	double alpha = 1 - sqrt(1.0 - _beta);
	int s = ceil(-log(alpha / 2.0) / (2.0 * pow(threshold - _kappa, 2.0)));
	return s;
}

int KolmogorovComputer::getSForThreshold(double threshold) {
	int s = 1;
	double tol = kolmogorovCdfInverse(s);
	while (tol < threshold - _kappa) {
		s++;
		tol = kolmogorovCdfInverse(s);
	}
	return s;
}

#ifdef __cplusplus
extern "C" {
#endif
/**
 Supporting function for kolmogorov_cdf_marsaglia, not to be used directly.
 */
void mMultiply(double *A, double *B, double *C, int m) {
	int i, j, k;
	double s;
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++) {
			s = 0.;
			for (k = 0; k < m; k++)
				s += A[i * m + k] * B[k * m + j];
			C[i * m + j] = s;
		}
}

/**
 Supporting function for kolmogorov_cdf_marsaglia, not to be used directly.
 */
void mPower(double *A, int eA, double *V, int *eV, int m, int n) {
	double *B;
	int eB, i;
	if (n == 1) {
		for (i = 0; i < m * m; i++)
			V[i] = A[i];
		*eV = eA;
		return;
	}
	mPower(A, eA, V, eV, m, n / 2);
	B = (double*) malloc((m * m) * sizeof(double));
	mMultiply(V, V, B, m);
	eB = 2 * (*eV);
	if (n % 2 == 0) {
		for (i = 0; i < m * m; i++)
			V[i] = B[i];
		*eV = eB;
	} else {
		mMultiply(A, B, V, m);
		*eV = eA + eB;
	}
	if (V[(m / 2) * m + (m / 2)] > 1e140) {
		for (i = 0; i < m * m; i++)
			V[i] = V[i] * 1e-140;
		*eV += 140;
	}
	free(B);
}

double kolmogorov_cdf_marsaglia(int n, double d) {
	int k, m, i, j, g, eH, eQ;
	double h, s, *H, *Q;

	//OMIT NEXT LINE IF YOU REQUIRE >7 DIGIT ACCURACY IN THE RIGHT TAIL
	s = d * d * n;
	if (s > 7.24 || (s > 3.76 && n > 99))
		return 1 - 2 * exp(-(2.000071 + .331 / sqrt(n) + 1.409 / n) * s);

	k = (int) (n * d) + 1;
	m = 2 * k - 1;
	h = k - n * d;
	H = (double*) malloc((m * m) * sizeof(double));
	Q = (double*) malloc((m * m) * sizeof(double));
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++)
			if (i - j + 1 < 0)
				H[i * m + j] = 0;
			else
				H[i * m + j] = 1;
	for (i = 0; i < m; i++) {
		H[i * m] -= pow(h, i + 1);
		H[(m - 1) * m + i] -= pow(h, (m - i));
	}
	H[(m - 1) * m] += (2 * h - 1 > 0 ? pow(2 * h - 1, m) : 0);
	for (i = 0; i < m; i++)
		for (j = 0; j < m; j++)
			if (i - j + 1 > 0)
				for (g = 1; g <= i - j + 1; g++)
					H[i * m + j] /= g;
	eH = 0;
	mPower(H, eH, Q, &eQ, m, n);
	s = Q[(k - 1) * m + k - 1];
	for (i = 1; i <= n; i++) {
		s = s * i / n;
		if (s < 1e-140) {
			s *= 1e140;
			eQ -= 140;
		}
	}
	s *= pow(10., eQ);
	free(H);
	free(Q);
	return s;
}

#ifdef __cplusplus
}
#endif

// q = 1- alpha or, in terms of beta: q = 1 - (1 - sqrt(beta))
double KolmogorovComputer::kolmogorovCdfInverse(int S, double q,
		double tolerance) {
	double x_lo = 0.0;

	// Set initial guesses using the DKW bounds
	double x_hi = sqrt(-1 / (2 * (double) S) * log((1 - q) / 2));

	KolmogorovCdfRoots kolmogorov_roots(S, q);
	KolmogorovTol kolmogorov_tol(tolerance);

	std::pair<double, double> result = boost::math::tools::bisect<
			KolmogorovCdfRoots, double, KolmogorovTol>(kolmogorov_roots, x_lo,
			x_hi, kolmogorov_tol);

	return (result.first + result.second) / 2.0;
}

double KolmogorovComputer::kolmogorovCdfInverse(int S) {
	double q = 1 - (1 - sqrt(1 - _beta));

	return kolmogorovCdfInverse(S, q, _tolerance);
}

} /* namespace INSIGHTv3 */


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include <eve/function/erfc_inv.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <eve/function/is_negative.hpp>
#include <eve/function/is_positive.hpp>
#include <eve/constant/inf.hpp>
#include <eve/constant/minf.hpp>
#include <eve/constant/nan.hpp>
#include <eve/platform.hpp>
#include <cmath>

// TTS_CASE_TPL("Check eve::erfc_inv return type", EVE_TYPE)
// {
//   TTS_EXPR_IS(eve::erfc_inv(T(0)), T);
// }

TTS_CASE_TPL("Check eve::erfc_inv behavior", EVE_TYPE)
{
  auto eve__erfc_inv =  [](auto x) { return eve::erfc_inv(x); };

 if constexpr( eve::platform::supports_invalids )
  {
    TTS_IEEE_EQUAL(eve__erfc_inv(eve::nan(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(eve__erfc_inv(eve::inf(eve::as<T>()))  , eve::nan(eve::as<T>()) );
    TTS_IEEE_EQUAL(eve__erfc_inv(eve::minf(eve::as<T>())) , eve::nan(eve::as<T>()) );
  }

  TTS_ULP_EQUAL(eve__erfc_inv(T(35)), eve::nan(eve::as<T>()), 0.5);
  TTS_ULP_EQUAL(eve__erfc_inv(T(-35)), eve::nan(eve::as<T>()), 0.5);

 TTS_IEEE_EQUAL(eve__erfc_inv(T( 0 )),eve::inf(eve::as<T>()) );
 TTS_ULP_EQUAL(eve__erfc_inv(T( 0.1 )), T( boost::math::erfc_inv(0.1)), 0.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 0.2 )), T( boost::math::erfc_inv(0.2)), 1.0 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 0.3 )), T( boost::math::erfc_inv(0.3)), 1 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 0.5 )), T( boost::math::erfc_inv(0.5)),  1 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 0.15)), T( boost::math::erfc_inv(0.15)), 0.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 0.75)), T( boost::math::erfc_inv(0.75)), 0.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.1 )), T( boost::math::erfc_inv(1.1)), 1.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.2 )), T( boost::math::erfc_inv(1.2)), 1.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.3 )), T( boost::math::erfc_inv(1.3)), 0.5 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.5 )), T( boost::math::erfc_inv(1.5)),  1 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.15)), T( boost::math::erfc_inv(1.15)), 1 );
  TTS_ULP_EQUAL(eve__erfc_inv(T( 1.75)), T( boost::math::erfc_inv(1.75)), 0.5 );
}


#include <stdlib.h>
#include <iostream>
#include <memory>
#include "conex/cone_program.h"
#include "conex/constraint.h"
#include "conex/linear_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {
using DenseMatrix = Eigen::MatrixXd;
using Eigen::VectorXd;

TEST(LP, Dense) {
  for (int i = 0; i < 1; i++) {
    SolverConfiguration config;
    config.prepare_dual_variables = true;
    config.inv_sqrt_mu_max = 1000000;

    int m = 5;
    int n = 6 + 2 * i;
    double eps = 1e-12;

    DenseMatrix Alinear = DenseMatrix::Random(n, m);
    DenseMatrix Clinear(n, 1);
    Clinear.setConstant(1);

    LinearConstraint linear_constraint{n, &Alinear, &Clinear};

    Program prog(m);
    prog.SetNumberOfVariables(m);
    prog.AddConstraint(linear_constraint);

    VectorXd b = Alinear.transpose() * Clinear;
    DenseMatrix y(m, 1);
    Solve(b, prog, config, y.data());

    VectorXd x(n);
    prog.GetDualVariable(0, &x);

    VectorXd slack = Clinear - Alinear * y;
    EXPECT_TRUE((Alinear.transpose() * x - b).norm() <= eps);
    EXPECT_TRUE((slack).minCoeff() >= -eps);
  }
}

Eigen::VectorXd Vars(const Eigen::VectorXd& x, std::vector<int> indices) {
  Eigen::VectorXd z(indices.size());
  int cnt = 0;
  for (auto i : indices) {
    z(cnt++) = x(i);
  }
  return z;
}

using Eigen::MatrixXd;
using std::vector;
auto Combine(vector<MatrixXd> A, vector<MatrixXd> C,
             vector<vector<int> > vars) {
  int n = A.at(0).rows() * A.size();
  int m = A.at(0).cols() * A.size();
  Eigen::MatrixXd Af(n, m);
  Eigen::MatrixXd Cf(n, 1);
  Af.setZero();
  Cf.setZero();
  int cnt = 0;
  int max_v = 0;
  for (size_t i = 0; i < A.size(); i++) {
    for (int k = 0; k < A.at(i).rows(); k++) {
      Cf(cnt, 0) = C.at(i)(k, 0);
      for (size_t j = 0; j < vars.at(i).size(); j++) {
        Af(cnt, vars.at(i).at(j)) = A.at(i)(k, j);
        if (vars.at(i).at(j) > max_v) {
          max_v = vars.at(i).at(j);
        }
      }
      cnt++;
    }
  }

  Eigen::MatrixXd Ac = Af.topLeftCorner(cnt, max_v + 1);
  Eigen::MatrixXd Cc = Cf.topLeftCorner(cnt, 1);
  return LinearConstraint{Ac, Cc};
}

Eigen::VectorXd SolveSparseHelper(bool sparse) {
  double eps = 1e-8;
  using Eigen::MatrixXd;
  using std::vector;
  SolverConfiguration config;
  config.prepare_dual_variables = true;

  int number_of_constraints = 50;
  std::vector<std::vector<int> > variables(number_of_constraints);
  vector<MatrixXd> A(number_of_constraints);
  vector<MatrixXd> C(number_of_constraints);

  srand(1);
  int number_of_variables;
  int vars_per_constraint = 5;
  int rows_per_constraint = 10;
  int var_start = 0;
  for (int i = 0; i < number_of_constraints; i++) {
    for (int j = 0; j < vars_per_constraint; j++) {
      variables.at(i).push_back(var_start + j);
    }
    var_start = variables.at(i).back();
  }
  number_of_variables = variables.back().back() + 1;
  for (int i = 0; i < number_of_constraints; i++) {
    MatrixXd Ai =
        DenseMatrix::Random(rows_per_constraint, variables.at(i).size());
    MatrixXd Ci(rows_per_constraint, 1);
    Ci.setConstant(1 * (i * .01 + 1));
    C.at(i) = Ci;
    A.at(i) = Ai;
  }

  auto E = C.at(0);
  E.setConstant(1);
  Eigen::VectorXd b(number_of_variables);
  b.setZero();
  for (int i = 0; i < number_of_constraints; i++) {
    Eigen::VectorXd bi = A.at(i).transpose() * E;
    int cnt = 0;
    for (auto k : variables.at(i)) {
      b(k) += bi(cnt++);
    }
  }

  Program prog(number_of_variables);

  MatrixXd y(number_of_variables, 1);
  if (sparse) {
    for (int i = 0; i < number_of_constraints; i++) {
      prog.AddConstraint(LinearConstraint(A.at(i), C.at(i)), variables.at(i));
    }

    Solve(b, prog, config, y.data());
    MatrixXd Ax = b * 0;
    for (int i = 0; i < number_of_constraints; i++) {
      VectorXd slack = C.at(i) - A.at(i) * Vars(y, variables.at(i));
      EXPECT_TRUE((slack).minCoeff() >= -eps);

      MatrixXd xi(A.at(i).rows(), 1);
      prog.GetDualVariable(i, &xi);

      VectorXd temp = A.at(i).transpose() * xi;
      int k = 0;
      for (auto vk : variables.at(i)) {
        Ax(vk) += temp(k);
        k++;
      }
    }
    EXPECT_NEAR((Ax - b).norm(), 0, 1e-8);
  } else {
    prog.AddConstraint(Combine(A, C, variables));
    Solve(b, prog, config, y.data());

    auto res = b;
    auto L = Combine(A, C, variables);
    VectorXd slack = L.constraint_affine_ - L.constraint_matrix_ * y;

    MatrixXd xi(L.constraint_affine_.rows(), 1);
    prog.GetDualVariable(0, &xi);
    EXPECT_NEAR((b - L.constraint_matrix_.transpose() * xi).norm(), 0, 1e-8);
  }

  return y;
}

// Solve the same problem in dense and sparse format.
TEST(LP, Sparse) {
  auto y1 = SolveSparseHelper(true);
  auto y2 = SolveSparseHelper(false);
  EXPECT_NEAR((y1 - y2).norm(), 0, 1e-7);
}

Eigen::VectorXd SolveFillIn(bool sparse) {
  double eps = 1e-8;
  using Eigen::MatrixXd;
  using std::vector;
  SolverConfiguration config;
  config.prepare_dual_variables = true;

  int number_of_constraints = 4;
  std::vector<std::vector<int> > variables{{0, 1}, {1, 2}, {2, 3}, {0, 3}};

  int number_of_variables = 3 + 1;
  vector<MatrixXd> A(number_of_constraints);
  vector<MatrixXd> C(number_of_constraints);

  srand(1);
  int rows_per_constraint = 3;

  for (int i = 0; i < number_of_constraints; i++) {
    MatrixXd Ai =
        DenseMatrix::Random(rows_per_constraint, variables.at(i).size());
    MatrixXd Ci(rows_per_constraint, 1);
    Ci.setConstant(1 * (i * .01 + 1));
    C.at(i) = Ci;
    A.at(i) = Ai;
  }

  auto E = C.at(0);
  E.setConstant(1);
  Eigen::VectorXd b(number_of_variables);
  b.setZero();
  for (int i = 0; i < number_of_constraints; i++) {
    Eigen::VectorXd bi = A.at(i).transpose() * E;
    int cnt = 0;
    for (auto k : variables.at(i)) {
      b(k) += bi(cnt++);
    }
  }

  Program prog(number_of_variables);

  MatrixXd y(number_of_variables, 1);
  if (sparse) {
    for (int i = 0; i < number_of_constraints; i++) {
      prog.AddConstraint(LinearConstraint(A.at(i), C.at(i)), variables.at(i));
    }

    Solve(b, prog, config, y.data());
    MatrixXd Ax = b * 0;
    for (int i = 0; i < number_of_constraints; i++) {
      VectorXd slack = C.at(i) - A.at(i) * Vars(y, variables.at(i));
      EXPECT_TRUE((slack).minCoeff() >= -eps);

      MatrixXd xi(A.at(i).rows(), 1);
      prog.GetDualVariable(i, &xi);
      VectorXd temp = A.at(i).transpose() * xi;
      int k = 0;
      for (auto vk : variables.at(i)) {
        Ax(vk) += temp(k);
        k++;
      }
    }
    EXPECT_NEAR((Ax - b).norm(), 0, 1e-8);
  } else {
    prog.AddConstraint(Combine(A, C, variables));
    Solve(b, prog, config, y.data());

    auto res = b;
    auto L = Combine(A, C, variables);
    VectorXd slack = L.constraint_affine_ - L.constraint_matrix_ * y;

    MatrixXd xi(L.constraint_affine_.rows(), 1);
    prog.GetDualVariable(0, &xi);
    EXPECT_NEAR((b - L.constraint_matrix_.transpose() * xi).norm(), 0, 1e-8);
  }

  return y;
}

TEST(LP, SparseWithFillIn) {
  DUMP("HEHEH!");
  auto y1 = SolveFillIn(true);
  auto y2 = SolveFillIn(false);
  EXPECT_NEAR((y1 - y2).norm(), 0, 1e-7);
}

}  // namespace conex


/**
 * @file systemode_main.cc
 * @brief NPDE homework SystemODE
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <cmath>
#include <iomanip>
#include <iostream>

#include "../../../lecturecodes/helperfiles/polyfit.h"
#include "systemode.h"

/* SAM_LISTING_BEGIN_0 */
int main() {
  // PARAMETERS
  double T = 1;
  int n = 5;

  // INITIAL VALUE
  Eigen::VectorXd y0(2 * n);
  for (int i = 0; i < n; ++i) {
    y0(i) = (i + 1.) / n;
    y0(i + n) = -1;
  }

  // SETUP
  double conv_rate = 0;
  std::cout << std::setw(8) << "M" << std::setw(20) << "Error" << std::endl;

  //====================
  // Your code goes here
  //====================

  std::cout << "Convergence rate: " << std::round(std::abs(conv_rate))
            << std::endl;

  return 0;
}
/* SAM_LISTING_END_0 */


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ILOGB_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ILOGB_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing ilogb capabilities

    This function returns the integer truncation
    of the base 2 logarithm of x.

    It coincides with the @ref exponent function
    on all platforms supported.

    @par Semantic:

    @code
    auto r = ilogb(x);
    @endcode

    is similar to:

    @code
    auto r = saturated_(toint)(log2(x));
    @endcode

    @see exponent, log2, toint, saturated

  **/
  Value ilogb(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/ilogb.hpp>
#include <boost/simd/function/simd/ilogb.hpp>

#endif


#pragma once

#include <dlib/clustering.h>
#include <optional>
#include <vector>

namespace otus {
  struct Point {
    using Cluster = std::optional<unsigned long>;

    Cluster cluster { };
    float x { }, y { };
  };

  class Clusterer {
  public:
    Clusterer(std::vector<Point> &data, float gamma=0.1, float accuracy=0.01):
    data(data),
    kCentroid(KernelType(gamma), accuracy) { }

    void operator()(int numberOfClusters);

  private:
    using SampleType = dlib::matrix<float, 2, 1>;
    using KernelType = dlib::radial_basis_kernel<SampleType>;

    std::vector<Point> & data;
    dlib::kcentroid<KernelType> kCentroid;
  };
}