#include "teca_integrated_vapor_transport.h"

#include "teca_cartesian_mesh.h"
#include "teca_array_collection.h"
#include "teca_variant_array.h"
#include "teca_metadata.h"
#include "teca_coordinate_util.h"

#include <algorithm>
#include <iostream>
#include <string>
#include <cmath>

#if defined(TECA_HAS_BOOST)
#include <boost/program_options.hpp>
#endif

using std::string;
using std::vector;
using std::cerr;
using std::endl;
using std::cos;

//#define TECA_DEBUG

namespace {
template <typename coord_t, typename num_t>
void cartesian_ivt(unsigned long nx, unsigned long ny,
    unsigned long nz, const coord_t *plev, const num_t *wind,
    const num_t *q, num_t *ivt)
{
    unsigned long nxy = nx*ny;
    unsigned long nxyz = nxy*nz;

    // compute the integrand
    num_t *f = (num_t*)malloc(nxyz*sizeof(num_t));
    for (unsigned long i = 0; i < nxyz; ++i)
        f[i] = wind[i]*q[i];

    // initialize the result
    memset(ivt, 0, nxy*sizeof(num_t));

    // work an x-y slice at  a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        num_t *f_k0 = f + knxy;
        num_t *f_k1 = f_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long q = 0; q < nxy; ++q)
        {
            ivt[q] += h2 * (f_k0[q] + f_k1[q]);
        }
    }

    // free up the integrand
    free(f);

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        ivt[i] *= m1g;
}

template <typename coord_t, typename num_t>
void cartesian_ivt(unsigned long nx, unsigned long ny,
    unsigned long nz, const coord_t *plev, const num_t *wind,
    const char *wind_valid, const num_t *q, const char *q_valid,
    num_t *ivt)
{
    unsigned long nxy = nx*ny;
    unsigned long nxyz = nxy*nz;

    // compute the mask
    char *mask = (char*)malloc(nxyz);
    for (unsigned long i = 0; i < nxyz; ++i)
        mask[i] = wind_valid[i] && q_valid[i] ? 1 : 0;

    // compute the integrand
    num_t *f = (num_t*)malloc(nxyz*sizeof(num_t));
    for (unsigned long i = 0; i < nxyz; ++i)
        f[i] = wind[i]*q[i];

    // initialize the result
    memset(ivt, 0, nxy*sizeof(num_t));

    // work an x-y slice at a time
    unsigned long nzm1 = nz - 1;
    for (unsigned long k = 0; k < nzm1; ++k)
    {
        // dp over the slice
        num_t h2 = num_t(0.5) * (plev[k+1] - plev[k]);

        // the current two x-y-planes of data
        unsigned long knxy = k*nxy;
        num_t *f_k0 = f + knxy;
        num_t *f_k1 = f_k0 + nxy;

        char *mask_k0 = mask + knxy;
        char *mask_k1 = mask_k0 + nxy;

        // accumulate this plane of data using trapazoid rule
        for (unsigned long q = 0; q < nxy; ++q)
        {
            ivt[q] += ((mask_k0[q] && mask_k1[q]) ?
               h2 * (f_k0[q] + f_k1[q]) : num_t(0));
        }
    }

    // free up the integrand and mask
    free(mask);
    free(f);

    // check the sign, in this way we can handle both increasing and decreasing
    // pressure coordinates
    num_t s = plev[1] - plev[0] < num_t(0) ? num_t(-1) : num_t(1);

    // scale by -1/g
    num_t m1g = s/num_t(9.80665);
    for (unsigned long i = 0; i < nxy; ++i)
        ivt[i] *= m1g;
}
}

// --------------------------------------------------------------------------
teca_integrated_vapor_transport::teca_integrated_vapor_transport() :
    wind_u_variable("ua"), wind_v_variable("va"),
    specific_humidity_variable("hus"), ivt_u_variable("ivt_u"),
    ivt_v_variable("ivt_v"), fill_value(1.0e20)
{
    this->set_number_of_input_connections(1);
    this->set_number_of_output_ports(1);
}

// --------------------------------------------------------------------------
teca_integrated_vapor_transport::~teca_integrated_vapor_transport()
{}

#if defined(TECA_HAS_BOOST)
// --------------------------------------------------------------------------
void teca_integrated_vapor_transport::get_properties_description(
    const string &prefix, options_description &global_opts)
{
    options_description opts("Options for "
        + (prefix.empty()?"teca_integrated_vapor_transport":prefix));

    opts.add_options()
        TECA_POPTS_GET(std::string, prefix, wind_u_variable,
            "name of the variable containg the lon component of the wind vector")
        TECA_POPTS_GET(std::string, prefix, wind_v_variable,
            "name of the variable containg the lat component of the wind vector")
        TECA_POPTS_GET(std::string, prefix, specific_humidity_variable,
            "name of the variable containg the specific humidity")
        TECA_POPTS_GET(double, prefix, fill_value,
            "the value of the NetCDF _FillValue attribute")
        ;

    this->teca_algorithm::get_properties_description(prefix, opts);

    global_opts.add(opts);
}

// --------------------------------------------------------------------------
void teca_integrated_vapor_transport::set_properties(
    const string &prefix, variables_map &opts)
{
    this->teca_algorithm::set_properties(prefix, opts);

    TECA_POPTS_SET(opts, std::string, prefix, wind_u_variable)
    TECA_POPTS_SET(opts, std::string, prefix, wind_v_variable)
    TECA_POPTS_SET(opts, std::string, prefix, specific_humidity_variable)
    TECA_POPTS_SET(opts, double, prefix, fill_value)
}
#endif

// --------------------------------------------------------------------------
teca_metadata teca_integrated_vapor_transport::get_output_metadata(
    unsigned int port,
    const std::vector<teca_metadata> &input_md)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_vapor_transport::get_output_metadata" << std::endl;
#endif
    (void)port;

    // set things up in the first pass, and don't modify in subsequent passes
    // due to threading concerns

    if (this->get_number_of_derived_variables() == 0)
    {
        // the base class will handle dealing with the transformation of
        // mesh dimensions and reporting the array we produce, but we have
        // to determine the data type and tell the name of the produced array.
        const teca_metadata &md = input_md[0];

        teca_metadata attributes;
        if (md.get("attributes", attributes))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes are misisng")
            return teca_metadata();
        }

        teca_metadata u_atts;
        if (attributes.get(this->wind_u_variable, u_atts))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->wind_u_variable
                << "\" are misisng")
            return teca_metadata();
        }

        int type_code = 0;
        if (u_atts.get("type_code", type_code))
        {
            TECA_FATAL_ERROR("Failed to determine output data type "
                "because attributes for \"" << this->wind_u_variable
                << "\" is misisng a \"type_code\"")
            return teca_metadata();
        }

        teca_array_attributes ivt_u_atts(
            type_code, teca_array_attributes::point_centering,
            0, "kg m^{-1} s^{-1}", "longitudinal integrated vapor transport",
            "the longitudinal component of integrated vapor transport",
            1, this->fill_value);

        teca_array_attributes ivt_v_atts(
            type_code, teca_array_attributes::point_centering,
            0, "kg m^{-1} s^{-1}", "latitudinal integrated vapor transport",
            "the latitudinal component of integrated vapor transport",
            this->fill_value);

        // install name and attributes of the output variables in the base classs
        this->append_derived_variable(this->ivt_u_variable);
        this->append_derived_variable(this->ivt_v_variable);

        this->append_derived_variable_attribute(ivt_u_atts);
        this->append_derived_variable_attribute(ivt_v_atts);

    }

    if (this->get_number_of_dependent_variables() == 0)
    {
        // install the names of the input variables in the base class
        this->append_dependent_variable(this->wind_u_variable);
        this->append_dependent_variable(this->wind_v_variable);
        this->append_dependent_variable(this->specific_humidity_variable);
    }

    // invoke the base class method, which does the work of transforming
    // the mesh and reporting the variables and their attributes.
    return teca_vertical_reduction::get_output_metadata(port, input_md);
}

// --------------------------------------------------------------------------
std::vector<teca_metadata> teca_integrated_vapor_transport::get_upstream_request(
    unsigned int port,
    const std::vector<teca_metadata> &input_md,
    const teca_metadata &request)
{
    // invoke the base class method
    return teca_vertical_reduction::get_upstream_request(port, input_md, request);
}

// --------------------------------------------------------------------------
const_p_teca_dataset teca_integrated_vapor_transport::execute(
    unsigned int port,
    const std::vector<const_p_teca_dataset> &input_data,
    const teca_metadata &request)
{
#ifdef TECA_DEBUG
    std::cerr << teca_parallel_id()
        << "teca_integrated_vapor_transport::execute" << std::endl;
#endif
    (void)port;

    // get the input mesh
    const_p_teca_cartesian_mesh in_mesh
        = std::dynamic_pointer_cast<const teca_cartesian_mesh>(input_data[0]);

    if (!in_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because a cartesian mesh is required.")
        return nullptr;
    }

    // get the input dimensions
    unsigned long extent[6] = {0};
    if (in_mesh->get_extent(extent))
    {
        TECA_FATAL_ERROR("Failed to compute IVT because mesh extent is missing.")
        return nullptr;
    }

    unsigned long nx = extent[1] - extent[0] + 1;
    unsigned long ny = extent[3] - extent[2] + 1;
    unsigned long nz = extent[5] - extent[4] + 1;

    // get the pressure coordinates
    const_p_teca_variant_array p = in_mesh->get_z_coordinates();
    if (!p)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because pressure coordinates are missing")
        return nullptr;
    }

    if (p->size() < 2)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because z dimensions "
            << p->size() << " < 2 as required by the integration method")
        return nullptr;
    }

    // gather the input arrays
    const_p_teca_variant_array wind_u =
        in_mesh->get_point_arrays()->get(this->wind_u_variable);

    if (!wind_u)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because longitudinal wind \""
            << this->wind_u_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array wind_u_valid =
           in_mesh->get_point_arrays()->get(this->wind_u_variable + "_valid");

    const_p_teca_variant_array wind_v =
        in_mesh->get_point_arrays()->get(this->wind_v_variable);

    if (!wind_v)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because latitudinal wind \""
            << this->wind_v_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array wind_v_valid =
           in_mesh->get_point_arrays()->get(this->wind_v_variable + "_valid");

    const_p_teca_variant_array q =
        in_mesh->get_point_arrays()->get(this->specific_humidity_variable);

    if (!q)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because specific humidity \""
            << this->specific_humidity_variable << "\" is missing")
        return nullptr;
    }

    const_p_teca_variant_array q_valid =
           in_mesh->get_point_arrays()->get(this->specific_humidity_variable + "_valid");

    // the base class will construct the output mesh
    p_teca_cartesian_mesh out_mesh
        = std::dynamic_pointer_cast<teca_cartesian_mesh>(
            std::const_pointer_cast<teca_dataset>(
                teca_vertical_reduction::execute(port, input_data, request)));

    if (!out_mesh)
    {
        TECA_FATAL_ERROR("Failed to compute IVT because the output mesh was "
            "not constructed")
        return nullptr;
    }

    // allocate the output arrays
    unsigned long nxy = nx*ny;
    p_teca_variant_array ivt_u = wind_u->new_instance(nxy);
    p_teca_variant_array ivt_v = wind_u->new_instance(nxy);

    // store the result
    out_mesh->get_point_arrays()->set(this->ivt_u_variable, ivt_u);
    out_mesh->get_point_arrays()->set(this->ivt_v_variable, ivt_v);

    // calculate IVT
    NESTED_TEMPLATE_DISPATCH_FP(const teca_variant_array_impl,
        p.get(), _COORDS,

        const NT_COORDS *p_p = static_cast<TT_COORDS*>(p.get())->get();

        NESTED_TEMPLATE_DISPATCH_FP(teca_variant_array_impl,
            ivt_u.get(), _DATA,

            NT_DATA *p_ivt_u = static_cast<TT_DATA*>(ivt_u.get())->get();
            NT_DATA *p_ivt_v = static_cast<TT_DATA*>(ivt_v.get())->get();

            const NT_DATA *p_wind_u = static_cast<const TT_DATA*>(wind_u.get())->get();
            const NT_DATA *p_wind_v = static_cast<const TT_DATA*>(wind_v.get())->get();
            const NT_DATA *p_q = static_cast<const TT_DATA*>(q.get())->get();

            const char *p_wind_u_valid = nullptr;
            const char *p_wind_v_valid = nullptr;
            const char *p_q_valid = nullptr;
            if (wind_u_valid)
            {
                using TT_MASK = teca_char_array;

                p_wind_u_valid = dynamic_cast<const TT_MASK*>(wind_u_valid.get())->get();
                p_wind_v_valid = dynamic_cast<const TT_MASK*>(wind_v_valid.get())->get();
                p_q_valid = dynamic_cast<const TT_MASK*>(q_valid.get())->get();

                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_u, p_wind_u_valid, p_q, p_q_valid, p_ivt_u);
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_v, p_wind_v_valid, p_q, p_q_valid, p_ivt_v);
            }
            else
            {
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_u, p_q, p_ivt_u);
                ::cartesian_ivt(nx, ny, nz, p_p, p_wind_v, p_q, p_ivt_v);
            }
            )
        )

    // pass 2D arrays through.
    p_teca_array_collection in_arrays =
        std::const_pointer_cast<teca_array_collection>(in_mesh->get_point_arrays());

    p_teca_array_collection out_arrays = out_mesh->get_point_arrays();

    int n_arrays = in_arrays->size();
    for (int i = 0; i < n_arrays; ++i)
    {
        p_teca_variant_array array = in_arrays->get(i);
        if (array->size() == nxy)
        {
            // pass the array.
            out_arrays->append(in_arrays->get_name(i), array);
        }
    }

    return out_mesh;
}


#include <Eigen/Dense>
#include <assert.h>
#include <boost/math/special_functions/sign.hpp>
#include <iostream>
#include <math.h>

#include "grid.hpp"
#include "spdlog/spdlog.h"

using namespace Eigen;
using namespace SnowSimulator;

// ============================================================================
// GRIDCELL METHODS
// ============================================================================

GridCell::GridCell() {}

void GridCell::addMaterialPoint(MaterialPoint *materialPoint) {
  m_materialPoints.push_back(materialPoint);
  materialPoint->m_cell = this;
}

void GridCell::clear() { m_materialPoints.clear(); }

// ============================================================================
// GRIDNODE METHODS
// ============================================================================

GridNode::GridNode(Vector3i idx, Grid *grid)
    : m_idx(idx), m_grid(grid), m_mass(0), m_velocity(Vector3f::Zero()),
      m_nextVelocity(Vector3f::Zero()), m_force(Vector3f::Zero()) {}

Vector3f GridNode::position() const {
  return m_idx.cast<float>() * m_grid->m_spacing;
}

void GridNode::zeroForce() { m_force = Vector3f::Zero(); }

void GridNode::addForce(Vector3f force) { m_force += force; }

/**
 * The cubic B-spline formulation of the grid basis function used for
 * determining how particle properties are transferred to the grid. Both
 * this function and the gradient of this function below are called for each
 * pair of particle and grid node. This is referred to as N in the paper.
 */
float GridNode::cubicBSpline(float x) const {
  float absx = fabs(x);

  if (absx >= 2) {
    return 0;
  } else if (absx < 1) {
    return (0.5f * absx - 1.f) * absx * absx + (2.0f / 3);
  } else {
    return (((-1.0f / 6) * absx + 1) * absx - 2) * absx + (4.0f / 3);
  }
}

float GridNode::gradCubicBSpline(float x) const {
  float absx = fabs(x);
  // int sign = copysign(1, x);
  int sign = x > 0 ? 1 : -1;

  if (absx < 1) {
    return sign * (1.5 * absx - 2) * absx;
  } else if (absx < 2) {
    return sign * ((-0.5f * absx + 2) * absx - 2);
  } else {
    return 0;
  }
}

/**
 * Computes the weight necessary for rasterizing the properties of a
 * materialPoint to the grid. This is referred to as w in the paper.
 */
float GridNode::basisFunction(Vector3f particlePos) const {
  Vector3f offset = particlePos / m_grid->m_spacing - m_idx.cast<float>();

  return cubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
         cubicBSpline(offset.z());
}

Vector3f GridNode::gradBasisFunction(Vector3f particlePos) const {
  float invSpacing = 1.0f / m_grid->m_spacing;
  Vector3f offset = invSpacing * particlePos - m_idx.cast<float>();

  float gx = gradCubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
             cubicBSpline(offset.z());
  float gy = cubicBSpline(offset.x()) * gradCubicBSpline(offset.y()) *
             cubicBSpline(offset.z());
  float gz = cubicBSpline(offset.x()) * cubicBSpline(offset.y()) *
             gradCubicBSpline(offset.z());
  return Vector3f(gx, gy, gz);
}

// ============================================================================
// GRID METHODS
// ============================================================================

Grid::Grid(Vector3f origin, Vector3i dimensions, float spacing)
    : m_origin(origin), m_dim(dimensions), m_spacing(spacing) {
  auto logger = spdlog::get("snowsim");

  logger->info("Creating grid of size ({}, {}, {})", m_dim.x(), m_dim.y(),
               m_dim.z());

  int num_nodes = m_dim.prod();
  logger->info("Total node count: {}", num_nodes);

  // Allocate space for each of the gridNodes

  logger->info("Allocating grid nodes and cells...");

  for (int i = 0; i < num_nodes; i++) {
    GridNode *node = new GridNode(idxToVector(i), this);
    m_gridNodes.push_back(node);
    m_gridCells.push_back(new GridCell());
  }

  // Make sure each GridNode has a reference to the 4^3 surrounding GridCells
  // within the support radius of the basis function.
  // Each GridCell also needs to know about the 6^3 surrounding GridNodes.

  logger->info("Linking grid nodes to neighboring cells...");

  for (int i = 0; i < num_nodes; i++) {
    Vector3i idx = idxToVector(i);
    GridNode *current = m_gridNodes[i];

    // GridCell indexing is such that the corresponding GridNode is at the
    // top-left corner of the GridCell.

    Vector3i minIdx = (idx.array() - 2).max(0);
    Vector3i maxIdx = (idx.array() + 1).min(m_dim.array() - 1);

    for (int ox = minIdx.x(); ox < maxIdx.x(); ox++) {
      for (int oy = minIdx.y(); oy < maxIdx.y(); oy++) {
        for (int oz = minIdx.z(); oz < maxIdx.z(); oz++) {
          int cellIdx = vectorToIdx(Vector3i(ox, oy, oz));
          GridCell *neighborCell = m_gridCells[cellIdx];
          current->m_neighborCells.push_back(neighborCell);
        }
      }
    }
  }
}

std::vector<GridNode *> Grid::getAllNodes() { return m_gridNodes; }


///////////////////////////////////////////////////////////////////////////////
// asy_distribution.hpp                                                      //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_ASY_DISTRIBUTION_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_ASY_DISTRIBUTION_HPP_ER_2010
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/policies/policy.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{

    namespace result_of{

        template<typename T1,
            typename Policy = boost::math::policies::policy<> >
        struct asy_distribution{
            typedef boost::math::chi_squared_distribution<T1,Policy> type;
        };

    }// result_of

    template<typename T1,typename AccSet,typename H0,typename Policy>
    typename pearson_chisq_statistic::result_of::asy_distribution<T1,Policy>::type
    asy_distribution(
        const H0& hypothesis,
        const AccSet& acc,
        const Policy& pol
    )
    {
        namespace ns = pearson_chisq_statistic;
        typedef typename ns::result_of::asy_distribution<
            T1,Policy>::type result_;
        T1 df = static_cast<T1>( ns::degrees_of_freedom( hypothesis, acc ) );
        return result_( df );
    }

    template<typename T1,typename AccSet,typename H0>
    typename pearson_chisq_statistic::result_of::asy_distribution<T1>::type
    asy_distribution(
        const H0& hypothesis,
        const AccSet& acc
    ){
        typedef boost::math::policies::policy<> pol_;
        return asy_distribution<T1>( hypothesis, acc, pol_() );
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


//
// Created by keszocze on 10.10.18.
//

#pragma once

#include <vector>
#include <cudd/cplusplus/cuddObj.hh>

#include <boost/multiprecision/cpp_int.hpp>

namespace abo::error_metrics {

    /**
     * @brief Computes the error rate, i.e. the number of inputs for which f_hat differs from f
     * The result is scaled to lie between 0 and 1
     * The error rate is computed symbolically using BDDs
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @return The computed error rate in the interval [0, 1]
     */
    double error_rate(const Cudd & mgr, const  std::vector<BDD>& f, const std::vector<BDD>& f_hat);
    double error_rate(const Cudd & mgr, const BDD& f, const BDD& f_hat);

    /**
     * @brief Computes the error rate, i.e. the number of inputs for which f_hat differs from f
     * The result is scaled to lie between 0 and 1
     * The error rate is computed symbolically using ADDs
     * The computation time will be significantly longer than the BDD variant
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @return The computed error rate in the interval [0, 1]
     */
    double error_rate_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat);

    /**
     * @brief Approximates the error rate, i.e. the number of inputs for which f_hat differs from f
     * The result is scaled to lie between 0 and 1
     * The result is computed using sampling, no proper guarantee on the introduced error can be given
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @param samples The number of samples to use
     * @return The approximated error rate in the interval [0, 1]
     */
    double error_rate_sampling(const Cudd &mgr, const std::vector<BDD>& f, const std::vector<BDD>& f_hat, long samples = 10000);

    /**
     * @brief Approximates the error rate, i.e. the number of inputs for which f_hat differs from f
     * The result is scaled to lie between 0 and 1
     * The result is computed using an improved sampling method that offers faster convergence than
     * the regular error rate, but still no proper guarantee on the introduced error can be given
     * @param mgr The BDD object manager
     * @param f The original function
     * @param f_hat The approximated function. Must have the same number of bits as f
     * @param samples The number of samples to use
     * @return The approximated error rate in the interval [0, 1]
     */
    double error_rate_efficient_sampling(const Cudd &mgr, const std::vector<BDD>& f, const std::vector<BDD>& f_hat, long samples = 10000);
}


#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Core>

std::vector<Eigen::VectorXd> read_coordinates(std::string file_name) {
  std::string s;
  std::vector<Eigen::VectorXd> return_points;
  std::ifstream scan_poses_file (file_name);
  if (scan_poses_file.is_open())
  {
    while ( getline (scan_poses_file,s) )
    {
      Eigen::VectorXd pointPose(3);
      int ind = 0;
      // parse the string
      std::string delimiter = " ";
      size_t pos = 0;
      std::string token;
      while ((pos = s.find(delimiter)) != std::string::npos) {
          token = s.substr(0, pos);
          pointPose(ind) =  std::stod(token);
          s.erase(0, pos + delimiter.length());
          ind += 1;
      }
      pointPose(ind) = std::stod(s);
      return_points.push_back(pointPose);
    }
    scan_poses_file.close();
  }
  return return_points;

}

int main(void)
{

    std::vector<Eigen::VectorXd> img_1 = read_coordinates("/home/nehil/catkin_ws_registration/src/tracking_results/aruco_in_pos_2.txt");
    std::vector<Eigen::VectorXd> img_2 = read_coordinates("/home/nehil/catkin_ws_registration/src/tracking_results/aruco_in_pos_3.txt");
    std::cout << img_1.size() << std::endl;
    std::cout << img_2.size() << std::endl;
    double distance_x = 0;
    double distance_y = 0;
    double distance_z = 0;
    double distance = 0;
    for(unsigned int i = 0; i < img_1.size(); i++) {
      distance_x += std::abs((img_2[i].x() - img_1[i].x()));
      distance_y += std::abs((img_2[i].y() - img_1[i].y()));
      distance_z += (img_2[i].z() - img_1[i].z());
      auto point_dist = std::sqrt(std::pow(img_2[i].x() - img_1[i].x(), 2) + std::pow(img_2[i].y() - img_1[i].y(), 2) + std::pow(img_2[i].z() - img_1[i].z(), 2)) - 206.0f;
      std::cout << i + 1 << " : " << point_dist << std::endl;
      distance += point_dist;
    }
    //std::cout << "Diff in x: " << distance_x/img_1.size() << "\nDiff in y: " << distance_y/img_1.size() << "\nDiff in z: " << distance_z/img_1.size() << "\n";
    std::cout << "Euclidean distance: " << distance/img_1.size() << std::endl;
    return 0;
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"
#include "matmul.h"
#include <NTL/ZZ.h>
#include <NTL/lzz_pXFactoring.h>
#include <cassert>
#include <cstdio>

static bool noPrint = false;

static MatMulBase*
buildSingleBlockMatrix(const EncryptedArray& ea, const vector<ZZX>& vec);

template<class type> class SingleBlockMatrix : public BlockMatMul<type> {
  PA_INJECT(type)

  Mat<R> data;

public:
  SingleBlockMatrix(const EncryptedArray& _ea, const vector<ZZX>& vec) :
    BlockMatMul<type>(_ea)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long d = _ea.getDegree();

    data.SetDims(d, d);
    for (long i = 0; i < d; i++)
      for (long j = 0; j < d; j++)
         conv(data[i][j], coeff(vec[i], j));
  }

  virtual bool get(Mat<R>& out, long i, long j) const
  {
    assert(i >= 0 && i < this->getEA().size());
    assert(j >= 0 && j < this->getEA().size());
    if (i != j) return true;
    out = data;
    return false;
  }
};

static MatMulBase*
buildSingleBlockMatrix(const EncryptedArray& ea, const vector<ZZX>& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new SingleBlockMatrix<PA_GF2>(ea, vec);
    }
    case PA_zz_p_tag: {
      return new SingleBlockMatrix<PA_zz_p>(ea, vec);
    }
    default: return nullptr;
  }
}

template<class type> class MultiBlockMatrix : public BlockMatMul<type> {
  PA_INJECT(type)

  Vec< Mat<R> > data;

public:
  MultiBlockMatrix(const EncryptedArray& _ea, const vector<vector<ZZX> >& vec):
    BlockMatMul<type>(_ea)
  {
    RBak bak; bak.save(); _ea.getAlMod().restoreContext();
    long n = _ea.size();
    long d = _ea.getDegree();

    data.SetLength(n);
    for (long k = 0; k < n; k++) {
      data[k].SetDims(d, d);
      for (long i = 0; i < d; i++)
        for (long j = 0; j < d; j++)
           conv(data[k][i][j], coeff(vec[k][i], j));
    }
  }

  virtual bool get(Mat<R>& out, long i, long j) const {
    assert(i >= 0 && i < this->getEA().size());
    assert(j >= 0 && j < this->getEA().size());
    if (i != j) return true;
    out = data[i];
    return false;
  }
};

static MatMulBase* buildMultiBlockMatrix(const EncryptedArray& ea,
					 const vector< vector<ZZX> >& vec)
{
  switch (ea.getTag()) {
    case PA_GF2_tag: {
      return new MultiBlockMatrix<PA_GF2>(ea, vec);
    }
    case PA_zz_p_tag: {
      return new MultiBlockMatrix<PA_zz_p>(ea, vec);
    }
    default: return nullptr;
  }
}

void  TestIt(long m, long p, long r, long d)
{
  if (!noPrint)
    cout << "\n\n******** TestIt" << (isDryRun()? "(dry run):" : ":")
       << " m=" << m
       << ", p=" << p
       << ", r=" << r
       << ", d=" << d
       << endl;

  FHEcontext context(m, p, r);
  buildModChain(context, /*L=*/3, /*c=*/2);

  ZZX G;
  if (d == 0) {
    G = context.alMod.getFactorsOverZZ()[0];
    d = deg(G);
  }
  else
    G = makeIrredPoly(p, d);

  if (!noPrint) {
    context.zMStar.printout();
    cout << endl;
    cout << "G = " << G << "\n";

    cout << "generating keys and key-switching matrices... " << std::flush;
  }
  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(/*w=*/64);// A Hamming-weight-w secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need
  if (!noPrint) {
    cout << "done\n";
    cout << "computing masks and tables for rotation... " << std::flush;
  }
  EncryptedArray ea(context, G);
  if (!noPrint)
    cout << "done\n";

  long nslots = ea.size();

  NewPlaintextArray p0(ea);
  NewPlaintextArray pp0(ea);

  Ctxt c0(publicKey);

  if (!noPrint)
    cout << "\nTest #1: Apply the same linear transformation to all slots\n";
  {
  vector<ZZX> LM(d); // LM selects even coefficients
  for (long j = 0; j < d; j++)
    if (j % 2 == 0) LM[j] = ZZX(j, 1);

  // "building" the linearized-polynomial coefficients
  vector<ZZX> C;
  ea.buildLinPolyCoeffs(C, LM);

  random(ea, p0);
  ea.encrypt(c0, publicKey, p0);
  applyLinPoly1(ea, c0, C);
  ea.decrypt(c0, secretKey, pp0);

  shared_ptr<MatMulBase> mat(buildSingleBlockMatrix(ea, LM));
  NewPlaintextArray p1(p0);
  blockMatMul(p1, *mat);
  if (equals(ea, pp0, p1))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  if (!noPrint)
    cout << "\nTest #2: Apply different transformations to the different slots\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] rotates the coefficients in the i'th slot by (i % d)
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      long jj = (i+j) % d;
      LM[i][j] = ZZX(jj, 1);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  random(ea, p0);
  ea.encrypt(c0, publicKey, p0);
  applyLinPolyMany(ea, c0, C); // apply the linearized polynomials
  ea.decrypt(c0, secretKey, pp0);

  shared_ptr<MatMulBase> mat(buildMultiBlockMatrix(ea, LM));
  NewPlaintextArray p1(p0);
  blockMatMul(p1, *mat);
  if (equals(ea, pp0, p1))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }

  if (!noPrint)
    cout << "\nTest #3: Testing low-level (cached) implementation\n";
  {
  vector< vector<ZZX> > LM(nslots);
  // LM[i] adds coefficients (i % d) and (i+1 % d) in the i'th slot
  for (long i = 0; i < nslots; i++) {
    LM[i].resize(d);
    for (long j = 0; j < d; j++)  {
      if ( j == (i % d) || j == ((i+1)%d) )
	LM[i][j] = conv<ZZX>(1L);
    }
  }

  // "building" the linearized-polynomial coefficients
  vector< vector<ZZX> > C(nslots);
  for (long i = 0; i < nslots; i++)
    ea.buildLinPolyCoeffs(C[i], LM[i]);

  // "encoding" the linearized-polynomial coefficients
  vector<ZZX> encodedC(d);
  for (long j = 0; j < d; j++) {
    vector<ZZX> v(nslots);
    for (long i = 0; i < nslots; i++) v[i] = C[i][j];
    ea.encode(encodedC[j], v);
  }

  random(ea, p0);
  ea.encrypt(c0, publicKey, p0);
  applyLinPolyLL(c0, encodedC, ea.getDegree()); // apply linearized polynomials
  ea.decrypt(c0, secretKey, pp0);

  shared_ptr<MatMulBase> mat(buildMultiBlockMatrix(ea, LM));
  NewPlaintextArray p1(p0);
  blockMatMul(p1, *mat);
  if (equals(ea, pp0, p1))
    cout << "GOOD\n";
  else
    cout << "BAD\n";
  }
}

int main(int argc, char *argv[])
{
  ArgMapping amap;

  bool dry = false;
  amap.arg("dry", dry, "dry=1 for a dry-run");

  long m=91;
  amap.arg("m", m, "use specified value as modulus");

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long d=0;
  amap.arg("d", d, "degree of the field extension");
  amap.note("d == 0 => factors[0] defines extension");

  amap.arg("noPrint", noPrint, "suppress printouts");

  amap.parse(argc, argv);

  long repeat = 2;
  setTimersOn();
  setDryRun(dry);
  for (long repeat_cnt = 0; repeat_cnt < repeat; repeat_cnt++) {
    TestIt(m, p, r, d);
  }

}


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

#include "galois/Galois.h"
#include "galois/Timer.h"
#include "galois/Timer.h"
#include "galois/graphs/Graph.h"
#include "galois/graphs/LCGraph.h"
#include "galois/Reduction.h"
#include "galois/ParallelSTL.h"
#include "galois/substrate/PaddedLock.h"
#include "Lonestar/BoilerPlate.h"

#ifdef HAS_EIGEN
#include <Eigen/Sparse>
#endif

#include <random>
#include <iostream>
#include <cassert>
#include <algorithm>
#include <fstream>
#include <vector>

/**           CONFIG           **/

static const char* const name =
    "Stochastic Gradient Descent for Linear Support Vector Machines";
static const char* const desc = "Implements a linear support vector machine "
                                "using stochastic gradient descent";
static const char* const url = "sgdsvm";

enum class UpdateType {
  Wild,
  WildOrig,
  ReplicateByThread,
  ReplicateBySocket,
  Staleness
};

enum class AlgoType {
  PrimalStochasticGradientDescent,
  LeastSquares,
  DualCoordinateDescentL1Loss,
  DualCoordinateDescentL2Loss
};

namespace cll = llvm::cl;
static cll::opt<std::string> inputGraphFilename(cll::Positional,
                                                cll::desc("<graph input file>"),
                                                cll::Required);
static cll::opt<std::string> inputLabelFilename(cll::Positional,
                                                cll::desc("<label input file>"),
                                                cll::Required);
static cll::opt<double>
    creg("creg", cll::desc("the regularization parameter C"), cll::init(1.0));
static cll::opt<bool>
    shuffleSamples("shuffle", cll::desc("shuffle samples between iterations"),
                   cll::init(false));
static cll::opt<unsigned> SEED("seed", cll::desc("random seed"),
                               cll::init(~0U));
static cll::opt<bool> printObjective("printObjective",
                                     cll::desc("print objective value"),
                                     cll::init(false));
static cll::opt<bool> printAccuracy("printAccuracy",
                                    cll::desc("print accuracy value"),
                                    cll::init(true));
static cll::opt<double>
    fractionTraining("fractionTraining",
                     cll::desc("fraction of samples to use for training"),
                     cll::init(0.8));
static cll::opt<size_t>
    numberTraining("numberTraining",
                   cll::desc("number of samples to use for training"),
                   cll::init(0));
static cll::opt<double> tol("tol", cll::desc("convergence tolerance"),
                            cll::init(0.1));
static cll::opt<unsigned>
    maxIterations("maxIterations", cll::desc("maximum number of iterations"),
                  cll::init(1000));
static cll::opt<unsigned> fixedIterations(
    "fixedIterations",
    cll::desc("run specific number of iterations, ignoring convergence"),
    cll::init(0));
static cll::opt<UpdateType> updateType(
    "update", cll::desc("Update type:"),
    cll::values(clEnumValN(UpdateType::Wild, "wild",
                           "unsynchronized (default)"),
                clEnumValN(UpdateType::WildOrig, "wildorig", "unsynchronized"),
                clEnumValN(UpdateType::ReplicateByThread, "replicateByThread",
                           "thread replication"),
                clEnumValN(UpdateType::ReplicateBySocket, "replicateBySocket",
                           "socket replication"),
                clEnumValN(UpdateType::Staleness, "staleness", "stale reads"),
                clEnumValEnd),
    cll::init(UpdateType::Wild));
static cll::opt<AlgoType> algoType(
    "algo", cll::desc("Algorithm:"),
    cll::values(
        clEnumValN(AlgoType::PrimalStochasticGradientDescent, "psgd",
                   "primal stochastic gradient descent (default)"),
        clEnumValN(AlgoType::DualCoordinateDescentL1Loss, "dcdl1",
                   "Dual coordinate descent L1 loss"),
        clEnumValN(AlgoType::DualCoordinateDescentL2Loss, "dcdl2",
                   "Dual coordinate descent L2 loss"),
#ifdef HAS_EIGEN
        clEnumValN(AlgoType::LeastSquares, "ls",
                   "minimize l2 norm of residual as least squares problem"),
#endif
        clEnumValEnd),
    cll::init(AlgoType::PrimalStochasticGradientDescent));

/**          DATA TYPES        **/

typedef struct Node {
  double w;  // weight - relevant for variable nodes
  int field; // variable nodes - variable count, sample nodes - label
  Node() : w(0.0), field(0) {}
} Node;

using Graph =
    galois::graphs::LC_CSR_Graph<Node,
                                 double>::with_out_of_line_lockable<true>::type;
using GNode = Graph::GraphNode;

/**         CONSTANTS AND PARAMETERS       **/
unsigned NUM_SAMPLES   = 0;
unsigned NUM_VARIABLES = 0;

unsigned variableNodeToId(GNode variable_node) {
  return ((unsigned)variable_node) - NUM_SAMPLES;
}

// undef to test specialization for dense feature space
//#define DENSE
//#define DENSE_NUM_FEATURES 500

template <typename T, UpdateType UT>
class DiffractedCollection {
  galois::substrate::PerThreadStorage<T*> thread;
  galois::substrate::PerSocketStorage<T*> socket;
  galois::LargeArray<T> old;
  size_t size;
  unsigned num_threads;
  unsigned num_sockets;

  template <typename GetFn>
  void doMerge(const GetFn& getFn) {
    bool byThread =
        UT == UpdateType::ReplicateByThread || UT == UpdateType::Staleness;
    double* local = byThread ? *thread.getLocal() : *socket.getLocal();
    galois::do_all(boost::counting_iterator<unsigned>(0),
                   boost::counting_iterator<unsigned>(size), [&](unsigned i) {
                     unsigned n = byThread ? num_threads : num_sockets;
                     for (unsigned j = 1; j < n; j++) {
                       double o = byThread ? (*thread.getRemote(j))[i]
                                           : (*socket.getRemoteByPkg(j))[i];
                       local[i] += o;
                     }
                     local[i] /= n;
                     auto& v = getFn(i);
                     if (UT == UpdateType::Staleness)
                       v = (old[i] = local[i]);
                     else
                       v = local[i];
                   });
    galois::on_each([&](unsigned tid, unsigned total) {
      switch (UT) {
      case UpdateType::Staleness:
      case UpdateType::ReplicateByThread:
        if (tid)
          std::copy(local, local + size, *thread.getLocal());
        break;
      case UpdateType::ReplicateBySocket:
        if (tid && galois::substrate::getThreadPool().isLeader(tid))
          std::copy(local, local + size, *socket.getLocal());
        break;
      default:
        abort();
      }
    });
  }

public:
  DiffractedCollection(size_t n) : size(n) {
    num_threads = galois::getActiveThreads();
    num_sockets = galois::substrate::getThreadPool().getCumulativeMaxSocket(
                      num_threads - 1) +
                  1;

    if (UT == UpdateType::Staleness)
      old.create(n);
    switch (UT) {
    case UpdateType::ReplicateByThread:
    case UpdateType::Staleness:
      galois::on_each([n, this](unsigned tid, unsigned total) {
        T* p               = new T[n];
        *thread.getLocal() = p;
        std::fill(p, p + n, 0);
      });
    case UpdateType::ReplicateBySocket:
      galois::on_each([n, this](unsigned tid, unsigned total) {
        if (galois::substrate::getThreadPool().isLeader(tid)) {
          T* p               = new T[n];
          *socket.getLocal() = p;
          std::fill(p, p + n, 0);
        }
      });
    case UpdateType::Wild:
    case UpdateType::WildOrig:
      break;
    default:
      abort();
    }
  }

  struct Accessor {
    T* rptr;
    T* wptr;
    T* bptr;

    Accessor(T* p) : rptr(p), wptr(p), bptr(nullptr) {}
    Accessor(T* p1, T* p2) : rptr(p1), wptr(p2), bptr(nullptr) {}
    Accessor(T* p1, T* p2, T* p3) : rptr(p1), wptr(p2), bptr(p3) {}

    T& read(T& addr, ptrdiff_t x) {
      switch (UT) {
      case UpdateType::WildOrig:
      case UpdateType::Wild:
        return addr;
      default:
        return rptr[x];
      }
    }
    T& write(T& addr, ptrdiff_t x) {
      switch (UT) {
      case UpdateType::WildOrig:
      case UpdateType::Wild:
        return addr;
      default:
        return wptr[x];
      }
    }
    void writeBig(T& addr, const T& value) {
      if (!bptr)
        return;
      assert(rptr == wptr);
      bptr[std::distance(wptr, &addr)] = value;
    }
  };

  Accessor get() {
    if (num_sockets > 1 && UT == UpdateType::ReplicateBySocket) {
      unsigned tid       = galois::substrate::ThreadPool::getTID();
      unsigned my_socket = galois::substrate::ThreadPool::getSocket();
      unsigned next      = (my_socket + 1) % num_sockets;
      return Accessor{*socket.getLocal(), *socket.getLocal(),
                      *socket.getRemoteByPkg(next)};
    }

    switch (UT) {
    case UpdateType::Wild:
    case UpdateType::WildOrig:
    case UpdateType::Staleness:
      return Accessor{&old[0], *thread.getLocal()};
    case UpdateType::ReplicateBySocket:
      return Accessor{*socket.getLocal()};
    case UpdateType::ReplicateByThread:
      return Accessor{*thread.getLocal()};
    default:
      abort();
    }
  }

  template <typename GetFn>
  void merge(const GetFn& getFn) {
    switch (UT) {
    case UpdateType::Wild:
    case UpdateType::WildOrig:
      return;
    case UpdateType::Staleness:
    case UpdateType::ReplicateBySocket:
    case UpdateType::ReplicateByThread:
      return doMerge(getFn);
    default:
      abort();
    }
  }
};

template <UpdateType UT>
struct LinearSVM {
  typedef int tt_needs_per_iter_alloc;
  typedef int tt_does_not_need_aborts;

  Graph& g;
  DiffractedCollection<double, UT>& dstate;
  galois::GAccumulator<size_t>& bigUpdates;
  double learningRate;

#ifdef DENSE
  Node* baseNodeData;
  ptrdiff_t edgeOffset;
  double* baseEdgeData;
#endif

  LinearSVM(Graph& _g, DiffractedCollection<double, UT>& d, double _lr,
            galois::GAccumulator<size_t>& b)
      : g(_g), dstate(d), bigUpdates(b), learningRate(_lr) {
#ifdef DENSE
    baseNodeData = &g.getData(g.getEdgeDst(g.edge_begin(0)));
    edgeOffset   = std::distance(&g.getData(NUM_SAMPLES), baseNodeData);
    baseEdgeData = &g.getEdgeData(g.edge_begin(0));
#endif
  }

  void operator()(GNode n, galois::UserContext<GNode>& ctx) {
    galois::PerIterAllocTy& alloc = ctx.getPerIterAlloc();

    // Store edge data in iteration-local temporary to reduce cache misses
#ifdef DENSE
    const ptrdiff_t size = DENSE_NUM_FEATURES;
#else
    ptrdiff_t size =
        std::distance(g.edge_begin(n, galois::MethodFlag::UNPROTECTED),
                      g.edge_end(n, galois::MethodFlag::UNPROTECTED));
#endif
    // regularized factors
    double* rfactors = (double*)alloc.allocate(sizeof(double) * size);
    // document weights
    double* dweights = (double*)alloc.allocate(sizeof(double) * size);
    // model weights
    double* mweights = (double*)alloc.allocate(sizeof(double) * size);
    // write destinations
    double** wptrs = (double**)alloc.allocate(sizeof(double*) * size);

    // Gather
    size_t cur = 0;
    double dot = 0.0;
    auto d     = dstate.get();
#ifdef DENSE
    double* myEdgeData = &baseEdgeData[size * n];
    for (cur = 0; cur < size;) {
      int varCount = baseNodeData[cur].field;
#else
    for (auto edge_it : g.out_edges(n)) {
      GNode variable_node = g.getEdgeDst(edge_it);
      Node& var_data      = g.getData(variable_node);
      int varCount        = var_data.field;
#endif

      double weight;
#ifdef DENSE
      weight = d.read(baseNodeData[cur].w, cur + edgeOffset);
#else
      wptrs[cur]    = &d.write(var_data.w, variableNodeToId(variable_node));
      weight        = d.read(var_data.w, variableNodeToId(variable_node));
#endif
      mweights[cur] = weight;
#ifdef DENSE
      dweights[cur] = myEdgeData[cur];
#else
      dweights[cur] = g.getEdgeData(edge_it);
#endif
      if (UT == UpdateType::WildOrig) {
        rfactors[cur] = (creg * varCount);
      } else {
        rfactors[cur] = mweights[cur] / (creg * varCount);
      }
      dot += mweights[cur] * dweights[cur];
      cur += 1;
    }

    Node& sample_data = g.getData(n);
    int label         = sample_data.field;

    bool bigUpdate = label * dot < 1;
    if (bigUpdate)
      bigUpdates += size;
    for (cur = 0; cur < size; ++cur) {
      double delta;
      if (UT == UpdateType::WildOrig) {
        if (bigUpdate)
          delta = learningRate *
                  (*wptrs[cur] / rfactors[cur] - label * dweights[cur]);
        else
          delta = *wptrs[cur] / rfactors[cur];
      } else {
        if (bigUpdate)
          delta = learningRate * (rfactors[cur] - label * dweights[cur]);
        else
          delta = rfactors[cur];
      }
#ifdef DENSE
      d.write(baseNodeData[cur].w, cur + edgeOffset) = mweights[cur] - delta;
#else
      if (UT == UpdateType::WildOrig) {
        double v    = *wptrs[cur] - delta;
        *wptrs[cur] = v;
        if (bigUpdate)
          d.writeBig(*wptrs[cur], v);
      } else {
        double v    = mweights[cur] - delta;
        *wptrs[cur] = v;
        if (bigUpdate)
          d.writeBig(*wptrs[cur], v);
      }
#endif
    }
  }
};

void printParameters(const std::vector<GNode>& trainingSamples,
                     const std::vector<GNode>& testingSamples) {
  std::cout << "Input graph file: " << inputGraphFilename << "\n";
  std::cout << "Input label file: " << inputLabelFilename << "\n";
  std::cout << "Threads: " << galois::getActiveThreads() << "\n";
  std::cout << "Samples: " << NUM_SAMPLES << "\n";
  std::cout << "Variables: " << NUM_VARIABLES << "\n";
  std::cout << "Training samples: " << trainingSamples.size() << "\n";
  std::cout << "Testing samples: " << testingSamples.size() << "\n";
  std::cout << "Algo type: ";
  switch (algoType) {
  case AlgoType::PrimalStochasticGradientDescent:
    std::cout << "primal stochastic gradient descent";
    break;
  case AlgoType::DualCoordinateDescentL1Loss:
    std::cout << "dual coordinate descent L1 Loss";
    break;
  case AlgoType::DualCoordinateDescentL2Loss:
    std::cout << "dual coordinate descent L2 Loss";
    break;
  case AlgoType::LeastSquares:
    std::cout << "least squares";
    break;
  default:
    abort();
  }
  std::cout << "\n";

  std::cout << "Update type: ";
  switch (updateType) {
  case UpdateType::Wild:
    std::cout << "wild";
    break;
  case UpdateType::WildOrig:
    std::cout << "wild orig";
    break;
  case UpdateType::ReplicateByThread:
    std::cout << "replicate by thread";
    break;
  case UpdateType::ReplicateBySocket:
    std::cout << "replicate by socket";
    break;
  case UpdateType::Staleness:
    std::cout << "stale reads";
    break;
  default:
    abort();
  }
  std::cout << "\n";
}

void initializeVariableCounts(Graph& g) {
  for (auto n : g) {
    for (auto edge_it : g.out_edges(n)) {
      GNode variable_node = g.getEdgeDst(edge_it);
      Node& data          = g.getData(variable_node);
      data.field++; // increase count of variable occurrences
    }
  }
}

unsigned loadLabels(Graph& g, std::string filename) {
  std::ifstream infile(filename);

  unsigned sample_id;
  int label;
  int num_labels = 0;
  while (infile >> sample_id >> label) {
    g.getData(sample_id).field = label;
    ++num_labels;
  }

  return num_labels;
}

size_t getNumCorrect(Graph& g, std::vector<GNode>& testing_samples) {
  galois::GAccumulator<size_t> correct;

  galois::do_all(testing_samples.begin(), testing_samples.end(), [&](GNode n) {
    double sum = 0.0;
    Node& data = g.getData(n);
    int label  = data.field;
    for (auto edge_it : g.out_edges(n)) {
      GNode variable_node = g.getEdgeDst(edge_it);
      Node& data          = g.getData(variable_node);
      double weight       = g.getEdgeData(edge_it);
      sum += data.w * weight;
    }

    if (sum <= 0.0 && label == -1) {
      correct += 1;
    } else if (sum > 0.0 && label == 1) {
      correct += 1;
    }
  });

  return correct.reduce();
}

double getPrimalObjective(Graph& g, const std::vector<GNode>& trainingSamples) {
  // 0.5 * w^Tw + C * sum_i [max(0, 1 - y_i * w^T * x_i)]^2
  galois::GAccumulator<double> objective;

  galois::do_all(trainingSamples.begin(), trainingSamples.end(), [&](GNode n) {
    double sum = 0.0;
    Node& data = g.getData(n);
    int label  = data.field;
    for (auto edge_it : g.out_edges(n)) {
      GNode variable_node = g.getEdgeDst(edge_it);
      Node& data          = g.getData(variable_node);
      double weight       = g.getEdgeData(edge_it);
      sum += data.w * weight;
    }

    double o = std::max(0.0, 1 - label * sum);
    objective += o * o;
  });

  galois::GAccumulator<double> norm;
  galois::do_all(boost::counting_iterator<size_t>(0),
                 boost::counting_iterator<size_t>(NUM_VARIABLES),
                 [&](size_t i) {
                   double v = g.getData(i + NUM_SAMPLES).w;
                   norm += v * v;
                 });
  return objective.reduce() * creg + 0.5 * norm.reduce();
}

template <UpdateType UT>
void runPrimalSgd(Graph& g, std::mt19937& gen,
                  std::vector<GNode>& trainingSamples,
                  std::vector<GNode>& testingSamples) {
  galois::TimeAccumulator accumTimer;
  accumTimer.start();

  DiffractedCollection<double, UT> dstate(NUM_VARIABLES);

  galois::StatTimer sgdTime("SgdTime");

  unsigned iterations = maxIterations;
  double minObj       = std::numeric_limits<double>::max();
  if (fixedIterations)
    iterations = fixedIterations;

  for (unsigned iter = 1; iter <= iterations; ++iter) {
    sgdTime.start();

    // include shuffling time in the time taken per iteration
    // also: not parallel
    if (shuffleSamples)
      std::shuffle(trainingSamples.begin(), trainingSamples.end(), gen);

    double learning_rate = 30 / (100.0 + iter);
    auto ts_begin        = trainingSamples.begin();
    auto ts_end          = trainingSamples.end();
    auto ln              = galois::loopname("LinearSVM");
    auto wl = galois::wl<galois::worklists::PerSocketChunkFIFO<32>>();
    galois::GAccumulator<size_t> bigUpdates;

    galois::Timer flopTimer;
    flopTimer.start();

    galois::for_each(ts_begin, ts_end,
                     LinearSVM<UT>(g, dstate, learning_rate, bigUpdates), ln,
                     wl);

    flopTimer.stop();
    sgdTime.stop();

    size_t numBigUpdates = bigUpdates.reduce();
    double flop   = 4 * g.sizeEdges() + 2 + 3 * numBigUpdates + g.sizeEdges();
    size_t millis = flopTimer.get();
    double gflops = 0;
    if (millis)
      gflops = flop / millis / 1e6;

    dstate.merge(
        [&g](ptrdiff_t x) -> double& { return g.getData(x + NUM_SAMPLES).w; });

    accumTimer.stop();
    std::cout << iter << " GFLOP/s " << gflops << " "
              << "(" << millis / 1e3 << " s)"
              << " AccumTime " << accumTimer.get() / 1e3;
    accumTimer.start();
    if (printAccuracy) {
      std::cout << " Accuracy: "
                << getNumCorrect(g, testingSamples) /
                       (double)testingSamples.size();
    }
    double obj = 0;
    if (!fixedIterations) {
      obj = getPrimalObjective(g, trainingSamples);
    }
    if (printObjective) {
      std::cout << " Obj: " << obj;
    }
    std::cout << "\n";
    if (!fixedIterations) {
      if (std::fabs((obj - minObj) / minObj) < tol) {
        std::cout << "Converged in " << iter << " iterations.\n";
        return;
      }
      minObj = std::min(obj, minObj);
    }
  }

  if (!fixedIterations)
    std::cout << "Failed to converge\n";
}

double getDualObjective(Graph& g, const std::vector<GNode>& trainingSamples,
                        double* diag, const std::vector<double>& alpha) {
  // 0.5 * w^Tw + C * sum_i [max(0, 1 - y_i * w^T * x_i)]^2
  galois::GAccumulator<double> objective;

  galois::do_all(trainingSamples.begin(), trainingSamples.end(), [&](GNode n) {
    // Node& data = g.getData(n);
    int label = g.getData(n).field;
    objective += alpha[n] * (alpha[n] * diag[label + 1] - 2);
  });

  galois::do_all(boost::counting_iterator<size_t>(0),
                 boost::counting_iterator<size_t>(NUM_VARIABLES),
                 [&](size_t i) {
                   double v = g.getData(i + NUM_SAMPLES).w;
                   objective += v * v;
                 });
  return objective.reduce();
}

// TODO(ddn): Parallelize
// See Algorithm 3 of Hsieh et al., ICML 2008
void runDualCoordinateDescent(Graph& g, std::mt19937& gen,
                              std::vector<GNode>& trainingSamples,
                              std::vector<GNode>& testingSamples,
                              bool useL1Loss) {
  galois::TimeAccumulator accumTimer;
  accumTimer.start();

  std::vector<double> alpha(NUM_SAMPLES);
  std::vector<double> QD(NUM_SAMPLES);

  double diag[] = {0.5 / creg, 0, 0.5 / creg};
  double ub[]   = {std::numeric_limits<double>::max(), 0,
                 std::numeric_limits<double>::max()};
  if (useL1Loss) {
    diag[0] = 0;
    diag[2] = 0;
    ub[0]   = creg;
    ub[2]   = creg;
  }

  for (auto ii = g.begin(), ei = g.begin() + NUM_SAMPLES; ii != ei; ++ii) {
    int& label = g.getData(*ii).field;
    if (label != 1 && label != -1) {
      label = label <= 0 ? -1 : 1;
    }

    QD[*ii] = diag[label + 1];
    for (auto edge : g.out_edges(*ii)) {
      double val = g.getEdgeData(edge);
      QD[*ii] += val * val;
    }
  }

  galois::StatTimer cdTime("CDTime");
  double maxPG              = std::numeric_limits<double>::max();
  double minPG              = std::numeric_limits<double>::lowest();
  std::vector<GNode> active = trainingSamples;
  std::vector<GNode> activeNew;
  unsigned iter;
  for (iter = 1; iter <= maxIterations; ++iter) {
    cdTime.start();

    double maxPGNew = std::numeric_limits<double>::lowest();
    double minPGNew = std::numeric_limits<double>::max();
    if (iter != 1 && shuffleSamples) {
      std::shuffle(active.begin(), active.end(), gen);
    }

    galois::Timer flopTimer;
    flopTimer.start();
    size_t flop = 0;
    for (GNode n : active) {
      double G  = 0;
      int label = g.getData(n).field;
      flop += 2 * std::distance(g.edge_begin(n), g.edge_end(n));
      for (auto edge : g.out_edges(n)) {
        G += g.getData(g.getEdgeDst(edge)).w * g.getEdgeData(edge);
      }
      G = G * label - 1;
      G += alpha[n] * diag[label + 1];
      flop += 3;

      double C  = ub[label + 1];
      double PG = 0;
      if (alpha[n] == 0) {
        if (G > maxPG) {
          continue;
        } else if (G < 0) {
          PG = G;
        }
      } else if (alpha[n] == C) {
        if (G < minPG) {
          continue;
        } else if (G > 0) {
          PG = G;
        }
      } else {
        PG = G;
      }
      maxPGNew = std::max(maxPGNew, PG);
      minPGNew = std::min(minPGNew, PG);

      if (std::fabs(PG) > 1.0e-12) {
        double a = alpha[n];
        alpha[n] = std::min(std::max(alpha[n] - G / QD[n], 0.0), C);
        double d = (alpha[n] - a) * label;
        flop += 3;
        flop += 2 * std::distance(g.edge_begin(n), g.edge_end(n));
        for (auto edge : g.out_edges(n)) {
          double& w = g.getData(g.getEdgeDst(edge)).w;
          w += d * g.getEdgeData(edge);
        }
      }
      activeNew.push_back(n);
    }
    flopTimer.stop();
    cdTime.stop();

    accumTimer.stop();
    size_t millis = flopTimer.get();
    double gflops = 0;
    if (millis)
      gflops = flop / millis / 1e6;
    std::cout << iter << " GFLOP/s " << gflops << " "
              << "(" << millis / 1e3 << " s)"
              << " AccumTime " << accumTimer.get() / 1e3 << " ActiveSet "
              << active.size();
    accumTimer.start();
    if (printAccuracy) {
      double accuracy =
          getNumCorrect(g, testingSamples) / (double)testingSamples.size();
      std::cout << " Accuracy: " << accuracy;
    }
    if (printObjective) {
      std::cout << " Obj: "
                << getDualObjective(g, trainingSamples, diag, alpha);
    }
    std::cout << "\n";

    active.clear();
    std::swap(active, activeNew);

    if (maxPGNew - minPGNew <= tol) {
      if (active.size() == trainingSamples.size()) {
        std::cout << "Converged in " << iter << " iterations\n";
        return;
      } else {
        active = trainingSamples;
        maxPG  = std::numeric_limits<double>::max();
        minPG  = std::numeric_limits<double>::lowest();
      }
    } else {
      maxPG = maxPGNew;
      minPG = minPGNew;
      if (maxPG <= 0)
        maxPG = std::numeric_limits<double>::max();
      if (minPG >= 0)
        minPG = std::numeric_limits<double>::lowest();
    }
  }

  std::cout << "Failed to converge\n";
}

#ifdef HAS_EIGEN
void runLeastSquares(Graph& g, std::mt19937& gen,
                     std::vector<GNode>& trainingSamples,
                     std::vector<GNode>& testingSamples) {
  Eigen::SparseMatrix<double> A(NUM_SAMPLES, NUM_VARIABLES);
  {
    typedef Eigen::Triplet<double> Triplet;
    std::vector<Triplet> triplets{g.sizeEdges()};
    {
      auto it = triplets.begin();
      for (auto n : g) {
        for (auto edge : g.out_edges(n)) {
          *it++ =
              Triplet(n, g.getEdgeDst(edge) - NUM_SAMPLES, g.getEdgeData(edge));
        }
      }
    }
    A.setFromTriplets(triplets.begin(), triplets.end());
  }

  Eigen::VectorXd b(NUM_SAMPLES);
  size_t cur = 0;
  for (auto ii = g.begin(), ei = g.begin() + NUM_SAMPLES; ii != ei; ++ii) {
    b(cur++) = g.getData(*ii).field;
  }

  // Least-squares problem: minimize ||Ax - b||_2
  // normal equation: A^T A x = A^T b
  Eigen::VectorXd x;
  // Cholesky is almost always faster but QR is more numerically stable
  if (g.sizeEdges() > 10000) {
    // Solve normal equation directly with Cholesky
    Eigen::SparseMatrix<double> AT  = A.transpose();
    Eigen::SparseMatrix<double> ATA = AT * A;
    for (int i = 0; i < ATA.rows(); ++i)
      ATA.coeffRef(i, i) += creg;
    Eigen::VectorXd ATb = AT * b;
    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
    solver.compute(ATA);
    x = solver.solve(ATb);
    std::cout << "cg iterations: " << solver.iterations() << "\n";
    std::cout << "cg est error: " << solver.error() << "\n";
  } else {
    // TODO add L2 regularizer
    // Decompose normal equation
    // A = QR
    // A^T A x = A^T b
    // R^T Q^T Q R x = R^T Q^T b => ... => R x = Q^T b
    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
        solver;
    solver.compute(A);
    if (solver.info() != Eigen::Success) {
      GALOIS_DIE("factorization failed");
    }
    Eigen::VectorXd QTb = solver.matrixQ().transpose() * b;
    int r               = solver.rank();
    Eigen::VectorXd out;
    out.resize(A.cols());
    out.topRows(r) = solver.matrixR()
                         .topLeftCorner(r, r)
                         .triangularView<Eigen::Upper>()
                         .solve(QTb.topRows(r));
    out.bottomRows(out.rows() - r).setZero();
    x = solver.colsPermutation() * out;
  }

  // Verify
  {
    for (size_t i = 0; i < NUM_VARIABLES; ++i) {
      g.getData(i + NUM_SAMPLES).w = x(i);
    }
    std::vector<GNode> allSamples(g.begin(), g.begin() + NUM_SAMPLES);
    size_t n = getNumCorrect(g, allSamples);
    std::cout << "All: " << n / (double)NUM_SAMPLES << " (" << n << "/"
              << NUM_SAMPLES << ")\n";
    n = getNumCorrect(g, testingSamples);
    std::cout << "Testing: " << n / (double)testingSamples.size() << " (" << n
              << "/" << testingSamples.size() << ")\n";
  }
}
#endif

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

  Graph g;
  galois::graphs::readGraph(g, inputGraphFilename);
  NUM_SAMPLES = loadLabels(g, inputLabelFilename);
  initializeVariableCounts(g);
  NUM_VARIABLES = g.size() - NUM_SAMPLES;
  assert(NUM_SAMPLES > 0 && NUM_VARIABLES > 0);

  // put samples in a list and shuffle them
  std::random_device rd;
  std::mt19937 gen(SEED == ~0U ? rd() : SEED);

  std::vector<GNode> allSamples(g.begin(), g.begin() + NUM_SAMPLES);
  std::shuffle(allSamples.begin(), allSamples.end(), gen);

  // copy a fraction of the samples to the training samples list
  unsigned numTraining = numberTraining;
  if (numTraining == 0 || numTraining >= NUM_SAMPLES)
    numTraining = std::min(
        static_cast<unsigned>(NUM_SAMPLES * fractionTraining), NUM_SAMPLES);
  std::vector<GNode> trainingSamples(allSamples.begin(),
                                     allSamples.begin() + numTraining);
  // the remainder of samples go into the testing samples list
  std::vector<GNode> testingSamples(allSamples.begin() + numTraining,
                                    allSamples.end());

  printParameters(trainingSamples, testingSamples);
  if (printAccuracy) {
    std::cout << "Initial";
    if (printAccuracy) {
      std::cout << " Accuracy: "
                << getNumCorrect(g, testingSamples) /
                       (double)testingSamples.size();
    }
    std::cout << "\n";
  }

  galois::StatTimer timer;
  timer.start();
  switch (algoType) {
  case AlgoType::PrimalStochasticGradientDescent:
    switch (updateType) {
    case UpdateType::Wild:
      runPrimalSgd<UpdateType::Wild>(g, gen, trainingSamples, testingSamples);
      break;
    case UpdateType::WildOrig:
      runPrimalSgd<UpdateType::WildOrig>(g, gen, trainingSamples,
                                         testingSamples);
      break;
    case UpdateType::ReplicateBySocket:
      runPrimalSgd<UpdateType::ReplicateBySocket>(g, gen, trainingSamples,
                                                  testingSamples);
      break;
    case UpdateType::ReplicateByThread:
      runPrimalSgd<UpdateType::ReplicateByThread>(g, gen, trainingSamples,
                                                  testingSamples);
      break;
    case UpdateType::Staleness:
      runPrimalSgd<UpdateType::Staleness>(g, gen, trainingSamples,
                                          testingSamples);
      break;
    default:
      abort();
    }
    break;
  case AlgoType::DualCoordinateDescentL1Loss:
    runDualCoordinateDescent(g, gen, trainingSamples, testingSamples, true);
    break;
  case AlgoType::DualCoordinateDescentL2Loss:
    runDualCoordinateDescent(g, gen, trainingSamples, testingSamples, false);
    break;
#ifdef HAS_EIGEN
  case AlgoType::LeastSquares:
    runLeastSquares(g, gen, trainingSamples, testingSamples);
    break;
#endif
  default:
    abort();
  }
  timer.stop();

  return 0;
}


/**********************************************************************
*  Copyright (c) 2008-2015, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library 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
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_DATA_VECTOR_HPP
#define UTILITIES_DATA_VECTOR_HPP

#include "../UtilitiesAPI.hpp"

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>

namespace openstudio {

/// Workaround to get Vector typedef, http://www.gotw.ca/gotw/079.htm
struct UTILITIES_API VectorStruct
{
  typedef boost::numeric::ublas::vector<double> VectorType;
  typedef boost::numeric::ublas::scalar_vector<double> ScalarVectorType;
};

/// Vector
typedef VectorStruct::VectorType Vector;

/// ScalarVector
typedef VectorStruct::ScalarVectorType ScalarVector;

/// Helper function to construct Vector from std::vector<double>.
UTILITIES_API Vector createVector(const std::vector<double>& values);

/// Helper function to construct Vector from std::vector<long>
UTILITIES_API Vector createVector(const std::vector<long>& values);

UTILITIES_API std::vector<double> toStandardVector(const Vector& values);

//////////////////////////////////////////////////////////////////////////
// Begin SWIG'able, copy and paste into Vector.i
//////////////////////////////////////////////////////////////////////////

/** @name Operators */
//@{

UTILITIES_API bool operator==(const Vector& lhs, const Vector& rhs);
UTILITIES_API bool operator!=(const Vector& lhs, const Vector& rhs);

//@}
/** @name Interpolation */
//@{

// The following link has hints for interpolation
// http://o2scl.sourceforge.net/o2scl/html/index.html#intp_section

/** Enum to specify the interpolation method. */
enum InterpMethod{LinearInterp, NearestInterp, HoldLastInterp, HoldNextInterp};

/** Enum to specify the extrapolation method. */
enum ExtrapMethod{NoneExtrap, NearestExtrap};

/** Data structure for holding interpolation information. */
struct UTILITIES_API InterpInfo{
  bool extrapolated; // was point out of range
  unsigned ia, ib; // indices of two nearest points
  double wa, wb; // weights of two nearest points
};

/** Linear interpolation of the function y = f(x) at point xi. Assumes that x is strictly
 *  increasing. */
UTILITIES_API InterpInfo interpInfo(const Vector& x, double xi);

/** Linear interpolation of the function y = f(x) at point xi. Assumes that x is strictly
 *  increasing */
UTILITIES_API double interp(const Vector& x, const Vector& y, double xi,
                            InterpMethod interpMethod = LinearInterp,
                            ExtrapMethod extrapMethod = NoneExtrap);

/** Linear interpolation of the function y = f(x) at points xi. Assumes that x is strictly
 *  increasing. */
UTILITIES_API Vector interp(const Vector& x, const Vector& y, const Vector& xi,
                            InterpMethod interpMethod = LinearInterp,
                            ExtrapMethod extrapMethod = NoneExtrap);

//@}
/** @name Common Methods and Vector Operations */
//@{

/** Generates a Vector of N points randomly drawn between and including a and b. */
UTILITIES_API Vector randVector(double a, double b, unsigned N);

/** Generates a Vector of N points linearly spaced between and including a and b. */
UTILITIES_API Vector linspace(double a, double b, unsigned N);

/** Generates a Vector linearly spaced points starting at a and ending before or at b with
 *  interval delta. */
UTILITIES_API Vector deltaSpace(double a, double b, double delta);

/** Generates a Vector of N points logarithmically spaced between and including base^a and
 *  base^b. */
UTILITIES_API Vector logspace(double a, double b, unsigned N, double base = 10.0);

/** Take the natural logarithm of elements of a Vector. */
UTILITIES_API Vector log(const Vector& x);

/** Take the logarithm of elements of a Vector with certain base. */
UTILITIES_API Vector log(const Vector& x, double base);

/** Compute the cumulative sum of a Vector. */
UTILITIES_API Vector cumsum(const Vector& x, double runningSum = 0.0);

/** Returns the dot product between lhs and rhs. */
UTILITIES_API double dot(const Vector& lhs, const Vector& rhs);

/** Returns the sum of vector's values. */
UTILITIES_API double sum(const Vector& vector);

/** Returns the largest element of vector. */
UTILITIES_API double maximum(const Vector& vector);

/** Returns the smallest element of vector. */
UTILITIES_API double minimum(const Vector& vector);

/** Returns the mean of vector's values */
UTILITIES_API double mean(const Vector& vector);

/** Returns the sample variance of vector's values. */
UTILITIES_API double variance(const Vector& vector);

/** Returns the standard deviation of vector's values. */
UTILITIES_API double stdDev(const Vector& vector);

/** Returns std::function pointer to sum(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> sumVectorFunctor();

/** Returns std::function pointer to maximum(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> maximumVectorFunctor();

/** Returns std::function pointer to minimum(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> minimumVectorFunctor();

/** Returns std::function pointer to mean(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> meanVectorFunctor();

/** Returns std::function pointer to variance(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> varianceVectorFunctor();

/** Returns std::function pointer to stdDev(const Vector&). */
UTILITIES_API std::function<double (const Vector&)> stdDevVectorFunctor();

/** Evaluates functor(vector). For use in SWIG bindings. */
UTILITIES_API double evaluateDoubleFromVectorFunctor(
    const std::function<double (const Vector&)>& functor,
    const Vector& vector);

//@}

} // openstudio

#endif //UTILITIES_DATA_VECTOR_HPP


//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_EDGE_CONNECTIVITY
#define BOOST_EDGE_CONNECTIVITY

// WARNING: not-yet fully tested!

#include <boost/config.hpp>
#include <vector>
#include <set>
#include <algorithm>
#include <boost/graph/edmonds_karp_max_flow.hpp>

namespace boost {

  namespace detail {

    template <class Graph>
    inline
    std::pair<typename graph_traits<Graph>::vertex_descriptor,
              typename graph_traits<Graph>::degree_size_type>
    min_degree_vertex(Graph& g)
    {
      typedef graph_traits<Graph> Traits;
      typename Traits::vertex_descriptor p;
      typedef typename Traits::degree_size_type size_type;
      size_type delta = (std::numeric_limits<size_type>::max)();

      typename Traits::vertex_iterator i, iend;
      for (tie(i, iend) = vertices(g); i != iend; ++i)
        if (degree(*i, g) < delta) {
          delta = degree(*i, g);
          p = *i;
        }
      return std::make_pair(p, delta);
    }

    template <class Graph, class OutputIterator>
    void neighbors(const Graph& g,
                   typename graph_traits<Graph>::vertex_descriptor u,
                   OutputIterator result)
    {
      typename graph_traits<Graph>::adjacency_iterator ai, aend;
      for (tie(ai, aend) = adjacent_vertices(u, g); ai != aend; ++ai)
        *result++ = *ai;
    }

    template <class Graph, class VertexIterator, class OutputIterator>
    void neighbors(const Graph& g,
                   VertexIterator first, VertexIterator last,
                   OutputIterator result)
    {
      for (; first != last; ++first)
        neighbors(g, *first, result);
    }

  } // namespace detail

  // O(m n)
  template <class VertexListGraph, class OutputIterator>
  typename graph_traits<VertexListGraph>::degree_size_type
  edge_connectivity(VertexListGraph& g, OutputIterator disconnecting_set)
  {
    //-------------------------------------------------------------------------
    // Type Definitions
    typedef graph_traits<VertexListGraph> Traits;
    typedef typename Traits::vertex_iterator vertex_iterator;
    typedef typename Traits::edge_iterator edge_iterator;
    typedef typename Traits::out_edge_iterator out_edge_iterator;
    typedef typename Traits::vertex_descriptor vertex_descriptor;
    typedef typename Traits::degree_size_type degree_size_type;
    typedef color_traits<default_color_type> Color;

    typedef adjacency_list_traits<vecS, vecS, directedS> Tr;
    typedef typename Tr::edge_descriptor Tr_edge_desc;
    typedef adjacency_list<vecS, vecS, directedS, no_property,
      property<edge_capacity_t, degree_size_type,
        property<edge_residual_capacity_t, degree_size_type,
          property<edge_reverse_t, Tr_edge_desc> > > >
      FlowGraph;
    typedef typename graph_traits<FlowGraph>::edge_descriptor edge_descriptor;

    //-------------------------------------------------------------------------
    // Variable Declarations
    vertex_descriptor u, v, p, k;
    edge_descriptor e1, e2;
    bool inserted;
    vertex_iterator vi, vi_end;
    edge_iterator ei, ei_end;
    degree_size_type delta, alpha_star, alpha_S_k;
    std::set<vertex_descriptor> S, neighbor_S;
    std::vector<vertex_descriptor> S_star, non_neighbor_S;
    std::vector<default_color_type> color(num_vertices(g));
    std::vector<edge_descriptor> pred(num_vertices(g));

    //-------------------------------------------------------------------------
    // Create a network flow graph out of the undirected graph
    FlowGraph flow_g(num_vertices(g));

    typename property_map<FlowGraph, edge_capacity_t>::type
      cap = get(edge_capacity, flow_g);
    typename property_map<FlowGraph, edge_residual_capacity_t>::type
      res_cap = get(edge_residual_capacity, flow_g);
    typename property_map<FlowGraph, edge_reverse_t>::type
      rev_edge = get(edge_reverse, flow_g);

    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      u = source(*ei, g), v = target(*ei, g);
      tie(e1, inserted) = add_edge(u, v, flow_g);
      cap[e1] = 1;
      tie(e2, inserted) = add_edge(v, u, flow_g);
      cap[e2] = 1; // not sure about this
      rev_edge[e1] = e2;
      rev_edge[e2] = e1;
    }

    //-------------------------------------------------------------------------
    // The Algorithm

    tie(p, delta) = detail::min_degree_vertex(g);
    S_star.push_back(p);
    alpha_star = delta;
    S.insert(p);
    neighbor_S.insert(p);
    detail::neighbors(g, S.begin(), S.end(),
                      std::inserter(neighbor_S, neighbor_S.begin()));

    std::set_difference(vertices(g).first, vertices(g).second,
                        neighbor_S.begin(), neighbor_S.end(),
                        std::back_inserter(non_neighbor_S));

    while (!non_neighbor_S.empty()) { // at most n - 1 times
      k = non_neighbor_S.front();

      alpha_S_k = edmonds_karp_max_flow
        (flow_g, p, k, cap, res_cap, rev_edge, &color[0], &pred[0]);

      if (alpha_S_k < alpha_star) {
        alpha_star = alpha_S_k;
        S_star.clear();
        for (tie(vi, vi_end) = vertices(flow_g); vi != vi_end; ++vi)
          if (color[*vi] != Color::white())
            S_star.push_back(*vi);
      }
      S.insert(k);
      neighbor_S.insert(k);
      detail::neighbors(g, k, std::inserter(neighbor_S, neighbor_S.begin()));
      non_neighbor_S.clear();
      std::set_difference(vertices(g).first, vertices(g).second,
                          neighbor_S.begin(), neighbor_S.end(),
                          std::back_inserter(non_neighbor_S));
    }
    //-------------------------------------------------------------------------
    // Compute edges of the cut [S*, ~S*]
    std::vector<bool> in_S_star(num_vertices(g), false);
    typename std::vector<vertex_descriptor>::iterator si;
    for (si = S_star.begin(); si != S_star.end(); ++si)
      in_S_star[*si] = true;

    degree_size_type c = 0;
    for (si = S_star.begin(); si != S_star.end(); ++si) {
      out_edge_iterator ei, ei_end;
      for (tie(ei, ei_end) = out_edges(*si, g); ei != ei_end; ++ei)
        if (!in_S_star[target(*ei, g)]) {
          *disconnecting_set++ = *ei;
          ++c;
        }
    }
    return c;
  }

} // namespace boost

#endif // BOOST_EDGE_CONNECTIVITY


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <memory>
#include <iostream>
#include <iomanip>
#include <boost/noncopyable.hpp>
#include "Eigen/Core"
#include "Eigen/LU"
#include "metro/regression/Design.hpp"
#include "metro/regression/LogLikelihood.hpp"
#include "metro/regression/NormalWeightedLogLikelihood.hpp"

// #define DEBUG_NORMALWEIGHTEDLOGLIKELIHOOD 1

namespace metro {
	namespace regression {
		namespace impl {
			double compute_mvn_constant( int const k, double determinant ) {
				return -0.5 * k * std::log( 2 * 3.141592654 ) - 0.5 * std::log( determinant ) ;
			}
		}

		NormalWeightedLogLikelihood::UniquePtr NormalWeightedLogLikelihood::create(
			LogLikelihood::UniquePtr ll,
			Vector const mean,
			Matrix const covariance,
			Normalisation normalisation
		) {
			return UniquePtr( new NormalWeightedLogLikelihood( ll, mean, covariance, normalisation ) ) ;
		}

		NormalWeightedLogLikelihood::NormalWeightedLogLikelihood(
			LogLikelihood::UniquePtr ll,
			Vector const mean,
			Matrix const covariance,
			Normalisation normalisation
		):
			m_ll( ll ),
			m_mean( mean ),
			m_covariance( covariance ),
			m_normalisation( normalisation ),
			m_solver( covariance ),
			m_inverse_covariance( m_solver.solve( Matrix::Identity( mean.size(), mean.size() ))),
			m_constant( impl::compute_mvn_constant( m_mean.size(), m_covariance.determinant() ))
		{}

		std::string NormalWeightedLogLikelihood::get_parameter_name( std::size_t i ) const {
			return m_ll->get_parameter_name(i) ;
		}
		NormalWeightedLogLikelihood::IntegerMatrix NormalWeightedLogLikelihood::identify_parameters() const {
			return m_ll->identify_parameters() ;
		}

		int NormalWeightedLogLikelihood::number_of_parameters() const {
			return m_ll->number_of_parameters() ;
		}

		int NormalWeightedLogLikelihood::number_of_outcomes() const {
			return m_ll->number_of_outcomes() ;
		}

		void NormalWeightedLogLikelihood::evaluate_at( Point const& parameters, int const numberOfDerivatives ) {
			m_ll->evaluate_at( parameters, numberOfDerivatives ) ;
			evaluate_impl( numberOfDerivatives ) ;
		}

		void NormalWeightedLogLikelihood::evaluate( int const numberOfDerivatives ) {
			m_ll->evaluate( numberOfDerivatives ) ;
			evaluate_impl( numberOfDerivatives ) ;
		}

		void NormalWeightedLogLikelihood::evaluate_impl( int const numberOfDerivatives ) {
			Vector const& parameters = m_ll->parameters() ;
			Vector solved = m_solver.solve( parameters - m_mean ) ;
			m_log_density = -0.5 * ((parameters - m_mean).transpose() * solved)(0) ;
			m_value_of_first_derivative = -solved ;
			m_value_of_second_derivative = -m_inverse_covariance ;

#if DEBUG_NORMALWEIGHTEDLOGLIKELIHOOD
			std::cerr << "NormalWeightedLogLikelihood::evaluate_impl()\:\n" ;
			std::cerr << "parameters = " << parameters.transpose() << ".\n" ;
			std::cerr << "constant = " << m_constant << ".\n" ;
			std::cerr << "solved = \n" << solved << ".\n" ;
			std::cerr << "log density = " << m_log_density << "\n" ;
#endif
		}

		std::string NormalWeightedLogLikelihood::get_summary() const {
			std::ostringstream ostr ;
			ostr << "normal-weighted:" << m_ll->get_summary() << "\n" ;
			ostr << "normal-weighted: using the following prior covariance:\n" ;
			std::size_t max_label_length = 0 ;
			for( int i = 0; i < m_covariance.rows(); ++i ) {
				max_label_length = std::max( max_label_length, m_ll->get_parameter_name(i).size() ) ;
			}
			ostr
				<< std::setw(3) << "" << "  "
				<< std::setw( max_label_length + 2 ) << "parameter"
				<< ":" ;
			for( int j = 0; j < m_covariance.cols(); ++j ) {
				ostr << " " << std::setw(5) << (j+1) ;
			}
			ostr << "\n" ;
			for( int i = 0; i < m_covariance.rows(); ++i ) {
				ostr << std::setw(3) << i+1
					<< ": " << std::setw( max_label_length + 2 ) << m_ll->get_parameter_name(i) << ":" ;
				for( int j = 0; j < m_covariance.cols(); ++j ) {
					ostr << " " << std::setw(5) << std::setprecision(4) << m_covariance(i,j) ;
				}
				ostr << "\n" ;
			}
			return ostr.str() ;
		}
	}
}