#include <poplar/Graph.hpp>
#include <poplar/Engine.hpp>
#include <poplar/IPUModel.hpp>
#include <poplar/DeviceManager.hpp>
#include <poplin/MatMul.hpp>
#include <poplin/codelets.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poplar/CSRFunctions.hpp>
// #include <poplar/codelets.hpp>
#include <popops/Reduce.hpp>
#include <popops/Operation.hpp>
#include <poplar/ProfileValue.hpp>

#include <Eigen/Dense>

#include <tbb/tbb.h>
#include <tbb/concurrent_vector.h>
#include <tbb/task_scheduler_init.h>
#include <tbb/concurrent_unordered_map.h>

#include <boost/program_options.hpp>

#include <random>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <string>
#include <algorithm>
#include <math.h>
#include <cmath>

#include "../include/dataio.h"
#include "../include/util.h"

using namespace std;
using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace Eigen;

// Returns compute set that updates the factors at the new linearisation point which is the current estimate of the beliefs
ComputeSet buildRelineariseCS(Graph &graph,
                                unsigned n_keyframes,
                                unsigned n_points,
                                unsigned n_edges,
                                unsigned cam_dofs,
                                unsigned lmks_per_tile,
                                unsigned factors_per_tile,
                                unsigned n_cam_tiles,
                                unsigned n_lmk_tiles,
                                std::vector<unsigned> measurements_camIDs,
                                std::vector<unsigned> measurements_lIDs,
                                Tensor measurements,
                                Tensor meas_variances,
                                Tensor K,
                                Tensor cam_beliefs_eta,
                                Tensor cam_beliefs_lambda,
                                Tensor lmk_beliefs_eta,
                                Tensor lmk_beliefs_lambda,
                                Tensor factor_potentials_eta,
                                Tensor factor_potentials_lambda,
                                Tensor robust_flag) {

  // A compute set is a set of vertices that are executed in parallel.
  ComputeSet cs_relinearise = graph.addComputeSet("cs_relinearise");

  // Add vertices to update keyframe node beliefs.
  for (unsigned edgeID = 0; edgeID < n_edges; ++edgeID) {

    VertexRef vtx = graph.addVertex(cs_relinearise, "RelineariseFactorVertex");

    unsigned adj_cID = measurements_camIDs[edgeID];
    unsigned adj_lID = measurements_lIDs[edgeID];

    // Set tile mapping of vertex.    edge_tile = n_cam_tiles + n_lmk_tiles + floor((float)edgeID / (float)factors_per_tile);

    graph.setTileMapping(vtx, n_cam_tiles + floor((float)edgeID / (float)factors_per_tile));
    // graph.setTileMapping(vtx, n_cam_tiles + floor((float)adj_lID / (float)lmks_per_tile));

    // Connect data to the fields of the vertex. ie. define edges.
    graph.connect(vtx["measurement"], measurements.slice(edgeID * 2, (edgeID + 1) * 2));
    graph.connect(vtx["meas_variance"], meas_variances[edgeID]);
    graph.connect(vtx["robust_flag"], robust_flag[edgeID]);
    graph.connect(vtx["K_"], K.slice(edgeID * 9, (edgeID + 1) * 9));
    graph.connect(vtx["kf_belief_eta_"], cam_beliefs_eta.slice(adj_cID * cam_dofs, (adj_cID + 1) * cam_dofs));
    graph.connect(vtx["kf_belief_lambda_"], cam_beliefs_lambda.slice(adj_cID * cam_dofs * cam_dofs, (adj_cID + 1) * cam_dofs * cam_dofs));
    graph.connect(vtx["lmk_belief_eta_"], lmk_beliefs_eta.slice(adj_lID * 3, (adj_lID + 1) * 3));
    graph.connect(vtx["lmk_belief_lambda_"], lmk_beliefs_lambda.slice(adj_lID * 3 * 3, (adj_lID + 1) * 3 * 3));
    graph.connect(vtx["factor_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3)));
    graph.connect(vtx["factor_lambda_cc_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs));
    graph.connect(vtx["factor_lambda_ll_"], factor_potentials_lambda.slice((edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)));
    graph.connect(vtx["factor_lambda_cl_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs, edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3));
    graph.connect(vtx["factor_lambda_lc_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3));
  }

  return cs_relinearise;
}

// Builds a program to update the beliefs in 4 series.
vector<ComputeSet> buildUpdateBeliefsProg(Graph &graph,
                                unsigned n_keyframes,
                                unsigned n_points,
                                unsigned cam_dofs,
                                unsigned max_nkfedges,
                                unsigned max_nlmkedges,
                                Tensor cam_messages_eta,
                                Tensor cam_messages_lambda,
                                Tensor lmk_messages_eta,
                                Tensor lmk_messages_lambda,
                                Tensor cam_beliefs_eta,
                                Tensor cam_beliefs_lambda,
                                Tensor lmk_beliefs_eta,
                                Tensor lmk_beliefs_lambda) {

  Sequence prog;
  // Reduce along incoming messages
  Operation ADD = Operation::ADD;
  ReduceParams params(ADD);

  OptionFlags reduceOpts {
    {"accumType.interTile", "float"},
    {"accumType.inVertex", "float"}
  };
  vector<ComputeSet> cs;
  reduceWithOutput(graph, cam_messages_eta.reshape({n_keyframes, max_nkfedges + 1, cam_dofs}),
                    cam_beliefs_eta.reshape({n_keyframes, cam_dofs}), {1}, params, cs, "", reduceOpts);
  reduceWithOutput(graph, cam_messages_lambda.reshape({n_keyframes, max_nkfedges + 1, cam_dofs*cam_dofs}),
                    cam_beliefs_lambda.reshape({n_keyframes, cam_dofs*cam_dofs}), {1}, params, cs, "", reduceOpts);
  reduceWithOutput(graph, lmk_messages_eta.reshape({n_points, max_nlmkedges + 1, 3}),
                    lmk_beliefs_eta.reshape({n_points, 3}), {1}, params, cs, "", reduceOpts);
  reduceWithOutput(graph, lmk_messages_lambda.reshape({n_points, max_nlmkedges + 1, 9}),
                    lmk_beliefs_lambda.reshape({n_points, 9}), {1}, params, cs, "", reduceOpts);

  return cs;
}

// Builds a program to weaken the strength of the priors at each variable node.
ComputeSet buildWeakenPriorCS(Graph &graph,
                                unsigned n_keyframes,
                                unsigned n_points,
                                unsigned cam_dofs,
                                unsigned max_nkfedges,
                                unsigned max_nlmkedges,
                                unsigned cams_per_tile,
                                unsigned lmks_per_tile,
                                unsigned n_cam_tiles,
                                Tensor cam_messages_eta,
                                Tensor cam_messages_lambda,
                                Tensor lmk_messages_eta,
                                Tensor lmk_messages_lambda,
                                Tensor cam_scaling,
                                Tensor lmk_scaling,
                                Tensor cam_weaken_flag,
                                Tensor lmk_weaken_flag) {

  // A compute set is a set of vertices that are executed in parallel.
  ComputeSet cs_weaken_prior = graph.addComputeSet("cs_weaken_prior");

  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    VertexRef vx = graph.addVertex(cs_weaken_prior, "WeakenPriorVertex");
    graph.setTileMapping(vx, floor((float)cID / (float)cams_per_tile));

    graph.connect(vx["scaling"], cam_scaling[cID]);
    graph.connect(vx["weaken_flag"], cam_weaken_flag[cID]);
    graph.connect(vx["prior_eta"], cam_messages_eta.slice(cID * cam_dofs * (max_nkfedges + 1), cID * cam_dofs * (max_nkfedges + 1) + cam_dofs));
    graph.connect(vx["prior_lambda"], cam_messages_lambda.slice(cID * cam_dofs * cam_dofs * (max_nkfedges + 1), cID * cam_dofs * cam_dofs * (max_nkfedges + 1) + cam_dofs * cam_dofs));
  }
  for (unsigned lID = 0; lID < n_points; ++lID) {
    VertexRef vx = graph.addVertex(cs_weaken_prior, "WeakenPriorVertex");
    graph.setTileMapping(vx, floor((float)lID / (float)lmks_per_tile) + n_cam_tiles);

    graph.connect(vx["scaling"], lmk_scaling[lID]);
    graph.connect(vx["weaken_flag"], lmk_weaken_flag[lID]);
    graph.connect(vx["prior_eta"], lmk_messages_eta.slice((max_nlmkedges + 1) * 3 * lID, (max_nlmkedges + 1) * 3 * lID + 3 ));
    graph.connect(vx["prior_lambda"], lmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * lID, (max_nlmkedges + 1) * 9 * lID + 9 ));
  }

  return cs_weaken_prior;
}

// This function returns  compute set that computes the outgoing messages at all factor nodes.
vector<ComputeSet> buildComputeMessagesCS(Graph &graph,
                                unsigned n_keyframes,
                                unsigned n_points,
                                unsigned n_edges,
                                unsigned cam_dofs,
                                unsigned max_nkfedges,
                                unsigned max_nlmkedges,
                                unsigned lmks_per_tile,
                                unsigned factors_per_tile,
                                unsigned n_cam_tiles,
                                unsigned n_lmk_tiles,
                                std::vector<unsigned> n_edges_per_kf,
                                std::vector<unsigned> n_edges_per_lmk,
                                std::vector<unsigned> measurements_camIDs,
                                std::vector<unsigned> measurements_lIDs,
                                Tensor cams_dofs,
                                Tensor lmk_dofs,
                                Tensor active_flag,
                                Tensor factor_potentials_eta,
                                Tensor factor_potentials_lambda,
                                Tensor cam_messages_eta,
                                Tensor cam_messages_lambda,
                                Tensor lmk_messages_eta,
                                Tensor lmk_messages_lambda,
                                Tensor pcam_messages_eta,
                                Tensor pcam_messages_lambda,
                                Tensor plmk_messages_eta,
                                Tensor plmk_messages_lambda,
                                Tensor cam_beliefs_eta,
                                Tensor cam_beliefs_lambda,
                                Tensor lmk_beliefs_eta,
                                Tensor lmk_beliefs_lambda,
                                Tensor measurements,
                                Tensor meas_variances,
                                Tensor K,
                                Tensor damping,
                                Tensor damping_count,
                                Tensor mu,
                                Tensor oldmu,
                                Tensor dmu,
                                Tensor robust_flag) {

  vector<ComputeSet> cs(2);

  cs[0] = graph.addComputeSet("cs_compmess_prep");

  // A compute set is a set of vertices that are executed in parallel.
  // This compute set contains the vertices to compute the outgoing messages at every factor node.
  cs[1] = graph.addComputeSet("cs_computemessages");

  // Loop through factor nodes in the graph
  for (unsigned edgeID = 0; edgeID < n_edges; ++edgeID) {
    unsigned adj_cID = measurements_camIDs[edgeID];
    unsigned adj_lID = measurements_lIDs[edgeID];

    unsigned tm = n_cam_tiles + floor((float)edgeID / (float)factors_per_tile);

    VertexRef vx_prep_mess = graph.addVertex(cs[0], "PrepMessageVertex");

    // Create vertices for the factor node. 2 vertices for computing eta messages in each direction and 2 for lambda messages
    VertexRef vx_cam_eta = graph.addVertex(cs[1], "ComputeCamMessageEtaVertex");  // varnode0 is outedge, varnode1 is nonoutedge
    VertexRef vx_cam_lambda = graph.addVertex(cs[1], "ComputeCamMessageLambdaVertex");

    VertexRef vx_lmk_eta = graph.addVertex(cs[1], "ComputeLmkMessageEtaVertex");  // varnode1 is outedge, varnode0 is nonoutedge
    VertexRef vx_lmk_lambda = graph.addVertex(cs[1], "ComputeLmkMessageLambdaVertex");

    // Set tile mapping of vertex.
    graph.setTileMapping(vx_prep_mess, tm);
    graph.setTileMapping(vx_cam_eta, tm);
    graph.setTileMapping(vx_cam_lambda, tm);
    graph.setTileMapping(vx_lmk_eta, tm);
    graph.setTileMapping(vx_lmk_lambda, tm);

    // Index of factor node at the given variable node.
    // Want to find how many times the variableID appears in measurements_nodeIDs before the edge we are at now.
    unsigned edges_c = 0;
    unsigned edges_l = 0;
    for (unsigned i = 0; i < edgeID; ++i) {
      if (measurements_camIDs[i] == adj_cID) {
        edges_c += 1;
      }
    }
    unsigned count_ledges_l = 0;
    for (unsigned i = 0; i < edgeID; ++i) {
      if (measurements_lIDs[i] == adj_lID) {
        edges_l += 1;
      }
    }

    // Connect vertex fields
    graph.connect(vx_prep_mess["damping"], damping[edgeID]);
    graph.connect(vx_prep_mess["damping_count"], damping_count[edgeID]);
    graph.connect(vx_prep_mess["active_flag"], active_flag[edgeID]);
    graph.connect(vx_prep_mess["robust_flag"], robust_flag[edgeID]);
    graph.connect(vx_prep_mess["mu"], mu.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3) ));
    graph.connect(vx_prep_mess["oldmu"], oldmu.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3) ));
    graph.connect(vx_prep_mess["dmu"], dmu[edgeID]);
    graph.connect(vx_prep_mess["measurement"], measurements.slice(edgeID * 2, (edgeID + 1) * 2));
    graph.connect(vx_prep_mess["meas_variance"], meas_variances[edgeID]);
    graph.connect(vx_prep_mess["K_"], K.slice(edgeID * 9, (edgeID + 1) * 9));
    graph.connect(vx_prep_mess["kf_belief_eta_"], cam_beliefs_eta.slice(adj_cID * cam_dofs, (adj_cID + 1) * cam_dofs));
    graph.connect(vx_prep_mess["kf_belief_lambda_"], cam_beliefs_lambda.slice(adj_cID * cam_dofs * cam_dofs, (adj_cID + 1) * cam_dofs * cam_dofs));
    graph.connect(vx_prep_mess["lmk_belief_eta_"], lmk_beliefs_eta.slice(adj_lID * 3, (adj_lID + 1) * 3));
    graph.connect(vx_prep_mess["lmk_belief_lambda_"], lmk_beliefs_lambda.slice(adj_lID * 3 * 3, (adj_lID + 1) * 3 * 3));
    graph.connect(vx_prep_mess["factor_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3)));
    graph.connect(vx_prep_mess["factor_lambda_cc_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs));
    graph.connect(vx_prep_mess["factor_lambda_ll_"], factor_potentials_lambda.slice((edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)));
    graph.connect(vx_prep_mess["factor_lambda_cl_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs, edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3));
    graph.connect(vx_prep_mess["factor_lambda_lc_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3));

    // Message is to keyframe node
    graph.connect(vx_cam_eta["damping"], damping[edgeID]);
    graph.connect(vx_cam_eta["active_flag"], active_flag[edgeID]);
    graph.connect(vx_cam_eta["outedge_dofs"], cams_dofs[adj_cID]);
    graph.connect(vx_cam_eta["nonoutedge_dofs"], lmk_dofs[adj_lID]);
    graph.connect(vx_cam_eta["f_outedge_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3), edgeID * (cam_dofs + 3) + cam_dofs));
    graph.connect(vx_cam_eta["f_nonoutedge_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3) + cam_dofs, (edgeID + 1) * (cam_dofs + 3)));
    graph.connect(vx_cam_eta["f_noe_noe_lambda_"], factor_potentials_lambda.slice((edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)));
    graph.connect(vx_cam_eta["f_oe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs, edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3));
    graph.connect(vx_cam_eta["belief_nonoutedge_eta_"], lmk_beliefs_eta.slice(adj_lID * 3, (adj_lID + 1) * 3));
    graph.connect(vx_cam_eta["belief_nonoutedge_lambda_"], lmk_beliefs_lambda.slice(adj_lID * 3 * 3, (adj_lID + 1) * 3 * 3));
    graph.connect(vx_cam_eta["pmess_nonoutedge_eta_"],
      plmk_messages_eta.slice((max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 1), (max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 2)));
    graph.connect(vx_cam_eta["pmess_nonoutedge_lambda_"],
      plmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 1), (max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 2)));
    graph.connect(vx_cam_eta["pmess_outedge_eta_"],
      pcam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 2)));
    graph.connect(vx_cam_eta["mess_outedge_eta_"],
      cam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 2)));

    graph.connect(vx_cam_lambda["outedge_dofs"], cams_dofs[adj_cID]);
    graph.connect(vx_cam_lambda["nonoutedge_dofs"], lmk_dofs[adj_lID]);
    graph.connect(vx_cam_lambda["active_flag"], active_flag[edgeID]);
    graph.connect(vx_cam_lambda["f_oe_oe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs));
    graph.connect(vx_cam_lambda["f_noe_noe_lambda_"], factor_potentials_lambda.slice((edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)));
    graph.connect(vx_cam_lambda["f_oe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs, edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3));
    graph.connect(vx_cam_lambda["f_noe_oe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3));
    graph.connect(vx_cam_lambda["belief_nonoutedge_lambda_"], lmk_beliefs_lambda.slice(adj_lID * 3 * 3, (adj_lID + 1) * 3 * 3));
    graph.connect(vx_cam_lambda["pmess_nonoutedge_lambda_"],
      plmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 1), (max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 2)));
    graph.connect(vx_cam_lambda["mess_outedge_lambda_"],
      cam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 2)));

    // Message is to landmark node
    graph.connect(vx_lmk_eta["damping"], damping[edgeID]);
    graph.connect(vx_lmk_eta["outedge_dofs"], lmk_dofs[adj_lID]);
    graph.connect(vx_lmk_eta["nonoutedge_dofs"], cams_dofs[adj_cID]);
    graph.connect(vx_lmk_eta["active_flag"], active_flag[edgeID]);
    graph.connect(vx_lmk_eta["f_outedge_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3) + cam_dofs, (edgeID + 1) * (cam_dofs + 3)));
    graph.connect(vx_lmk_eta["f_nonoutedge_eta_"], factor_potentials_eta.slice(edgeID * (cam_dofs + 3), edgeID * (cam_dofs + 3) + cam_dofs));
    graph.connect(vx_lmk_eta["f_noe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs));
    graph.connect(vx_lmk_eta["f_oe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3));
    graph.connect(vx_lmk_eta["belief_nonoutedge_eta_"], cam_beliefs_eta.slice(adj_cID * cam_dofs, (adj_cID + 1) * cam_dofs));
    graph.connect(vx_lmk_eta["belief_nonoutedge_lambda_"], cam_beliefs_lambda.slice(adj_cID * cam_dofs * cam_dofs, (adj_cID + 1) * cam_dofs * cam_dofs));
    graph.connect(vx_lmk_eta["pmess_nonoutedge_eta_"],
      pcam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 2)));
    graph.connect(vx_lmk_eta["pmess_nonoutedge_lambda_"],
      pcam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 2)));
    graph.connect(vx_lmk_eta["pmess_outedge_eta_"],
      plmk_messages_eta.slice((max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 1), (max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 2)));
    graph.connect(vx_lmk_eta["mess_outedge_eta_"],
      lmk_messages_eta.slice((max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 1), (max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 2)));

    graph.connect(vx_lmk_lambda["outedge_dofs"], lmk_dofs[adj_lID]);
    graph.connect(vx_lmk_lambda["nonoutedge_dofs"], cams_dofs[adj_cID]);
    graph.connect(vx_lmk_lambda["active_flag"], active_flag[edgeID]);
    graph.connect(vx_lmk_lambda["f_oe_oe_lambda_"], factor_potentials_lambda.slice((edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)));
    graph.connect(vx_lmk_lambda["f_noe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs));
    graph.connect(vx_lmk_lambda["f_oe_noe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3, (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3) - 3 * 3));
    graph.connect(vx_lmk_lambda["f_noe_oe_lambda_"], factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs, edgeID * (cam_dofs + 3) * (cam_dofs + 3) + cam_dofs * cam_dofs + cam_dofs * 3));
    graph.connect(vx_lmk_lambda["belief_nonoutedge_lambda_"], cam_beliefs_lambda.slice(adj_cID * cam_dofs * cam_dofs, (adj_cID + 1) * cam_dofs * cam_dofs));
    graph.connect(vx_lmk_lambda["pmess_nonoutedge_lambda_"],
      pcam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 1), (max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 2)));
    graph.connect(vx_lmk_lambda["mess_outedge_lambda_"],
      lmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 1), (max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 2)));

  }
  return cs;
}

struct Options {
  std::string dir;
  std::string bal_file;
  int nIPUs;
  bool profile;
  int iters_between_kfs;
  int cams_per_tile;
  float transnoise;
  float rotnoise;
  float lmktrans_noise;
  bool av_depth_on;
  float av_depth;
  float reproj_meas_var;
  float prior_std_weaker_factor;
  float first_cam_prior_std;
  float steps;
  int iters_before_damping;
  bool verbose;
};

Options parseOptions(int argc, char** argv) {
  Options options;
  std::string modeString;

  namespace po = boost::program_options;
  po::options_description desc("Options");
  desc.add_options()
  ("help", "Show command help")
  ("bal_file",
   po::value<std::string>(&options.bal_file)->required(),
   "Set the bal file"
  )
  ("ipus",
   po::value<int>(&options.nIPUs)->default_value(1),
   "Number of IPU chips to use. NB. Must be a power of 2!"
  )
  ("profile",
   po::value<bool>(&options.profile)->default_value(false),
   "Save profile report after execution"
  )
  ("iters_between_kfs",
   po::value<int>(&options.iters_between_kfs)->default_value(700),
   "Number of GBP iterations between sucessive keyframes."
  )
  ("camspertile",
   po::value<int>(&options.cams_per_tile)->default_value(1),
   "Maximum number of keyframe nodes placed on a single tile of the IPU."
  )
  ("tn",
   po::value<float>(&options.transnoise)->default_value(0.f),
   "Set keyframe translation noise value"
  )
  ("rn",
   po::value<float>(&options.rotnoise)->default_value(0.f),
   "Set keyframe rotation noise value"
  )
  ("ltn",
   po::value<float>(&options.lmktrans_noise)->default_value(0.f),
   "Set landmark translation noise noise value"
  )
  ("avdepth_on",
   po::value<bool>(&options.av_depth_on)->default_value(false),
   "bool: should landmarks be initialised at an average depth from the keyframe they are first observed by"
  )
  ("avdepth",
   po::value<float>(&options.av_depth)->default_value(1.f),
   "float: Average depth at which landmarks are initialed from the keyframe which they are first observed by."
  )
  ("reproj_meas_var",
   po::value<float>(&options.reproj_meas_var)->default_value(4.f),
   "Variance of Gaussian noise in Gaussian measurement model for the reprojection constraints"
  )
  ("prior_std_weaker_factor",
   po::value<float>(&options.prior_std_weaker_factor)->default_value(100.f),
   "Factor: std of gauss noise of reprojection factors / std of gauss noise of prior factors"
  )
  ("first_cam_prior_std",
   po::value<float>(&options.first_cam_prior_std)->default_value(0.01f),
   "Standard deviation of prior on pose of first keyframe, to anchor optimisation."
  )
  ("steps",
   po::value<float>(&options.steps)->default_value(5.f),
   "The priors are gradually weakened over this many steps."
  )
  ("undamped_start",
   po::value<int>(&options.iters_before_damping)->default_value(15),
   "Number of undamped iterations before damping GBP."
  )
  ("v",
   po::value<bool>(&options.verbose)->default_value(false),
   "Verbose: print beliefs"
  );

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  if (vm.count("help")) {
    std::cout << desc << "\n";
    throw std::runtime_error("help requested");
  }
  po::notify(vm);

  return options;
}

int main(int argc, char** argv) {

  const auto options = parseOptions(argc, argv);
  const char * f = options.bal_file.c_str();

  BALProblem bal_problem;
  if (!bal_problem.LoadFile(f)) {  //true for load planes and timestamps
    std::cerr << "ERROR: unable to open file " << f << "\n";
    return 1;
  }

  // Extract data from BALProblem object
  unsigned n_edges = bal_problem.n_edges();
  unsigned n_keyframes = bal_problem.n_keyframes();
  unsigned n_points = bal_problem.n_points();

  std::vector<float> Ksingle_ {(float)(bal_problem.fx()[0]), 0.f, (float)(bal_problem.cx()[0]), 0.f,
    (float)(bal_problem.fy()[0]), (float)(bal_problem.cy()[0]), 0.f, 0.f, 1.f};
  float K_ [n_edges * 9] = {};
  for (unsigned i = 0; i < n_edges; ++i) {
    for (unsigned j = 0; j < 9; ++j) {
      K_[i*9 + j] = Ksingle_[j];
    }
  }

  std::vector<float> meas_variances_ (n_edges, options.reproj_meas_var);
  std::vector<float> measurements_;
  std::vector<unsigned> measurements_camIDs;
  std::vector<unsigned> measurements_lIDs;
  for (int i = 0; i < bal_problem.n_edges(); ++i) {
    measurements_.push_back(bal_problem.observations()[2*i]);
    measurements_.push_back(bal_problem.observations()[2*i+1]);
    measurements_camIDs.push_back(bal_problem.camera_index(i));
    measurements_lIDs.push_back(bal_problem.point_index(i));
  }

  std::vector<unsigned> n_edges_per_kf;
  for (int i = 0; i < bal_problem.n_keyframes(); ++i) {
    n_edges_per_kf.push_back(std::count(measurements_camIDs.begin(), measurements_camIDs.end(), i));
  }
  std::vector<unsigned> n_edges_per_lmk;
  for (int i = 0; i < bal_problem.n_points(); ++i) {
    n_edges_per_lmk.push_back(std::count(measurements_lIDs.begin(), measurements_lIDs.end(), i));
  }

  std::vector<float> cam_priors_mean_;
  for (int i = 0; i < bal_problem.n_keyframes(); ++i) {
    for (int j = 0; j<6; ++j) {
      cam_priors_mean_.push_back((float)(bal_problem.camera(i)[j]));
    }
  }
  std::vector<float> lmk_priors_mean_;
  for (int i = 0; i < bal_problem.n_points(); ++i) {
    for (int j = 0; j<3; ++j) {
      lmk_priors_mean_.push_back((float)(bal_problem.point(i)[j]));
    }
  }

  // Add noise to initialisation here if desired
  if (options.transnoise != 0.f) {
    add_cam_trans_noise(cam_priors_mean_, n_keyframes, options.transnoise);
  }
  if (options.rotnoise != 0.f) {
    add_cam_rot_noise(cam_priors_mean_, n_keyframes, options.rotnoise);
  }
  if ((options.lmktrans_noise != 0.f) && !(options.av_depth_on)) {
    add_lmk_noise(lmk_priors_mean_, n_points, options.lmktrans_noise);
  } else if (options.av_depth_on) {
    av_depth_init(options.av_depth, n_keyframes, n_edges, n_points,
                  measurements_camIDs, measurements_lIDs, cam_priors_mean_, lmk_priors_mean_);
  }

  // Set priors to initially be similar strength to measurement factors
  std::vector<float> cam_priors_lambda_(bal_problem.n_keyframes() * 36, 0);
  std::vector<float> cam_priors_eta_(bal_problem.n_keyframes() * 6, 0);
  std::vector<float> lmk_priors_lambda_(bal_problem.n_points() * 9, 0);
  std::vector<float> lmk_priors_eta_(bal_problem.n_points() * 3, 0);
  set_prior_lambda(bal_problem, Ksingle_, options.reproj_meas_var,
                  cam_priors_mean_, cam_priors_eta_, cam_priors_lambda_,
                  lmk_priors_mean_, lmk_priors_eta_, lmk_priors_lambda_);

  // Compute factor to scale lambda and eta when weakening the prior over iterations
  float cam_scaling_ [n_keyframes] = {};
  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    if (cID == 0 || cID == 1) {
      cam_scaling_[cID] = exp(-1 / options.steps * log(cam_priors_lambda_[cID*36] * pow(options.first_cam_prior_std, 2)));
    } else {
      cam_scaling_[cID] = exp(-2 / options.steps * log(options.prior_std_weaker_factor));
    }
  }
  float lmk_scaling_ [n_points] = {};
  for (unsigned lID = 0; lID < n_points; ++lID) {
    lmk_scaling_[lID] = exp(-2 / options.steps * log(options.prior_std_weaker_factor));
  }

  std::cout << "Loaded data onto host!\n";

  unsigned cam_dofs = 6;
  std::vector<unsigned> cams_dofs_(n_keyframes, 6);
  std::vector<unsigned> lmk_dofs_(n_points, 3);

  std::vector<float> damping_(n_edges, 0.0);
  std::vector<int> damping_count_(n_edges, -options.iters_before_damping);
  std::vector<float> mu_(n_edges*(cam_dofs + 3), 0.0);
  std::vector<float> oldmu_(n_edges*(cam_dofs + 3), 0.0);
  std::vector<unsigned> robust_flag_(n_edges, 0);

  cout << "SLAM\n";

  unsigned steps = static_cast<unsigned>(options.steps);
  std::vector<unsigned int> active_flag_ (bal_problem.n_edges(), 0);  // edge active flag
  std::vector<unsigned int> cam_weaken_flag_ (bal_problem.n_keyframes(), 0);
  std::vector<unsigned int> lmk_weaken_flag_ (bal_problem.n_points(), 0);
  std::vector<unsigned int> lmk_active_flag (bal_problem.n_points(), 0);
  create_flags(bal_problem, active_flag_, cam_weaken_flag_, lmk_weaken_flag_,
               lmk_active_flag, steps);

  std::vector<unsigned> bad_associations;// = readUnsignedIntVector(dir + "/bad_associations.txt");

  // Create data buffers for streaming to and from the IPU
  unsigned max_nkfedges = *max_element(n_edges_per_kf.begin(), n_edges_per_kf.end());
  unsigned max_nlmkedges = *max_element(n_edges_per_lmk.begin(), n_edges_per_lmk.end());

  float cam_beliefs_eta_ [n_keyframes * cam_dofs] = {};
  float cam_beliefs_lambda_ [n_keyframes * cam_dofs * cam_dofs] = {};
  float lmk_beliefs_eta_ [n_points * 3] = {};
  float lmk_beliefs_lambda_ [n_points * 3 * 3] = {};

  std::cout << "\nNumber of keyframe nodes in the graph: " << n_keyframes << '\n';
  std::cout << "Number of landmark nodes in the graph: " << n_points << '\n';
  std::cout << "Number of edges in the graph: " << n_edges << '\n';

  // Allocate factor graph to tiles of IPU
  int nIPUs = options.nIPUs;
  if (options.nIPUs == 0) { nIPUs = 1;}
  if (options.nIPUs == 3) { nIPUs = 4;}
  if ((4<options.nIPUs) && (options.nIPUs<8)) { nIPUs = 8;}
  if ((8<options.nIPUs) && (options.nIPUs<16)) { nIPUs = 16;}

  int n_tiles = nIPUs * 1216;
  unsigned cams_per_tile = options.cams_per_tile;
  unsigned n_cam_tiles = ceil((float)n_keyframes / (float)cams_per_tile);
  float remaining_tiles = (float)(n_tiles - n_cam_tiles);
  unsigned lmks_per_tile = ceil((float)n_points / remaining_tiles);
  unsigned factors_per_tile = ceil((float)n_edges / remaining_tiles);

  unsigned n_lmk_tiles = ceil((float)n_points / (float)lmks_per_tile);
  unsigned n_factor_tiles = ceil((float)n_edges / (float)factors_per_tile);

  std::cout << "\nNumber of IPUs: " << nIPUs << '\n';
  cout << "Number of keyframe / landmark / factor nodes per tile: ";
  cout << cams_per_tile << " / " << lmks_per_tile << " / " << factors_per_tile  << "\n";
  cout << "Number of tiles used by camera / landmark / factor nodes / total : " << n_cam_tiles << " / " << n_lmk_tiles << " / " << n_factor_tiles << " / " << n_cam_tiles + n_factor_tiles << "\n";

  // Create the IPU device
  std::cout << "\nAttaching to IPU device " << std::endl;
  IPUModel ipuModel;
  auto dm = DeviceManager::createDeviceManager();
  auto hwDevices = dm.getDevices(TargetType::IPU, nIPUs);
  poplar::Device device;
  for (auto &testDevice : hwDevices) {
    if (testDevice.attach()) {
      device = std::move(testDevice);
      break;
    }
  }
  std::cout << "\nAttached to device: " << device.getId() << std::endl;

  // if (device.getId()) {
  //  std::cout << "Could not find a device\n";
  //  exit(-1);
  // }

  // Create the Graph object
  Graph graph(device.getTarget());
  popops::addCodelets(graph);
  graph.addCodelets("gbp_codelets.cpp");

  // CREATE VARIABLE NODES

  Tensor cams_dofs = graph.addVariable(UNSIGNED_INT, {n_keyframes}, "cams_dofs");
  Tensor lmk_dofs = graph.addVariable(UNSIGNED_INT, {n_points}, "lmk_dofs");

  Tensor cam_beliefs_eta = graph.addVariable(FLOAT, {cam_dofs * n_keyframes}, "cam_beliefs_eta");
  Tensor cam_beliefs_lambda = graph.addVariable(FLOAT, {cam_dofs * cam_dofs * n_keyframes}, "cam_beliefs_lambda");

  Tensor lmk_beliefs_eta = graph.addVariable(FLOAT, {3 * n_points}, "lmk_beliefs_eta");
  Tensor lmk_beliefs_lambda = graph.addVariable(FLOAT, {3 * 3 * n_points}, "lmk_beliefs_lambda");

  Tensor cam_scaling = graph.addVariable(FLOAT, {n_keyframes}, "cam_scaling");
  Tensor lmk_scaling = graph.addVariable(FLOAT, {n_points}, "lmk_scaling");
  Tensor cam_weaken_flag = graph.addVariable(UNSIGNED_INT, {n_keyframes}, "cam_weaken_flag");
  Tensor lmk_weaken_flag = graph.addVariable(UNSIGNED_INT, {n_points}, "lmk_weaken_flag");

  // Create Tensor for messages which are stored at the variable node
  Tensor cam_messages_eta = graph.addVariable(FLOAT, {(max_nkfedges + 1) * cam_dofs * n_keyframes}, "cam_messages_eta");
  Tensor cam_messages_lambda = graph.addVariable(FLOAT, {(max_nkfedges + 1) * cam_dofs * cam_dofs * n_keyframes}, "cam_messages_lambda");
  Tensor pcam_messages_eta = graph.addVariable(FLOAT, {(max_nkfedges + 1) * cam_dofs * n_keyframes}, "pcam_messages_eta");
  Tensor pcam_messages_lambda = graph.addVariable(FLOAT, {(max_nkfedges + 1) * cam_dofs * cam_dofs * n_keyframes}, "pcam_messages_lambda");
  Tensor lmk_messages_eta = graph.addVariable(FLOAT, {(max_nlmkedges + 1) * 3 * n_points}, "lmk_messages_eta");
  Tensor lmk_messages_lambda = graph.addVariable(FLOAT, {(max_nlmkedges + 1) * 9 * n_points}, "lmk_messages_lambda");
  Tensor plmk_messages_eta = graph.addVariable(FLOAT, {(max_nlmkedges + 1) * 3 * n_points}, "plmk_messages_eta");
  Tensor plmk_messages_lambda = graph.addVariable(FLOAT, {(max_nlmkedges + 1) * 9 * n_points}, "plmk_messages_lambda");

  // Create data streams to write to graph variables
  DataStream instream_cams_dofs = graph.addHostToDeviceFIFO("instream_cams_dofs", UNSIGNED_INT, n_keyframes);
  DataStream instream_lmk_dofs = graph.addHostToDeviceFIFO("instream_lmk_dofs", UNSIGNED_INT, n_points);

  DataStream instream_cam_scaling = graph.addHostToDeviceFIFO("instream_cam_scaling", FLOAT, n_keyframes);
  DataStream instream_lmk_scaling = graph.addHostToDeviceFIFO("instream_lmk_scaling", FLOAT, n_points);
  DataStream instream_cam_weaken_flag = graph.addHostToDeviceFIFO("instream_cam_weaken_flag", UNSIGNED_INT, n_keyframes);
  DataStream instream_lmk_weaken_flag = graph.addHostToDeviceFIFO("instream_lmk_weaken_flag", UNSIGNED_INT, n_points);

  // Create data streams to write into priors vector
  DataStream instream_cam_priors_eta = graph.addHostToDeviceFIFO("instream_cam_priors_eta", FLOAT, cam_dofs * n_keyframes);
  DataStream instream_cam_priors_lambda = graph.addHostToDeviceFIFO("instream_cam_priors_lambda", FLOAT, cam_dofs * cam_dofs * n_keyframes);
  DataStream instream_lmk_priors_eta = graph.addHostToDeviceFIFO("instream_lmk_priors_eta", FLOAT, 3 * n_points);
  DataStream instream_lmk_priors_lambda = graph.addHostToDeviceFIFO("instream_lmk_priors_lambda", FLOAT, 9 * n_points);

  DataStream outstream_cam_priors_eta = graph.addDeviceToHostFIFO("outstream_cam_priors_eta", FLOAT, cam_dofs * n_keyframes);
  DataStream outstream_cam_priors_lambda = graph.addDeviceToHostFIFO("outstream_cam_priors_lambda", FLOAT, cam_dofs * cam_dofs * n_keyframes);
  DataStream outstream_lmk_priors_eta = graph.addDeviceToHostFIFO("outstream_lmk_priors_eta", FLOAT, 3 * n_points);
  DataStream outstream_lmk_priors_lambda = graph.addDeviceToHostFIFO("outstream_lmk_priors_lambda", FLOAT, 9 * n_points);

  // Create data streams to read out beliefs
  DataStream outstream_cam_beliefs_eta = graph.addDeviceToHostFIFO("outstream_cam_beliefs_eta", FLOAT, n_keyframes * cam_dofs);
  DataStream outstream_cam_beliefs_lambda = graph.addDeviceToHostFIFO("outstream_cam_beliefs_lambda", FLOAT, n_keyframes * cam_dofs * cam_dofs);
  DataStream outstream_lmk_beliefs_eta = graph.addDeviceToHostFIFO("outstream_lmk_beliefs_eta", FLOAT, n_points * 3);
  DataStream outstream_lmk_beliefs_lambda = graph.addDeviceToHostFIFO("outstream_lmk_beliefs_lambda", FLOAT, n_points * 3 * 3);

  // Set tile mapping for keyframe nodes.
  unsigned cam_tile = 0;
  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    cam_tile = floor((float)cID / (float)cams_per_tile);
    graph.setTileMapping(cams_dofs[cID], cam_tile);
    graph.setTileMapping(cam_beliefs_eta.slice(cID * cam_dofs, (cID + 1) * cam_dofs), cam_tile);
    graph.setTileMapping(cam_beliefs_lambda.slice(cID * cam_dofs * cam_dofs, (cID + 1) * cam_dofs * cam_dofs), cam_tile);

    graph.setTileMapping(cam_scaling[cID], cam_tile);
    graph.setTileMapping(cam_weaken_flag[cID], cam_tile);

    // Store zero padding part of messages on variable node tile
    graph.setTileMapping(cam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * cID + n_edges_per_kf[cID] * cam_dofs,
                                                (max_nkfedges + 1) * cam_dofs * (cID + 1)), cam_tile);
    graph.setTileMapping(cam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * cID + n_edges_per_kf[cID] * cam_dofs * cam_dofs,
                                                   (max_nkfedges + 1) * cam_dofs * cam_dofs * (cID + 1) ), cam_tile);
    graph.setTileMapping(pcam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * cID + n_edges_per_kf[cID] * cam_dofs,
                                                 (max_nkfedges + 1) * cam_dofs * (cID + 1)), cam_tile);
    graph.setTileMapping(pcam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * cID + n_edges_per_kf[cID] * cam_dofs * cam_dofs,
                                                    (max_nkfedges + 1) * cam_dofs * cam_dofs * (cID + 1) ), cam_tile);
  }
  // Set tile mapping for landmark nodes.
  unsigned lmk_tile = 0;
  for (unsigned lID = 0; lID < n_points; ++lID) {
    lmk_tile = floor((float)lID / (float)lmks_per_tile);
    graph.setTileMapping(lmk_dofs[lID], lmk_tile + n_cam_tiles);
    graph.setTileMapping(lmk_beliefs_eta.slice(lID * 3, (lID + 1) * 3), lmk_tile + n_cam_tiles);
    graph.setTileMapping(lmk_beliefs_lambda.slice(lID * 3 * 3, (lID + 1) * 3 * 3), lmk_tile + n_cam_tiles);

    graph.setTileMapping(lmk_scaling[lID], lmk_tile + n_cam_tiles);
    graph.setTileMapping(lmk_weaken_flag[lID], lmk_tile + n_cam_tiles);

    // Store zero padding part of messages on variable node tile
    graph.setTileMapping(lmk_messages_eta.slice((max_nlmkedges + 1) * 3 * lID + n_edges_per_lmk[lID] * 3, (max_nlmkedges + 1) * 3 * (lID + 1)), lmk_tile + n_cam_tiles);
    graph.setTileMapping(lmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * lID + n_edges_per_lmk[lID] * 9, (max_nlmkedges + 1) * 9 * (lID + 1)), lmk_tile + n_cam_tiles);
    graph.setTileMapping(plmk_messages_eta.slice((max_nlmkedges + 1) * 3 * lID + n_edges_per_lmk[lID] * 3, (max_nlmkedges + 1) * 3 * (lID + 1)), lmk_tile + n_cam_tiles);
    graph.setTileMapping(plmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * lID + n_edges_per_lmk[lID] * 9, (max_nlmkedges + 1) * 9 * (lID + 1)), lmk_tile + n_cam_tiles);
  }

  // CREATE FACTOR NODES FOR EACH EDGE

  // Tensors for controlling damping
  Tensor damping = graph.addVariable(FLOAT, {n_edges}, "damping");
  Tensor damping_count = graph.addVariable(INT, {n_edges}, "damping_count");
  Tensor mu = graph.addVariable(FLOAT, {n_edges * (cam_dofs + 3)}, "mu");
  Tensor oldmu = graph.addVariable(FLOAT, {n_edges * (cam_dofs + 3)}, "oldmu");
  Tensor dmu = graph.addVariable(FLOAT, {n_edges}, "dmu");

  Tensor active_flag = graph.addVariable(UNSIGNED_INT, {n_edges}, "active_flag");
  Tensor robust_flag = graph.addVariable(UNSIGNED_INT, {n_edges}, "robust_flag");

  // Measurement information
  Tensor measurements = graph.addVariable(FLOAT, {n_edges * 2}, "measurements");  // 2 is the dimensionality of the measurement at a single measurement node.
  Tensor meas_variances = graph.addVariable(FLOAT, {n_edges}, "meas_variances");
  Tensor K = graph.addVariable(FLOAT, {n_edges * 9}, "K");

  // Factor potentials can be computed from the measurement and measurement variance.
  Tensor factor_potentials_eta = graph.addVariable(FLOAT, {n_edges * (cam_dofs + 3)}, "factor_potentials_eta");
  Tensor factor_potentials_lambda = graph.addVariable(FLOAT, {n_edges * (cam_dofs + 3) * (cam_dofs + 3)}, "factor_potentials_lambda");

  // Create handle to write the measurements and measurement variances at the start.
  DataStream instream_damping = graph.addHostToDeviceFIFO("instream_damping", FLOAT, n_edges);
  DataStream instream_damping_count = graph.addHostToDeviceFIFO("instream_damping_count", INT, n_edges);
  DataStream outstream_damping = graph.addDeviceToHostFIFO("outstream_damping", FLOAT, n_edges);
  DataStream outstream_damping_count = graph.addDeviceToHostFIFO("outstream_damping_count", INT, n_edges);
  DataStream instream_mu = graph.addHostToDeviceFIFO("instream_mu", FLOAT, n_edges * (cam_dofs + 3));
  DataStream instream_oldmu = graph.addHostToDeviceFIFO("instream_oldmu", FLOAT, n_edges * (cam_dofs + 3));

  DataStream instream_active_flag = graph.addHostToDeviceFIFO("instream_active_flag", UNSIGNED_INT, n_edges);
  DataStream outstream_robust_flag = graph.addDeviceToHostFIFO("outstream_robust_flag", UNSIGNED_INT, n_edges);

  DataStream instream_measurements = graph.addHostToDeviceFIFO("instream_measurements", FLOAT, n_edges * 2);
  DataStream instream_meas_variances = graph.addHostToDeviceFIFO("instream_meas_variances", FLOAT, n_edges);
  DataStream instream_K = graph.addHostToDeviceFIFO("instream_K", FLOAT, n_edges * 9);

  unsigned count_edges_per_kf [n_keyframes] = {};
  unsigned count_edges_per_lmk [n_points] = {};

  unsigned edge_tile = 0;
  for (unsigned edgeID = 0; edgeID < n_edges; ++edgeID) {
    unsigned adj_cID = measurements_camIDs[edgeID];
    unsigned adj_lID = measurements_lIDs[edgeID];

    edge_tile = n_cam_tiles + floor((float)edgeID / (float)factors_per_tile);

    // First n_keyframes + n_points tiles are used for variable nodes, so use the next n_edges tiles for factor nodes.
    graph.setTileMapping(damping[edgeID], edge_tile);
    graph.setTileMapping(damping_count[edgeID], edge_tile);
    graph.setTileMapping(mu.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3)), edge_tile);
    graph.setTileMapping(oldmu.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3)), edge_tile);
    graph.setTileMapping(dmu[edgeID], edge_tile);

    graph.setTileMapping(active_flag[edgeID], edge_tile);
    graph.setTileMapping(robust_flag[edgeID], edge_tile);

    graph.setTileMapping(measurements.slice(edgeID * 2, (edgeID + 1) * 2), edge_tile);
    graph.setTileMapping(meas_variances[edgeID], edge_tile);
    graph.setTileMapping(K.slice(edgeID * 9, (edgeID + 1) * 9), edge_tile);

    graph.setTileMapping(factor_potentials_eta.slice(edgeID * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3)), edge_tile);
    graph.setTileMapping(factor_potentials_lambda.slice(edgeID * (cam_dofs + 3) * (cam_dofs + 3), (edgeID + 1) * (cam_dofs + 3) * (cam_dofs + 3)), edge_tile);

    graph.setTileMapping(cam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs,
                                                (max_nkfedges + 1) * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs + cam_dofs), edge_tile);
    graph.setTileMapping(cam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs * cam_dofs,
                                                   (max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs * cam_dofs + cam_dofs * cam_dofs), edge_tile);
    graph.setTileMapping(pcam_messages_eta.slice((max_nkfedges + 1) * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs,
                                                (max_nkfedges + 1) * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs + cam_dofs), edge_tile);
    graph.setTileMapping(pcam_messages_lambda.slice((max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs * cam_dofs,
                                                   (max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + count_edges_per_kf[adj_cID] * cam_dofs * cam_dofs + cam_dofs * cam_dofs), edge_tile);
    graph.setTileMapping(lmk_messages_eta.slice((max_nlmkedges + 1) * 3 * adj_lID + count_edges_per_lmk[adj_lID] * 3, (max_nlmkedges + 1) * 3 * adj_lID + count_edges_per_lmk[adj_lID] * 3 + 3), edge_tile);
    graph.setTileMapping(lmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * adj_lID + count_edges_per_lmk[adj_lID] * 9, (max_nlmkedges + 1) * 9 * adj_lID + count_edges_per_lmk[adj_lID] * 9 + 9), edge_tile);
    graph.setTileMapping(plmk_messages_eta.slice((max_nlmkedges + 1) * 3 * adj_lID + count_edges_per_lmk[adj_lID] * 3, (max_nlmkedges + 1) * 3 * adj_lID + count_edges_per_lmk[adj_lID] * 3 + 3), edge_tile);
    graph.setTileMapping(plmk_messages_lambda.slice((max_nlmkedges + 1) * 9 * adj_lID + count_edges_per_lmk[adj_lID] * 9, (max_nlmkedges + 1) * 9 * adj_lID + count_edges_per_lmk[adj_lID] * 9 + 9), edge_tile);

    count_edges_per_kf[adj_cID] += 1;
    count_edges_per_lmk[adj_lID] += 1;
    }

  auto cs_relinearise = buildRelineariseCS(graph, n_keyframes, n_points, n_edges, cam_dofs, lmks_per_tile, factors_per_tile, n_cam_tiles, n_lmk_tiles,
                                          measurements_camIDs, measurements_lIDs,
                                          measurements, meas_variances, K, cam_beliefs_eta, cam_beliefs_lambda, lmk_beliefs_eta, lmk_beliefs_lambda,
                                          factor_potentials_eta, factor_potentials_lambda, robust_flag);

  auto cs_ub_red = buildUpdateBeliefsProg(graph, n_keyframes, n_points, cam_dofs, max_nkfedges, max_nlmkedges,
                                          cam_messages_eta, cam_messages_lambda, lmk_messages_eta, lmk_messages_lambda,
                                          cam_beliefs_eta, cam_beliefs_lambda, lmk_beliefs_eta, lmk_beliefs_lambda);

  auto cs_weaken_prior = buildWeakenPriorCS(graph, n_keyframes, n_points, cam_dofs, max_nkfedges, max_nlmkedges,
                                            cams_per_tile, lmks_per_tile, n_cam_tiles,
                                            cam_messages_eta, cam_messages_lambda, lmk_messages_eta, lmk_messages_lambda,
                                            cam_scaling, lmk_scaling, cam_weaken_flag, lmk_weaken_flag);

  auto csvec_computemessages = buildComputeMessagesCS(graph, n_keyframes, n_points, n_edges, cam_dofs, max_nkfedges, max_nlmkedges,
                                                  lmks_per_tile, factors_per_tile, n_cam_tiles, n_lmk_tiles, n_edges_per_kf, n_edges_per_lmk, measurements_camIDs,
                                                  measurements_lIDs, cams_dofs, lmk_dofs, active_flag, factor_potentials_eta, factor_potentials_lambda,
                                                  cam_messages_eta, cam_messages_lambda, lmk_messages_eta, lmk_messages_lambda,
                                                  pcam_messages_eta, pcam_messages_lambda, plmk_messages_eta, plmk_messages_lambda,
                                                  cam_beliefs_eta, cam_beliefs_lambda, lmk_beliefs_eta, lmk_beliefs_lambda,
                                                  measurements, meas_variances, K, damping, damping_count, mu, oldmu, dmu, robust_flag);

  Sequence prog_ub;
  for (const auto &cs : cs_ub_red) { prog_ub.add(Execute(cs));}  // Using reduction

  Sequence prog_weaken_prior;
  prog_weaken_prior.add(Execute(cs_weaken_prior));
  prog_weaken_prior.add(prog_ub);

  // Program to write initial data to tiles
  Sequence write_prog;
  write_prog.add(Copy(instream_damping, damping));
  write_prog.add(Copy(instream_damping_count, damping_count));
  write_prog.add(Copy(instream_mu, mu));
  write_prog.add(Copy(instream_oldmu, oldmu));
  write_prog.add(Copy(instream_active_flag, active_flag));
  write_prog.add(Copy(instream_cams_dofs, cams_dofs));
  write_prog.add(Copy(instream_lmk_dofs, lmk_dofs));
  write_prog.add(Copy(instream_cam_scaling, cam_scaling));
  write_prog.add(Copy(instream_lmk_scaling, lmk_scaling));
  write_prog.add(Copy(instream_cam_weaken_flag, cam_weaken_flag));
  write_prog.add(Copy(instream_lmk_weaken_flag, lmk_weaken_flag));
  write_prog.add(Copy(instream_cam_priors_eta, cam_messages_eta.reshape({n_keyframes, max_nkfedges + 1, cam_dofs}).slice({0, 0}, {n_keyframes, 1}) ));
  write_prog.add(Copy(instream_cam_priors_lambda, cam_messages_lambda.reshape({n_keyframes, max_nkfedges + 1, cam_dofs*cam_dofs}).slice({0, 0}, {n_keyframes, 1}) ));
  write_prog.add(Copy(instream_lmk_priors_eta, lmk_messages_eta.reshape({n_points, max_nlmkedges + 1, 3}).slice({0, 0}, {n_points, 1}) ));
  write_prog.add(Copy(instream_lmk_priors_lambda, lmk_messages_lambda.reshape({n_points, max_nlmkedges + 1, 9}).slice({0, 0}, {n_points, 1}) ));
  write_prog.add(Copy(instream_measurements, measurements));
  write_prog.add(Copy(instream_meas_variances, meas_variances));
  write_prog.add(Copy(instream_K, K));

  FloatingPointBehaviour behaviour(true, true, true, false, true);  // inv, div0, oflo, esr, nanoo

  Sequence linearise_prog;
  setFloatingPointBehaviour(graph, linearise_prog, behaviour, "");
  linearise_prog.add(prog_ub);  // To send priors
  linearise_prog.add(Execute(cs_relinearise)); // To compute factors for the first time

  Sequence gbp_iter_prog;
  setFloatingPointBehaviour(graph, linearise_prog, behaviour, "");
  gbp_iter_prog.add(Execute(csvec_computemessages[0]));  // Choose damping factor and relinearise if necessary
  gbp_iter_prog.add(Copy(mu, oldmu));
  gbp_iter_prog.add(Execute(csvec_computemessages[1]));
  gbp_iter_prog.add(prog_ub);
  // Messages from factor node have been used so can be copied to previous message holder
  gbp_iter_prog.add(Copy(cam_messages_eta, pcam_messages_eta));
  gbp_iter_prog.add(Copy(cam_messages_lambda, pcam_messages_lambda));
  gbp_iter_prog.add(Copy(lmk_messages_eta, plmk_messages_eta));
  gbp_iter_prog.add(Copy(lmk_messages_lambda, plmk_messages_lambda));

  // Program to stream out data from the IPU to the host
  Sequence read_prog;
  read_prog.add(Copy(cam_beliefs_eta, outstream_cam_beliefs_eta));
  read_prog.add(Copy(cam_beliefs_lambda, outstream_cam_beliefs_lambda));
  read_prog.add(Copy(lmk_beliefs_eta, outstream_lmk_beliefs_eta));
  read_prog.add(Copy(lmk_beliefs_lambda, outstream_lmk_beliefs_lambda));
  read_prog.add(Copy(damping, outstream_damping));
  read_prog.add(Copy(damping_count, outstream_damping_count));
  read_prog.add(Copy(robust_flag, outstream_robust_flag));

  Sequence read_priors_prog;
  read_priors_prog.add(Copy(cam_messages_eta.reshape({n_keyframes, max_nkfedges + 1, cam_dofs}).slice({0, 0}, {n_keyframes, 1}), outstream_cam_priors_eta));
  read_priors_prog.add(Copy(cam_messages_lambda.reshape({n_keyframes, max_nkfedges + 1, cam_dofs*cam_dofs}).slice({0, 0}, {n_keyframes, 1}), outstream_cam_priors_lambda));
  read_priors_prog.add(Copy(lmk_messages_eta.reshape({n_points, max_nlmkedges + 1, 3}).slice({0, 0}, {n_points, 1}), outstream_lmk_priors_eta));
  read_priors_prog.add(Copy(lmk_messages_lambda.reshape({n_points, max_nlmkedges + 1, 9}).slice({0, 0}, {n_points, 1}), outstream_lmk_priors_lambda));

  Sequence new_kf_prog;
  new_kf_prog.add(Copy(instream_damping_count, damping_count));
  new_kf_prog.add(Copy(instream_cam_priors_eta, cam_messages_eta.reshape({n_keyframes, max_nkfedges + 1, cam_dofs}).slice({0, 0}, {n_keyframes, 1}) ));
  new_kf_prog.add(Copy(instream_cam_priors_lambda, cam_messages_lambda.reshape({n_keyframes, max_nkfedges + 1, cam_dofs*cam_dofs}).slice({0, 0}, {n_keyframes, 1}) ));
  new_kf_prog.add(Copy(instream_lmk_priors_eta, lmk_messages_eta.reshape({n_points, max_nlmkedges + 1, 3}).slice({0, 0}, {n_points, 1}) ));
  new_kf_prog.add(Copy(instream_lmk_priors_lambda, lmk_messages_lambda.reshape({n_points, max_nlmkedges + 1, 9}).slice({0, 0}, {n_points, 1}) ));
  new_kf_prog.add(Copy(instream_active_flag, active_flag));
  new_kf_prog.add(Copy(instream_cam_weaken_flag, cam_weaken_flag));
  new_kf_prog.add(Copy(instream_lmk_weaken_flag, lmk_weaken_flag));
  new_kf_prog.add(prog_ub);  // Update belief of new nodes

  // Create the engine
  OptionFlags engineOpts {
    {"debug.instrumentCompute", "true"},
    {"debug.computeInstrumentationLevel", "tile"},
    {"debug.workerStackSizeInBytes", "4096"}
  };

  enum ProgamIds {
    WRITE_PROG = 0,
    LINEARISE_PROG,
    GBP_PROG,
    WEAKEN_PRIORS,
    READ_PROG,
    READ_PRIORS,
    NEW_KEYFRAME,
    NUM_PROGS
  };
  std::vector<poplar::program::Program> progs =
    {write_prog, linearise_prog, gbp_iter_prog, prog_weaken_prior, read_prog, read_priors_prog, new_kf_prog};

  Engine engine(graph, progs, engineOpts);
  engine.load(device);

  // Connect data streams for streaming in and out data. Connect stream to data on host.
  engine.connectStream(instream_damping, &damping_[0]);
  engine.connectStream(instream_damping_count, &damping_count_[0]);
  engine.connectStream(instream_mu, &mu_[0]);
  engine.connectStream(instream_oldmu, &oldmu_[0]);

  engine.connectStream(instream_cams_dofs, &cams_dofs_[0]);
  engine.connectStream(instream_lmk_dofs, &lmk_dofs_[0]);

  engine.connectStream(instream_cam_scaling, &cam_scaling_[0]);
  engine.connectStream(instream_lmk_scaling, &lmk_scaling_[0]);
  engine.connectStream(instream_cam_weaken_flag, &cam_weaken_flag_[0]);
  engine.connectStream(instream_lmk_weaken_flag, &lmk_weaken_flag_[0]);
  engine.connectStream(instream_active_flag, &active_flag_[0]);

  engine.connectStream(instream_cam_priors_eta, &cam_priors_eta_[0]);
  engine.connectStream(instream_cam_priors_lambda, &cam_priors_lambda_[0]);
  engine.connectStream(instream_lmk_priors_eta, &lmk_priors_eta_[0]);
  engine.connectStream(instream_lmk_priors_lambda, &lmk_priors_lambda_[0]);

  engine.connectStream(instream_measurements, &measurements_[0]);
  engine.connectStream(instream_meas_variances, &meas_variances_[0]);
  engine.connectStream(instream_K, &K_[0]);

  engine.connectStream(outstream_cam_beliefs_eta, cam_beliefs_eta_);
  engine.connectStream(outstream_cam_beliefs_lambda, cam_beliefs_lambda_);
  engine.connectStream(outstream_lmk_beliefs_eta, lmk_beliefs_eta_);
  engine.connectStream(outstream_lmk_beliefs_lambda, lmk_beliefs_lambda_);

  engine.connectStream(outstream_damping, &damping_[0]);
  engine.connectStream(outstream_damping_count, &damping_count_[0]);

  engine.connectStream(outstream_cam_priors_eta, &cam_priors_eta_[0]);
  engine.connectStream(outstream_cam_priors_lambda, &cam_priors_lambda_[0]);
  engine.connectStream(outstream_lmk_priors_eta, &lmk_priors_eta_[0]);
  engine.connectStream(outstream_lmk_priors_lambda, &lmk_priors_lambda_[0]);

  engine.connectStream(outstream_robust_flag, &robust_flag_[0]);

  // Run programs
  Engine::TimerTimePoint time0 = engine.getTimeStamp();
  std::cout << "Running program to stream initial data to IPU\n";
  engine.run(WRITE_PROG);
  std::cout << "Initial data streaming complete\n\n";

  std::cout << "Sending priors and computing factor potentials.\n";
  engine.run(LINEARISE_PROG);
  engine.run(READ_PROG);

  float reproj [2] = {};
  unsigned data_counter = 0;
  unsigned nrobust_edges = 0;
  int num_relins = 0;
  int n_new_lmks = 0;
  eval_reprojection_error(reproj, n_edges, active_flag_, cam_beliefs_eta_,
                          cam_beliefs_lambda_, lmk_beliefs_eta_, lmk_beliefs_lambda_,
                          &measurements_camIDs[0], &measurements_lIDs[0],
                          &measurements_[0], &Ksingle_[0], bad_associations);
  std::cout << "Initial Reprojection error: " << reproj[0] << " Cost " << reproj[1] << "\n";

  unsigned niters = (n_keyframes - 1) * options.iters_between_kfs - 1;
  unsigned iter = 0;
  std::cout << "Total number of GBP iterations: " << niters << "\n";
  std::cout << "GBP iterations between sucessive keyframes: " << options.iters_between_kfs << "\n";

  for (unsigned i = 0; i < (niters); ++i) {

      if ((i+1) % options.iters_between_kfs == 0) {
        iter = 0;
        data_counter += 1;  // Number of keyframes = data counter + 2

        n_new_lmks = update_flags(bal_problem, active_flag_, lmk_weaken_flag_,
                     cam_weaken_flag_, lmk_active_flag, steps, data_counter);

        std::cout << "\n**********************************************************";
        std::cout << "\n Adding keyframe " << data_counter + 1;
        std::cout << "\n Adding " << n_new_lmks << " new landmarks";
        std::cout << "\n**********************************************************\n\n";

        // Use previous kf for prior on next keyframe
        engine.run(READ_PRIORS); // Read current priors back to host

        initialise_new_kf(cam_priors_eta_, lmk_priors_eta_, cam_beliefs_eta_,
                          cam_beliefs_lambda_, cam_priors_lambda_, lmk_priors_lambda_,
                          lmk_weaken_flag_, data_counter, n_points);

        for (unsigned i = 0; i < n_edges; ++i) {
          damping_count_[i] = -15;
        }

        engine.run(NEW_KEYFRAME);
        engine.run(READ_PROG);

      }

    if (((iter+ 1) % 2 == 0) && (iter < options.steps * 2)) {
      cout << "Weakening priors \n";
      engine.run(WEAKEN_PRIORS);
    }

    engine.run(GBP_PROG); // synchronous update
    engine.run(READ_PROG); // Write beliefs out

    nrobust_edges = 0;
    for (unsigned i = 0; i < n_edges; ++i) {
      nrobust_edges += robust_flag_[i];
    }

    num_relins = 0;
    for (unsigned i = 0; i < n_edges; ++i) {
      if (damping_count_[i] == -8) {
        num_relins += 1;
      }
    }

    eval_reprojection_error(reproj, n_edges, active_flag_, cam_beliefs_eta_,
                            cam_beliefs_lambda_, lmk_beliefs_eta_, lmk_beliefs_lambda_,
                            &measurements_camIDs[0], &measurements_lIDs[0],
                            &measurements_[0], &Ksingle_[0], bad_associations);
    cout << "Iters " << options.iters_between_kfs*data_counter + iter;
    cout << " (since last kf " << iter << ") // Reprojection error " << reproj[0];
    cout << " // Cost " << reproj[1] << " // n relins: " << num_relins;
    cout << " // n robust edges " << nrobust_edges << "\n";

    if (options.verbose) {
      // Print beliefs that are streamed out
      std::cout << "\nKeyframe Eta beliefs: \n";
      for (unsigned i = 0; i <  6; ++i) {
        printf("%.12f  ", cam_beliefs_eta_[6+i]);
      };
      std::cout << "\nKeyframe Lambda beliefs: \n";
      for (unsigned i = 0; i <  36; ++i) {
        printf("%.12f  ", cam_beliefs_lambda_[36 + i]);
      }
      std::cout << '\n';

      std::cout << "\nLandmark Eta beliefs: \n";
      for (unsigned i = 0; i <  12; ++i) {
        printf("%.12f  ", lmk_beliefs_eta_[i]);
      }
      std::cout << "\nLandmark Lambda beliefs: \n";
      for (unsigned i = 0; i <  3 * 3 * 2; ++i) {
        printf("%.12f  ", lmk_beliefs_lambda_[i]);
      }
      std::cout << '\n';
    }

    iter += 1;
  }
  std::cout << "\n Finished GBP.\n";

  Engine::TimerTimePoint time1 = engine.getTimeStamp();
  string timing_report = engine.reportTiming(time0, time1);
  std::cout << timing_report << "\n";

  if (options.profile) {
    // Profiling
    char* log_dir = std::getenv("GC_PROFILE_LOG_DIR");
    std::string direc = log_dir ? std::string(log_dir) : ".";

    // Save intervals report
    std::ofstream fout(direc + "/intervals.csv");
    engine.reportIntervals(fout);

    // Graph Report
    poplar::ProfileValue graphProfile = engine.getGraphProfile();
    std::ofstream graphReport;
    graphReport.open(direc + "/graph.json");
    poplar::serializeToJSON(graphReport, graphProfile);
    graphReport.close();

    // Execution Report
    poplar::ProfileValue execProfile = engine.getExecutionProfile();
    std::ofstream execReport;
    execReport.open(direc + "/execution.json");
    poplar::serializeToJSON(execReport, execProfile);
    execReport.close();
  }

  return 0;
}


#ifndef IMPL_CONSTRAINED_SET_COVER_HPP
#define IMPL_CONSTRAINED_SET_COVER_HPP

#include <boost/dynamic_bitset.hpp>
#include <functional>
#include <unordered_map>
#include <vector>

template <typename Requirement, typename Candidate>
std::optional<std::vector<int>> constrained_set_cover(
		const std::vector<Requirement> &requirements,
		const std::vector<std::vector<Candidate>> &candidates,
		const std::function<boost::dynamic_bitset<>(const Candidate &)> &psi
) {
	struct satisfied_by {
		int candidate_id;
		boost::dynamic_bitset<> prev;
	};
	std::vector<std::unordered_map<boost::dynamic_bitset<>, satisfied_by>> D(candidates.size() + 1);
	D[0] = {{boost::dynamic_bitset<>(requirements.size()), {-1, boost::dynamic_bitset<>()}}};
	for (int i = 0; i < candidates.size(); i++) {
		for (int j = 0; j < candidates[i].size(); j++) {
			for (auto [r, _] : D[i]) {
				D[i + 1][r | psi(candidates[i][j])] = {j, r};
			}
		}
	}
	std::vector<int> res_candidate_id(candidates.size());
	boost::dynamic_bitset<> R;
	R.resize(requirements.size(), 1);
	for (int i = candidates.size(); i > 0; i--) {
		if (!D[i].count(R)) return std::nullopt;
		res_candidate_id[i - 1] = D[i][R].candidate_id;
		R = D[i][R].prev;
	}
	return res_candidate_id;
}

#endif //IMPL_CONSTRAINED_SET_COVER_HPP


// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#include <swri_transform_util/transform_util.h>

#include <cmath>
#include <algorithm>
#include <limits>
#include <utility>
#include <vector>

#include <boost/math/special_functions/sign.hpp>

#include <swri_transform_util/earth_constants.h>
#include <swri_math_util/constants.h>
#include <swri_math_util/math_util.h>
#include <swri_math_util/trig_util.h>

namespace swri_transform_util
{
  bool compare_rows(
      const std::pair<int32_t, double>& i,
      const std::pair<int32_t, double>& j)
  {
    return i.second > j.second;
  }

  tf::Transform GetRelativeTransform(
        double latitude,
        double longitude,
        double yaw,
        double reference_latitude,
        double reference_longitude,
        double reference_yaw)
  {
    tf::Transform transform = tf::Transform::getIdentity();

    tf::Quaternion reference_rotation = tf::Quaternion::getIdentity();
    reference_rotation.setRotation(tf::Vector3(0, 0, 1), reference_yaw);

    tf::Quaternion rotation = tf::Quaternion::getIdentity();
    rotation.setRotation(tf::Vector3(0, 0, 1), yaw);

    transform.setRotation(reference_rotation.inverse() * rotation);

    double x, y;
    LocalXyFromWgs84(
        latitude, longitude,
        reference_latitude, reference_longitude,
        x, y);

    tf::Vector3 origin =
        tf::Transform(reference_rotation) * tf::Vector3(x, y, 0);
    transform.setOrigin(origin);

    return transform;
  }

  double GreatCircleDistance(
      double src_latitude,
      double src_longitude,
      double dst_latitude,
      double dst_longitude)
  {
    double lat1 = src_latitude * swri_math_util::_deg_2_rad;
    double lon1 = src_longitude * swri_math_util::_deg_2_rad;

    double lat2 = dst_latitude * swri_math_util::_deg_2_rad;
    double lon2 = dst_longitude * swri_math_util::_deg_2_rad;

    double distance = 2.0 * std::asin(std::sqrt(
      std::pow(std::sin((lat1 - lat2) / 2.0), 2.0) +
      std::cos(lat1) * std::cos(lat2) *
      std::pow(std::sin((lon1 - lon2) / 2.0), 2.0)));

    return _earth_mean_radius * distance;
  }

  double GreatCircleDistance(const tf::Vector3& src, const tf::Vector3& dst)
  {
    return GreatCircleDistance(src.y(), src.x(), dst.y(), dst.x());
  }

  double GetBearing(
      double source_latitude,
      double source_longitude,
      double destination_latitude,
      double destination_longitude)
  {
    double lat1 = source_latitude * swri_math_util::_deg_2_rad;
    double lon1 = source_longitude * swri_math_util::_deg_2_rad;

    double lat2 = destination_latitude * swri_math_util::_deg_2_rad;
    double lon2 = destination_longitude * swri_math_util::_deg_2_rad;

    double d_lon = lon2 - lon1;

    double y = std::sin(d_lon) * std::cos(lat2);
    double x = std::cos(lat1) * std::sin(lat2) -
        std::sin(lat1) * std::cos(lat2) * std::cos(d_lon);

    return std::atan2(y, x) * swri_math_util::_rad_2_deg;
  }

  void GetMidpointLatLon(
      double latitude1,
      double longitude1,
      double latitude2,
      double longitude2,
      double& mid_latitude,
      double& mid_longitude)
  {
    double d_lon = (longitude2 - longitude1) * swri_math_util::_deg_2_rad;

    double lat1 = latitude1 * swri_math_util::_deg_2_rad;
    double lat2 = latitude2 * swri_math_util::_deg_2_rad;
    double lon1 = longitude1 * swri_math_util::_deg_2_rad;

    double x = std::cos(lat2) * std::cos(d_lon);
    double y = std::cos(lat2) * std::sin(d_lon);
    double tmp = std::cos(lat1) + x;
    double lat3 = std::atan2(
        std::sin(lat1) + std::sin(lat2), std::sqrt(tmp * tmp + y * y));
    double lon3 = lon1 + std::atan2(y, tmp);

    mid_latitude = lat3 * swri_math_util::_rad_2_deg;
    mid_longitude = lon3 * swri_math_util::_rad_2_deg;
  }

  double GetHeading(double src_x, double src_y, double dst_x, double dst_y)
  {
    return ToHeading(std::atan2(dst_y - src_y, dst_x - src_x));
  }

  double ToHeading(double yaw)
  {
    return swri_math_util::ToDegrees(swri_math_util::_half_pi - yaw);
  }

  double ToYaw(double heading)
  {
    return swri_math_util::ToRadians(-(heading - 90.0));
  }

  tf::Quaternion SnapToRightAngle(const tf::Quaternion& rotation)
  {
    if (rotation == tf::Quaternion::getIdentity())
    {
      return rotation;
    }

    tf::Matrix3x3 matrix(rotation);

    // First determine the order to process the rows in.  Rows with the largest
    // absolute max values will be ordered first.
    std::vector<std::pair<int32_t, double> > process_order(3);
    for (int32_t i = 0; i < 3; i++)
    {
      process_order[i].first = i;

      tf::Vector3 row = matrix.getRow(i).absolute();
      process_order[i].second = row[row.maxAxis()];
    }
    std::sort(process_order.begin(), process_order.end(), compare_rows);

    // Update the rotation matrix by operating on each row in the determined
    // order.  Each row will be aligned to its primary axis such that a single
    // element in each row will be either 1 or -1 and the rest will be 0.
    for (int32_t i = 0; i < 3; i++)
    {
      int32_t row_num = process_order[i].first;
      tf::Vector3 row = GetPrimaryAxis(matrix.getRow(row_num));

      for (int32_t j = 0; j < 3; j++)
      {
        matrix[row_num][j] = row[j];

        if (row[j] != 0)
        {
          for (int32_t k = 0; k < 3; k++)
          {
            if (k != row_num)
            {
              matrix[k][j] = 0;
            }
          }
        }
      }
    }

    // Verify that the resulting matrix is a valid rotation.
    if (!IsRotation(matrix))
    {
       // If this fails return the identity matrix.
       return tf::Quaternion::getIdentity();
    }

    tf::Quaternion snapped_rotation;
    matrix.getRotation(snapped_rotation);

    return snapped_rotation;
  }

  tf::Vector3 GetPrimaryAxis(const tf::Vector3& vector)
  {
    tf::Vector3 vector_out = vector;

    if (vector.length() > 0)
    {
      double max = 0;
      int index = 0;

      for (int32_t i = 0; i < 3; i++)
      {
        if (std::fabs(vector[i]) > max)
        {
          max = std::fabs(vector[i]);
          index = i;
        }
      }

      for (int32_t i = 0; i < 3; i++)
      {
        if (i == index)
        {
          vector_out[i] = 1.0 * boost::math::sign<double>(vector[i]);
        }
        else
        {
          vector_out[i] = 0;
        }
      }
    }

    return vector_out;
  }

  bool IsRotation(tf::Matrix3x3 matrix)
  {
    // Check that determinant is near 1.
    if (!swri_math_util::IsNear(matrix.determinant(), 1, 0.00001))
    {
      return false;
    }

    // Check that the each row is a unit vector.
    for (int32_t i = 0; i < 3; i++)
    {
      if (!swri_math_util::IsNear(matrix.getRow(i).length(), 1, 0.00001))
      {
        return false;
      }
    }

    // Check that the each column is a unit vector.
    for (int32_t i = 0; i < 3; i++)
    {
      if (!swri_math_util::IsNear(matrix.getColumn(i).length(), 1, 0.00001))
      {
        return false;
      }
    }

    return true;
  }

  tf::Matrix3x3 GetUpperLeft(const boost::array<double, 36>& matrix)
  {
    tf::Matrix3x3 sub_matrix;

    sub_matrix[0][0] = matrix[0];
    sub_matrix[0][1] = matrix[1];
    sub_matrix[0][2] = matrix[2];
    sub_matrix[1][0] = matrix[6];
    sub_matrix[1][1] = matrix[7];
    sub_matrix[1][2] = matrix[8];
    sub_matrix[2][0] = matrix[12];
    sub_matrix[2][1] = matrix[13];
    sub_matrix[2][2] = matrix[14];

    return sub_matrix;
  }

  tf::Matrix3x3 GetLowerRight(const boost::array<double, 36>& matrix)
  {
    tf::Matrix3x3 sub_matrix;

    sub_matrix[0][0] = matrix[21];
    sub_matrix[0][1] = matrix[22];
    sub_matrix[0][2] = matrix[23];
    sub_matrix[1][0] = matrix[27];
    sub_matrix[1][1] = matrix[28];
    sub_matrix[1][2] = matrix[29];
    sub_matrix[2][0] = matrix[33];
    sub_matrix[2][1] = matrix[34];
    sub_matrix[2][2] = matrix[35];

    return sub_matrix;
  }

  tf::Matrix3x3 Get3x3Cov(const boost::array<double, 9>& matrix)
  {
    tf::Matrix3x3 matrix_out;

    matrix_out[0][0] = matrix[0];
    matrix_out[0][1] = matrix[1];
    matrix_out[0][2] = matrix[2];
    matrix_out[1][0] = matrix[3];
    matrix_out[1][1] = matrix[4];
    matrix_out[1][2] = matrix[5];
    matrix_out[2][0] = matrix[6];
    matrix_out[2][1] = matrix[7];
    matrix_out[2][2] = matrix[8];

    return matrix_out;
  }

  void Set3x3Cov(
      const tf::Matrix3x3& matrix_in,
      boost::array<double, 9>& matrix_out)
  {
    matrix_out[0] = matrix_in[0][0];
    matrix_out[1] = matrix_in[0][1];
    matrix_out[2] = matrix_in[0][2];
    matrix_out[3] = matrix_in[1][0];
    matrix_out[4] = matrix_in[1][1];
    matrix_out[5] = matrix_in[1][2];
    matrix_out[6] = matrix_in[2][0];
    matrix_out[7] = matrix_in[2][1];
    matrix_out[8] = matrix_in[2][2];
  }

  void SetUpperLeft(
      const tf::Matrix3x3& sub_matrix,
      boost::array<double, 36>& matrix)
  {
    matrix[0] = sub_matrix[0][0];
    matrix[1] = sub_matrix[0][1];
    matrix[2] = sub_matrix[0][2];
    matrix[6] = sub_matrix[1][0];
    matrix[7] = sub_matrix[1][1];
    matrix[8] = sub_matrix[1][2];
    matrix[12] = sub_matrix[2][0];
    matrix[13] = sub_matrix[2][1];
    matrix[14] = sub_matrix[2][2];
  }

  void SetLowerRight(
      const tf::Matrix3x3& sub_matrix,
      boost::array<double, 36>& matrix)
  {
    matrix[21] = sub_matrix[0][0];
    matrix[22] = sub_matrix[0][1];
    matrix[23] = sub_matrix[0][2];
    matrix[27] = sub_matrix[1][0];
    matrix[28] = sub_matrix[1][1];
    matrix[29] = sub_matrix[1][2];
    matrix[33] = sub_matrix[2][0];
    matrix[34] = sub_matrix[2][1];
    matrix[35] = sub_matrix[2][2];
  }

  double LongitudeDegreesFromMeters(
    double latitude,
    double altitude,
    double arc_length)
  {
    return arc_length / ((altitude + _earth_equator_radius)
                         * std::cos(latitude * swri_math_util::_deg_2_rad)) * swri_math_util::_rad_2_deg;
  }

  double LatitudeDegreesFromMeters(
    double altitude,
    double arc_length)
  {
    return arc_length / (altitude + _earth_equator_radius) * swri_math_util::_rad_2_deg;
  }
}


#include "CinderAfterEffects.h"
#include "cinder/app/RendererGl.h"
#include "cinder/gl/gl.h"
#include "cinder/Rand.h"
#include "cinder/Vector.h"
#include "cinder/Perlin.h"
#include "cinder/Color.h"

#include <vector>
#include <cmath>
#include <functional>
#include <boost/range/irange.hpp>
#include <boost/range/algorithm_ext/erase.hpp>

using namespace ci;
using namespace ci::app;
using namespace std;
using namespace atarabi;

class Particle {
public:
	Particle(float birth, float life, vec2 position, vec2 velocity, Color color, float size) : birth_{ birth }, life_{ life }, position_{ position }, velocity_{ velocity }, color_{ color }, size_{ size } {}

	bool update(float time, float frame_duration, std::function < vec2(vec2) > &get_accel)
	{
		if (time >= birth_ + life_)
		{
			return false;
		}

		vec2 accel = get_accel(position_);
		velocity_ += accel;
		position_ += velocity_;
		return true;
	}

	void draw(gl::BatchRef &batch, float time) const
	{
		float rate = 1.f - 2.f * std::abs(time - (birth_ + 0.5f * life_)) / life_;
		float size = size_ * std::sqrt(rate);

		gl::ScopedModelMatrix scoped_model_matrix;
		gl::ScopedColor scoped_color{ color_ };
		gl::translate(vec3{ position_, 0.f });
		gl::scale(vec3{ size });
		batch->draw();
	}

private:
	float birth_;
	float life_;
	vec2 position_;
	vec2 velocity_;
	ColorA color_;
	float size_;
};

class ParticleApp : public AppAE {
public:
	void initializeAE() override;
	void setupAE() override;
	void mouseMoveAE(MouseEvent event) override;
	void updateAE() override;
	void drawAE() override;

private:
	gl::BatchRef circle_;

	Perlin			perlin_;
	std::vector<Particle> particles_;
	vec2 prev_position_;
	vec2 position_;
};

void ParticleApp::initializeAE()
{
	//create a circle
	gl::GlslProgRef shader = gl::context()->getStockShader(gl::ShaderDef().color());
	circle_ = gl::Batch::create(geom::Circle().radius(1).subdivisions(60), shader);

	//add parameters
	addParameter("Number", 100.f);
	addParameter("Life", 1.f);
	addParameter("Emitter Radius", 10.f);
	addParameter("Inherit Velocity", -50.f);
	addParameter("Perlin Intensity", 100.f);
	addParameter("Color", Color{ 1.f, 0.f, 0.f });
	addParameter("Color Variance", 20.f);
	addParameter("Size", 5.f);
}

void ParticleApp::setupAE()
{
	perlin_.setSeed(0);
	particles_.clear();
	particles_.shrink_to_fit();
	gl::enableAlphaBlending();
	prev_position_ = position_ = 0.5f * vec2{ getSize() };
}

void ParticleApp::mouseMoveAE(MouseEvent event)
{
	position_ = event.getPos();
	if (getCurrentFrame() == 0)
	{
		prev_position_ = position_;
	}

	setParameter("Mouse", position_);
}

void ParticleApp::updateAE()
{
	auto current_frame = getCurrentFrame();

	//get parameters
	float number = getParameter("Number");

	float current_life = getParameter("Life");
	float next_life = getParameter("Life", current_frame + 1);
	float delta_life = next_life - current_life;

	float emitter_radius = std::max(0.1f, static_cast<float>(getParameter("Emitter Radius")));

	float inherit_velocity = static_cast<float>(getParameter("Inherit Velocity")) * 0.01f;

	float perlin_intensity = static_cast<float>(getParameter("Perlin Intensity")) * 0.01f;

	Color current_color = getParameter("Color");
	Color next_color = getParameter("Color", current_frame + 1);
	Color delta_color = next_color - current_color;

	float color_variance = static_cast<float>(getParameter("Color Variance")) * 0.01f;

	float current_size = std::max(0.f, static_cast<float>(getParameter("Size")));
	float next_size = std::max(0.f, static_cast<float>(getParameter("Size", current_frame + 1)));
	float delta_size = next_size - current_size;

	//create particles
	float time = getCurrentTime();
	float fps = getFps();
	float frame_duration = 1.f / fps;

	int births = static_cast<int>(number * frame_duration + 0.5f);
	vec2 delta_position = position_ - prev_position_;
	vec2 velocity = length(delta_position) > 0.f ? normalize(delta_position) * inherit_velocity : vec2{};

	for (int i : boost::irange(0, births))
	{
		float rate = static_cast<float>(i) / births;
		float birth = time + frame_duration * rate;
		float life = current_life + rate * delta_life;
		vec2 position = prev_position_ + rate * delta_position;
		Color color = current_color + rate * delta_color + Color(ColorModel::CM_RGB, Rand::randVec3() * color_variance);
		float size = current_size + rate * delta_size;
		particles_.emplace_back(birth, life, position + Rand::randVec2() * Rand::randFloat(emitter_radius), velocity, color, size);
	}

	//update particles
	Perlin &perlin = perlin_;
	std::function < vec2(vec2) > get_accel = [&perlin, time, perlin_intensity](vec2 position) -> vec2 {
		vec3 deriv = perlin.dfBm({ position.x, position.y, time * 0.01f }) * perlin_intensity;
		return{ deriv.x, deriv.y };
	};

	boost::remove_erase_if(particles_, [time, frame_duration, &get_accel](Particle &particle) -> bool {
		return !particle.update(time, frame_duration, get_accel);
	});

	prev_position_ = position_;
}

void ParticleApp::drawAE()
{
	gl::clear(ColorA(0, 0, 0, 0));
	gl::ScopedViewMatrix scoped_view_matrix;

	float time = getCurrentTime();

	for (const auto &particle : particles_)
	{
		particle.draw(circle_, time);
	}
}

CINDER_APP(ParticleApp, RendererGl(RendererGl::Options().msaa(16)), [](App::Settings* settings)
{
	settings->setWindowSize(1280, 720);
	settings->setFrameRate(30.0f);
	settings->setResizable(false);
	settings->setFullScreen(false);
})


#include <boost/python.hpp>
#include <boost/python/dict.hpp>
#include <map>
#include <string>
#include <vector>
#include <tuple>
#include <stdlib.h>
#include <sstream>
#include <algorithm>
#include <iostream>
//convert int to alphabet
using namespace std;
typedef map<std::string,long double> strMap;
typedef map<std::string,strMap> MyMap;
typedef std::vector<std::string> strVec;
typedef std::vector<long double> doubleVec;

char nth_letter(int n)
{
    assert(n >= 1 && n <= 26);
    return "abcdefghijklmnopqrstuvwxyz"[n-1];
}

strVec probable(doubleVec *argmaf,strVec *arg_offspring_GT,strVec *arg_parent_GT){
	strVec possible_GT;
	doubleVec maf = *argmaf;
	strVec offspring_GT = *arg_offspring_GT;
	strVec parent_GT = *arg_parent_GT;
	if (std::find(offspring_GT.begin(),offspring_GT.end(),"00") != offspring_GT.end()){
		//if "00" is found in offspring_GT, then the fam is uninformative
		possible_GT={"0000"};
		return possible_GT;
	}
	else{
		strVec parental_GT;
		strVec alleles;
		for (int i=0; i!=maf.size(); i++){
			char letter=nth_letter(i+1);
			string s(1,letter);
			alleles.push_back(s);
		}
		for (int ait1=0; ait1!=maf.size(); ait1++){
			for (int ait2=0; ait2!=maf.size(); ait2++){
				string gt=alleles.at(ait1)+alleles.at(ait2);
				std::sort(gt.begin(),gt.end());
				if (find(parental_GT.begin(),parental_GT.end(),gt)==parental_GT.end()){
					parental_GT.push_back(gt);
				}
			}
		}
		long double prior_offgt = 0;
		strMap combined_prob;
		string fgt=parent_GT.at(0);
		string mgt=parent_GT.at(1);
		strVec possible_fgt, possible_mgt;
		if (parent_GT.at(0) == "None"){
			possible_fgt = parental_GT;
		}
		else{possible_fgt.push_back(fgt);}
		if (parent_GT.at(1) == "None"){
			possible_mgt = parental_GT;
		}
		else{possible_mgt.push_back(mgt);}
		for (auto fit=possible_fgt.begin();fit!=possible_fgt.end();fit++){
			for (auto mit=possible_mgt.begin(); mit!=possible_mgt.end(); mit++){
				string combined_gt = *fit+*mit;
				long double conditional_p=1;
				for (auto off_it=offspring_GT.begin();off_it!=offspring_GT.end();off_it++){
					double success=0;
					std::vector<int> pos1,pos2;
					string tmp_offgt = *off_it;
					for (auto tmp_it=combined_gt.begin();tmp_it!=combined_gt.end();tmp_it++){
						int idx=tmp_it-combined_gt.begin();
						if (*tmp_it==tmp_offgt.at(0)){
							pos1.push_back(idx);
						}
						if (*tmp_it==tmp_offgt.at(1)){
							pos2.push_back(idx);
						}
					}
					for (auto pit = pos1.begin();pit!=pos1.end();pit++){
						for (auto qit = pos2.begin();qit!=pos2.end();qit++){
							if (*pit<2&&*qit>1 || *pit>1&&*qit<2){
								success+=1;
							}
						}
					}
					conditional_p*=success/8;
					if (conditional_p==0){break;}
				}
				if (conditional_p==0){continue;}
				possible_GT.push_back(combined_gt);
			}
		}
		return possible_GT;
	}
}

/*int main(){
	strMap conditional_prob;
	strVec offspring_GT={"21","21"};
	long doubleVec maf={0.9,0.1};
	strVec parent_GT={"None","None"};
	conditional_prob=probable(&maf,&offspring_GT,&parent_GT);
	for (auto it=conditional_prob.begin();it!=conditional_prob.end();it++){
		cout<<it->first<<it->second<<endl;
	}
	return 0;
}*/

strVec tostrVec(boost::python::list pylist){
	strVec clist;
	for (int i=0; i<len(pylist);i++){
		string gt=boost::python::extract<std::string>(pylist[i]);
		clist.push_back(gt);
	}
	return clist;
}

doubleVec todoubleVec(boost::python::list pylist){
	doubleVec clist;
	for (int i=0; i<len(pylist);i++){
		long double gt=boost::python::extract<double>(pylist[i]);
		clist.push_back(gt);
	}
	return clist;
}

boost::python::dict todict(strMap cmap){
	boost::python::dict return_dic;
	for(auto it=cmap.begin();it!=cmap.end();it++){
		return_dic[it->first]=it->second;
	}
	return return_dic;
}

boost::python::list topythonlist(strVec pinv){
        boost::python::list postinv;
        for (auto it=pinv.begin(); it!=pinv.end(); it++){
                postinv.append(*it);
        }
        return postinv;
}

boost::python::list apply(boost::python::list pymaf, boost::python::list pyoffgt, boost::python::list pyparent_gt){
	strVec off_GT, parent_GT;
	off_GT=tostrVec(pyoffgt);
	parent_GT=tostrVec(pyparent_gt);
	doubleVec maf=todoubleVec(pymaf);
	strVec possible_GT;
	boost::python::list pypossible_GT;
	possible_GT=probable(&maf,&off_GT,&parent_GT);
	pypossible_GT=topythonlist(possible_GT);
	return pypossible_GT;
}

BOOST_PYTHON_MODULE(cmissingparents){
	using namespace boost::python;
	def("probable",probable);
	def("apply",apply);
}


/* -*- c++ -*- */
/*
 * Copyright 2013 Jiří Pinkava <j-pi@seznam.cz>.
 *
 * This is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * This software is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include "noise_level_estimator_impl.h"
#include <algorithm>
#include <boost/foreach.hpp>
#include <cmath>

#include <stdio.h>

namespace gr {
  namespace rstt {

    bool chunk_by_mean(const noise_level_estimator_impl::Chunk &l,
        const noise_level_estimator_impl::Chunk &r)
    {
      if (l.mean() == r.mean())
        return l.len() < r.len();
      return l.mean() < r.mean();
    }

    noise_level_estimator::sptr
    noise_level_estimator::make(float coverage, int nsplits)
    {
      return sptr(new noise_level_estimator_impl(coverage, nsplits));
    }

    noise_model
    noise_level_estimator_impl::estimate(const float *data, int data_items) const
    {
      idata.resize(data_items);
      float prev = 0;
      for (int idx = 0; idx < data_items; ++idx) {
        prev = idata[idx] = data[idx] + prev;
      }

      chunks.clear();
      chunks.push_back(Chunk(0, data_items, idata.data()));
      for (int chunkno = 0; chunkno < nsplits; ++chunkno) {
        Chunk chunk = chunks.back();
        chunks.pop_back();
        Chunk new_chunk = chunk.split(idata.data());
        chunks.insert(
            std::lower_bound(chunks.begin(), chunks.end(), chunk), chunk);
        chunks.insert(
            std::lower_bound(chunks.begin(), chunks.end(), new_chunk), new_chunk);
      }

      BOOST_FOREACH(Chunk &chunk, chunks) {
        chunk.shift_mean(data);
      }
      std::sort(chunks.begin(), chunks.end(), chunk_by_mean);

      float mean = 0;
      float sigma = 0;
      int len = 0;
      BOOST_FOREACH(Chunk &chunk, chunks) {
        mean += chunk.mean() * chunk.len();
        for (int idx = chunk.start(); idx < chunk.start() + chunk.len(); ++idx) {
          sigma += data[idx] * data[idx];
        }
        len += chunk.len();
        if (len >= coverage * data_items)
          break;
      }

      mean /= len;
      sigma = sqrt(sigma/len - mean*mean) * 2;

      return noise_model(mean, sigma);
    }

  } // namespace rstt
} // namespace gr


#define BOOST_TEST_MODULE BetweenTwoSetsTest

#include "BetweenTwoSets.hpp"
#include <set>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string.hpp>

BOOST_AUTO_TEST_SUITE(BetweenTwoSetsSuite)

using namespace com::github::nimelo;
using namespace std;

BOOST_AUTO_TEST_CASE(HappyPath)
{
  // Arrange
  vector<int> A = { 2, 4 };
  vector<int> B = { 16, 32, 96 };
  set<int> expected_result = { 4, 8, 16 };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_2)
{
  // Arrange
  vector<int> A = { 2, 4, 8 };
  vector<int> B = { 16, 32, 96 };
  set<int> expected_result = { 8, 16 };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_3)
{
  // Arrange
  vector<int> A = { 2, 4, 21 };
  vector<int> B = { 16, 32, 96 };
  set<int> expected_result = { };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_4)
{
  // Arrange
  vector<int> A = { 2, 3, 6 };
  vector<int> B = { 42, 7, 21 };
  set<int> expected_result = { };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_5)
{
  // Arrange
  vector<int> A = { 1, 7 };
  vector<int> B = { 14 * 3, 14, 28 };
  set<int> expected_result = { 7, 14 };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_6)
{
  // Arrange
  vector<int> A = { 1 };
  vector<int> B = { 100 };
  set<int> expected_result = { 1, 2, 4, 5, 10, 20, 25, 50, 100 };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_CASE(HappyPath_7)
{
  // Arrange
  vector<int> A = { 1 };
  vector<int> B = { 72, 48 };
  set<int> expected_result = { 1, 2, 3, 4, 6, 8, 12, 24 };

  BetweenTwoSets<int> bts(A, B);

  // Act
  auto actual_result = bts.get();

  // Assert
  BOOST_CHECK_EQUAL_COLLECTIONS(
    expected_result.begin(),
    expected_result.end(),
    actual_result.begin(),
    actual_result.end()
  );
}

BOOST_AUTO_TEST_SUITE_END()


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>

#include <scitbx/sparse/approx_equal.h>

namespace scitbx { namespace sparse { namespace boost_python {

template<typename T>
struct approx_equal_wrapper
{
  typedef approx_equal<T> wt;

  static bool vector_shared_cmp(wt const &self,
                                vector<T, af::shared> const &a,
                                vector<T, af::shared> const &b)
  {
    return self(a, b);
  }

  static bool vector_copy_cmp(wt const &self,
                              vector<T, copy_semantic_vector_container> const &a,
                              vector<T, copy_semantic_vector_container> const &b)
  {
    return self(a, b);
  }

  static bool matrix_cmp(wt const &self, matrix<T> const &a, matrix<T> const &b)
  {
    return self(a, b);
  }

  static void wrap(char const *name) {
    using namespace boost::python;
    class_<wt>(name, no_init)
      .def(init<T>(arg("tolerance")))
          .def_readwrite("tolerance", &wt::tolerance)
      .def("__call__", vector_shared_cmp)
      .def("__call__", vector_copy_cmp)
      .def("__call__", matrix_cmp);
  }
};

void wrap_approx_equal() {
  approx_equal_wrapper<double>::wrap("approx_equal");
}

}}} // scitbx::sparse::boost_python