// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <string>
#include <boost/type_traits/is_complex.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>

using namespace std;
using namespace mtl;

std::string program_dir; // Ugly global variable !!!

template <typename Matrix>
void inline test_file(Matrix& A, const char* file_name, const char* comment)
{
    cout << "Filename is " << mtl::io::join(program_dir, file_name) << "\n";
    mtl::io::matrix_market_istream ms(mtl::io::join(program_dir, file_name));
    ms >> A;
    std::cout << "Read from " << file_name << " (" << comment << ") is "
	      << num_rows(A) << "x" << num_cols(A) << "\n";

    if (num_rows(A) > 9 || num_cols(A) > 9) {
	int reordering[]= {0, 1, 2, 3, 4, 5, 6, 7, 8};
	mtl::mat::traits::reorder<>::type  R= mtl::mat::reorder(reordering, num_cols(A)),
	                                      R2= mtl::mat::reorder(reordering, num_rows(A));
	Matrix B0(R * A), B(B0 * trans(R2));
	std::cout << "A[0:9][0:9] is:\n" << B;
    } else
	std::cout << "A is:\n" << A;
}

template <typename Matrix>
void inline test(Matrix& A, const char* name)
{
    std::cout << "\n" << name << "\n";

    typedef typename mtl::Collection<Matrix>::value_type vt;

    test_file(A, "matrix_market/jgl009.mtx", "general pattern");
    if (boost::is_complex<vt>::value)
	test_file(A, "matrix_market/mhd1280b.mtx", "Hermitian");
    // test_file(A, "matrix_market/plskz362.mtx", "Skew-symmetric"); // has only 0s in A[:9][:9]
    test_file(A, "matrix_market/bcsstk01.mtx", "Real symmetric");

    Matrix B(mtl::io::matrix_market(mtl::io::join(program_dir, "matrix_market/jgl009.mtx"))), C;
    std::cout << "Matrix market file read in constructor:\n" << B;

    C= mtl::io::matrix_market(mtl::io::join(program_dir, "matrix_market/jgl009.mtx"));
    std::cout << "Matrix market file assigned:\n" << B;
 }

template <typename Matrix>
void inline failure_test(Matrix& A)
{
    try {
	A= mtl::io::matrix_market("File_not_exist_test.mtx");
    } catch (const mtl::file_not_found& e) {
	std::cerr << "Successfully caught exception for inexistant file. Error message is:\n" << e.what();
	return;
    }
    throw "No exception thrown for inexistant file.";
}

template< typename Matrix >
void read_test(Matrix& A, const char* name )
{
	typedef typename Collection<Matrix>::size_type my_size;
	dense2D< double > data(2,4);
	unsigned v(1);
	for(unsigned r(0); r < 2; ++r)
		for(unsigned c(0); c < 4; ++c,++v)
			data(r,c)=v;
	mtl::io::matrix_market_istream in("matrix_market/dense_read.mtx");
	in >> A;
	dense2D< double > diff(A-data);
	pair< my_size, my_size > pos(max_abs_pos(diff));
	if(diff(pos.first,pos.second) > 1e-14) {
		std::stringstream ss;
		ss << "could not read the dense matrix with type " << name << std::endl;
		ss << "wanted:\t" << data << "\n";
		ss << "got:\t" << A << "\n";
		throw std::runtime_error(ss.str());
	}
}

int main(int, char* argv[])
{
    using namespace mtl;

    compressed2D<double>                             cdc;
    compressed2D<std::complex<double> >              ccc;
    dense2D<double>                                  dc;
    dense2D<double, mat::parameters<col_major> >  dcc;
    dense2D<float>                                   fc;
    morton_dense<double,  morton_mask>               mdc;
    morton_dense<double, doppled_32_col_mask>        mcc;

    program_dir= mtl::io::directory_name(argv[0]);

    test(cdc, "compressed2D");
    test(ccc, "compressed2D complex");
    test(dc, "dense2D");
    test(dcc, "dense2D col-major");
    test(mdc, "pure Morton");
    test(mcc, "Hybrid col-major");

    failure_test(cdc);

    read_test(dc, "dense2D");
    //read_test(ccc, "compressed2D complex");
    read_test(dcc, "dense2D col-major");
    read_test(mdc, "pure Morton");
    read_test(mcc, "Hybrid col-major");

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

//[assign_points
//` Shows usage of Boost.Geometry's assign, Boost.Assign, and Boost.Range to assign ranges of a linestring

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/assign.hpp>
#include <boost/geometry/geometries/adapted/boost_range/filtered.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename T>
struct x_between
{
    x_between(T a, T b)
        : fa(a), fb(b)
    {}

    template <typename P>
    bool operator()(P const& p) const
    {
        return boost::geometry::get<0>(p) >= fa
            && boost::geometry::get<0>(p) <= fb;
    }
private :
    T fa, fb;
};

int main()
{
    using namespace boost::assign;

    typedef boost::geometry::model::linestring<boost::tuple<int, int> > ls;

    ls line1, line2, line3;

    line1 = tuple_list_of(0, 0)(2, 3)(4, 0)(6, 3)(8, 0)(10, 3)(12, 0); /*< tuple_list_of is part of Boost.Assign and can be used for Boost.Geometry if points are tuples >*/
    boost::geometry::assign_points(line2, tuple_list_of(0, 0)(2, 2)(4, 0)(6, 2)(8, 0)); /*< tuple_list_of delivers a range and can therefore be used in boost::geometry::assign >*/
    boost::geometry::assign_points(line3, line1 | boost::adaptors::filtered(x_between<int>(4, 8))); /*< Boost.Range adaptors can also be used in boost::geometry::assign >*/

    std::cout << "line 1: " << boost::geometry::dsv(line1) << std::endl;
    std::cout << "line 2: " << boost::geometry::dsv(line2) << std::endl;
    std::cout << "line 3: " << boost::geometry::dsv(line3) << std::endl;

    return 0;
}

//]

//[assign_points_output
/*`
Output:
[pre
line 1: ((0, 0), (2, 3), (4, 0), (6, 3), (8, 0), (10, 3), (12, 0))
line 2: ((0, 0), (2, 2), (4, 0), (6, 2), (8, 0))
line 3: ((4, 0), (6, 3), (8, 0))
]
*/
//]


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"
#include "../test_interval_laws.hpp"

#include <boost/icl/right_open_interval.hpp>
#include <boost/icl/left_open_interval.hpp>
#include <boost/icl/closed_interval.hpp>
#include <boost/icl/open_interval.hpp>

#include <boost/icl/discrete_interval.hpp>
#include <boost/icl/continuous_interval.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

#include "../test_icl_interval_shared.hpp"
#include "../test_icl_interval.hpp"
#include "../test_icl_dynamic_interval.hpp"
#include "../test_icl_discrete_interval.hpp"
#include "../test_icl_continuous_interval.hpp"
#include "../test_icl_static_interval.hpp"

//==============================================================================
//= Traits
//==============================================================================
BOOST_AUTO_TEST_CASE
(fastest_itl_discrete_interval_traits)
{            discrete_interval_traits<discrete_type_1, discrete_interval<discrete_type_1> >(); }

//==============================================================================

//- sta.asy.{dis|con} ----------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_itl_right_open_interval_ctor_4_ordered_types)
{                       interval_ctor_4_ordered_types<right_open_interval<ordered_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_right_open_interval_4_ordered_types)
{          singelizable_interval_4_ordered_types<right_open_interval<discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_right_open_interval_4_bicremental_types)
{          singelizable_interval_4_bicremental_types<right_open_interval<discrete_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_left_open_interval_ctor_4_ordered_types)
{                      interval_ctor_4_ordered_types<left_open_interval<ordered_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_left_open_interval_4_ordered_types_singelizable)
{         singelizable_interval_4_ordered_types<left_open_interval<signed_discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_left_open_interval_4_bicremental_types)
{         singelizable_interval_4_bicremental_types<left_open_interval<discrete_type_4> >(); }

//- coverables -----------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_cover_right_open_interval_4_bicremental_types)
{    coverable_asymmetric_interval_4_bicremental_types<right_open_interval<numeric_continuous_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_cover_left_open_interval_4_bicremental_types)
{    coverable_asymmetric_interval_4_bicremental_types<left_open_interval<numeric_continuous_type_3> >(); }

//- dyn.dis --------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_itl_discrete_interval_ctor_4_discrete_types_base)
{                     interval_ctor_4_ordered_types<discrete_interval<discrete_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_discrete_interval_ctor_4_discrete_types_dynamic)
{             dynamic_interval_ctor_4_ordered_types<discrete_interval<discrete_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_discrete_interval_4_ordered_types)
{        singelizable_interval_4_ordered_types<discrete_interval<discrete_type_3> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_discrete_interval_4_bicremental_types)
{        singelizable_interval_4_bicremental_types<discrete_interval<discrete_type_3> >(); }

//- dyn.con --------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_itl_continuous_interval_ctor_4_continuous_types_base)
{                       interval_ctor_4_ordered_types<continuous_interval<continuous_type_1> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_continuous_interval_ctor_4_continuous_types_dynamic)
{               dynamic_interval_ctor_4_ordered_types<continuous_interval<continuous_type_2> >(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_continuous_interval_4_continuous_types_singelizable)
{          singelizable_interval_4_ordered_types<continuous_interval<continuous_type_3> >(); }

//------------------------------------------------------------------------------

BOOST_AUTO_TEST_CASE
(fastest_itl_distant_intervals_4_discrete_types)
{            distant_intervals_4_discrete_types<discrete_type_1, std::less>(); }

BOOST_AUTO_TEST_CASE
(fastest_itl_distant_intervals_4_numeric_continuous_types)
{            distant_intervals_4_numeric_continuous_types<numeric_continuous_type_1, std::less>(); }

//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE
(fastest_itl_dynamic_interval_bounds_4_bicremental_types)
{            dynamic_interval_bounds_4_bicremental_types<bicremental_type_2>(); }

//==============================================================================
//==============================================================================
BOOST_AUTO_TEST_CASE
(fastest_icl_interval_equal_4_integral_types)
{            interval_equal_4_integral_types<integral_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_less_4_integral_types)
{            interval_less_4_integral_types<integral_type_3>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_touches_4_bicremental_types)
{            interval_touches_4_bicremental_types<bicremental_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_touches_4_integral_types)
{            interval_touches_4_integral_types<integral_type_4>(); }

#ifndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_ctor_specific)
{            interval_ctor_specific(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_equal_4_bicremental_continuous_types)
{            interval_equal_4_bicremental_continuous_types<bicremental_continuous_type_1>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_infix_intersect_4_bicremental_types)
{            interval_infix_intersect_4_bicremental_types<bicremental_type_4>(); }

#else

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_infix_intersect_4_bicremental_types)
{            interval_infix_intersect_4_bicremental_types<discrete_type_2>(); }

BOOST_AUTO_TEST_CASE
(fastest_icl_interval_subtract_4_bicremental_types)
{            interval_subtract_4_bicremental_types<bicremental_type_5>(); }

#endif // ndef BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS


//
// 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);
}


// BLAS level 3
// symmetric matrices, syrk

#include <stddef.h>
#include <iostream>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/ublas/symmetric.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;
namespace bindings = boost::numeric::bindings;

using std::cout;
using std::cin;
using std::endl;

typedef double real_t;

typedef ublas::matrix<real_t, ublas::column_major> cm_t;
typedef ublas::matrix<real_t, ublas::row_major> rm_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::upper> ucsa_t;
typedef ublas::symmetric_adaptor<cm_t, ublas::lower> lcsa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::upper> ursa_t;
typedef ublas::symmetric_adaptor<rm_t, ublas::lower> lrsa_t;

int main (int argc, char **argv) {
  int n = 0, k = 0;
  if (argc > 1) {
    n = atoi(argv [1]);
  }
  if (argc > 2) {
    k = atoi(argv [2]);
  }

  if (n <= 0) {
    cout << "n -> ";
    cin >> n;
  }
  if (k <= 0) {
    cout << "k -> ";
    cin >> k;
  }

  cm_t ac (n, k);
  rm_t ar (n, k);
  init_m (ac, rws1());
  init_m (ar, rws1());
  print_m (ac, "ac");
  cout << endl;
  print_m (ar, "ar");
  cout << endl << endl;

  cm_t cmu (n, n);
  cm_t cml (n, n);
  rm_t rmu (n, n);
  rm_t rml (n, n);
  ucsa_t ucsa (cmu);
  lcsa_t lcsa (cml);
  ursa_t ursa (rmu);
  lrsa_t lrsa (rml);

  blas::syrk (1.0, ac, 0.0, ucsa);
  blas::syrk (1.0, ac, 0.0, lcsa);
  blas::syrk (1.0, ar, 0.0, ursa);
  blas::syrk (1.0, ar, 0.0, lrsa);

  print_m (ucsa, "ucsa");
  cout << endl;
  print_m (lcsa, "lcsa");
  cout << endl;
  print_m (ursa, "ursa");
  cout << endl;
  print_m (lrsa, "lrsa");
  cout << endl << endl;

  // part 2

  cm_t act (k, n);
  rm_t art (k, n);
  init_m (act, cls1());
  init_m (art, cls1());
  print_m (act, "act");
  cout << endl;
  print_m (art, "art");
  cout << endl << endl;

  init_m (cmu, const_val<real_t> (0));
  init_m (cml, const_val<real_t> (0));
  init_m (rmu, const_val<real_t> (0));
  init_m (rml, const_val<real_t> (0));

  blas::syrk ( 1.0, bindings::trans(act), 0.0, bindings::upper(cmu));
  blas::syrk ( 1.0, bindings::trans(act), 0.0, bindings::lower(cml));
  blas::syrk ( 1.0, bindings::trans(art), 0.0, bindings::upper(rmu));
  blas::syrk ( 1.0, bindings::trans(art), 0.0, bindings::lower(rml));

  print_m (cmu, "cmu");
  cout << endl;
  print_m (cml, "cml");
  cout << endl;
  print_m (rmu, "rmu");
  cout << endl;
  print_m (rml, "rml");
  cout << endl;
}


// All content Copyright (C) 2018 Genomics plc
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "utils/median.hpp"

BOOST_AUTO_TEST_CASE( testMedianThrowsWithEmptyList )
{
    std::vector< double > input = {};
    BOOST_CHECK_THROW( wecall::utils::functional::median( input ), wecall::utils::wecall_exception );
}

BOOST_AUTO_TEST_CASE( testMedianWithOneElement )
{
    std::vector< double > input = {1.0};
    const auto expectedResult = 1.0;
    BOOST_CHECK_EQUAL( expectedResult, wecall::utils::functional::median( input ) );
}

BOOST_AUTO_TEST_CASE( testMedianWithTwoElements )
{
    std::vector< double > input = {3.0, 1.0};
    const auto expectedResult = 2.0;
    BOOST_CHECK_EQUAL( expectedResult, wecall::utils::functional::median( input ) );
}

BOOST_AUTO_TEST_CASE( testMedianWithTwoIntegerElements )
{
    std::vector< int > input = {2, 1};
    const auto expectedResult = 1;  // Mean of 2 & 1 is 1 for integers.
    BOOST_CHECK_EQUAL( expectedResult, wecall::utils::functional::median( input ) );
}

BOOST_AUTO_TEST_CASE( testMedianWithManyElements )
{
    std::vector< double > input = {8, 5, 9, 10, 1, 3, 4, 6, 7, 2};
    const auto expectedResult = 5.5;
    BOOST_CHECK_EQUAL( expectedResult, wecall::utils::functional::median( input ) );
}


#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;
}


#include "stdafx.h"
#include "..\\Lab3Task2\Calculator.h"
#include <boost/test/unit_test.hpp>
///#include <boost/test/output_test_stream.hpp>
//#include <iostream>
#include "..\\Lab3Task2\WorkWithUI.h"
#include "..\\Lab3Task2\Difinition.h"

struct CalculatorFixture
{
	CCalculator calculator;
};

BOOST_FIXTURE_TEST_SUITE(Calculator, CalculatorFixture)
	BOOST_AUTO_TEST_CASE(Does_not_provide_a_non_existent_variable)
	{
		BOOST_CHECK(!calculator.GetValue("b").is_initialized());
	}
	BOOST_AUTO_TEST_SUITE(When_declaring_a_variable)
		BOOST_AUTO_TEST_CASE(Sets_the_value_of_nan)
		{
			BOOST_CHECK(calculator.SetVariableIdentifier("b"));
			BOOST_CHECK(std::isnan(calculator.GetValue("b").get()));
		}
		BOOST_AUTO_TEST_CASE(Does_not_allow_to_set_a_variable_to_an_existing_name)
		{
			BOOST_CHECK(calculator.SetVariableIdentifier("a"));
			BOOST_CHECK(!calculator.SetVariableIdentifier("a"));
		}
	BOOST_AUTO_TEST_SUITE_END()
	BOOST_FIXTURE_TEST_SUITE(Allows_you_to_define_a_variable_value, CalculatorFixture)
		BOOST_AUTO_TEST_CASE(May_identify_an_integer_number)
		{
			BOOST_CHECK(calculator.SetVariableValue("b", 12));
			BOOST_CHECK_EQUAL(calculator.GetValue("b").get(),12);
		}
		BOOST_AUTO_TEST_CASE(May_identify_an_double_number)
		{
			BOOST_CHECK(calculator.SetVariableValue("b", 1.1));
			BOOST_CHECK_EQUAL(calculator.GetValue("b").get(), 1.1);
		}
		BOOST_AUTO_TEST_CASE(May_identify_an_negative_number)
		{
			BOOST_CHECK(calculator.SetVariableValue("b", -1.1));
			BOOST_CHECK_EQUAL(calculator.GetValue("b"), -1.1);
		}
		BOOST_AUTO_TEST_CASE(Allows_you_to_specify_a_value_to_another_variable)
		{
			BOOST_CHECK(calculator.SetVariableValue("b", -1.1));
			BOOST_CHECK(calculator.SetVariableValue("a", "b"));
			BOOST_CHECK_EQUAL(calculator.GetValue("a"), -1.1);
			BOOST_CHECK_EQUAL(calculator.GetValue("b"), -1.1);
		}
		BOOST_AUTO_TEST_CASE(Allows_you_to_specify_the_value_of_the_function)
		{
			BOOST_CHECK(calculator.SetVariableValue("a", 1));
			BOOST_CHECK(calculator.DefineFunction("y", "a"));
			BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 1);
		}
	BOOST_AUTO_TEST_SUITE_END()
		BOOST_FIXTURE_TEST_SUITE(Allows_you_to_declare_a_function, CalculatorFixture)
		BOOST_AUTO_TEST_CASE(Which_returns_the_value_of_an_existing_variable)
		{
			BOOST_CHECK(calculator.SetVariableValue("a", 1));
			BOOST_CHECK(calculator.DefineFunction("y", "a"));
			BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 1);
		}
		BOOST_AUTO_TEST_CASE(Which_returns_the_value_of_an_existing_function)
		{
			BOOST_CHECK(calculator.SetVariableValue("b", 1));
			BOOST_CHECK(calculator.SetVariableValue("a", 1));
			BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::ADD, "b"));
			BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 2);
		}
		BOOST_AUTO_TEST_CASE(changes_the_value_of_the_function_when_the_value_of_the_variable)
		{
			BOOST_CHECK(calculator.SetVariableValue("v", 42));
			BOOST_CHECK(calculator.DefineFunction("function", "v"));
			BOOST_CHECK_EQUAL(calculator.GetFunctionsValue("function").get(), 42);
			BOOST_CHECK(calculator.SetVariableValue("v", 43));
			BOOST_CHECK_EQUAL(calculator.GetFunctionsValue("function").get(), 43);
		}
		BOOST_AUTO_TEST_SUITE_END()

			BOOST_FIXTURE_TEST_SUITE(which_returns_the_result_of_a_binary_operation, CalculatorFixture)
				BOOST_AUTO_TEST_CASE(Adding_the_values_of_the_two_identifiers)
				{
					BOOST_CHECK(calculator.SetVariableValue("b", 1));
					BOOST_CHECK(calculator.SetVariableValue("a", 1));
					BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::ADD, "b"));
					BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 2);
				}
				BOOST_AUTO_TEST_CASE(Subtracting_the_values_of_the_two_identifiers)
				{
					BOOST_CHECK(calculator.SetVariableValue("b", 2));
					BOOST_CHECK(calculator.SetVariableValue("a", 1));
					BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::SUB, "b"));
					BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), -1);
				}
				BOOST_AUTO_TEST_CASE(Multiplying_the_values_of_the_two_identifiers)
				{
					BOOST_CHECK(calculator.SetVariableValue("b", 4));
					BOOST_CHECK(calculator.SetVariableValue("a", 1));
					BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::MUL, "b"));
					BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 4);
				}
				BOOST_AUTO_TEST_CASE(Divide_the_values_of_the_two_identifiers)
				{
					BOOST_CHECK(calculator.SetVariableValue("b", 4));
					BOOST_CHECK(calculator.SetVariableValue("a", 1));
					BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::DIV, "b"));
					BOOST_CHECK_EQUAL(calculator.GetValue("y").get(), 0.25);
				}
			BOOST_AUTO_TEST_SUITE_END()
		BOOST_FIXTURE_TEST_SUITE(Allows_to_obtain_values_of_the_functions_in_alphabetical_order, CalculatorFixture)
			BOOST_AUTO_TEST_CASE(returns_the_value_of_the_variable_in_alphabetical_order)
			{
				BOOST_CHECK(calculator.SetVariableValue("c", 3));
				BOOST_CHECK(calculator.SetVariableValue("b", 2));
				BOOST_CHECK(calculator.SetVariableValue("e", 1));
				BOOST_CHECK(calculator.SetVariableValue("a", 4));
				map <string, double> variableList = { {"a",4},
															{ "b",2 },
															{ "c",3 },
															{ "e",1 } };
				map <string, double> variableListTest;

				for (CCalculator::ConstIteratorForVariablesList iter = calculator.BeginItForVariablesList(); iter != calculator.EndItForVariablesList(); ++iter)
				{
					variableListTest[iter->first] = iter->second;
				}
				BOOST_CHECK(variableListTest == variableList);
			}
			BOOST_AUTO_TEST_CASE(returns_the_value_of_the_function_in_alphabetical_order)
			{
				BOOST_CHECK(calculator.SetVariableValue("c", 1));
				BOOST_CHECK(calculator.SetVariableValue("b", 1));
				BOOST_CHECK(calculator.SetVariableValue("e", 1));
				BOOST_CHECK(calculator.SetVariableValue("a", 3));
				BOOST_CHECK(calculator.DefineFunction("y", "a", OperationType::ADD, "b"));
				BOOST_CHECK(calculator.DefineFunction("x", "e", OperationType::ADD, "c"));
				map <string, double> functionListTest = { { "x", 2 }, { "y",4} };
				map <string, double> functionList;
				for (CCalculator::ConstIteratorForFunctionsList iter = calculator.BeginItForFunctionList(); iter != calculator.EndItForFunctionList(); ++iter)
				{
					functionList[iter->first] = calculator.GetFunctionsValue(iter->first).get();
				}

				BOOST_CHECK(functionList == functionListTest);
			}
		BOOST_AUTO_TEST_SUITE_END()
			BOOST_AUTO_TEST_SUITE(May_Calculation_Area_Circle)
				BOOST_AUTO_TEST_CASE(Calculation_Process)
					{
						BOOST_CHECK(calculator.SetVariableIdentifier("radius"));
						BOOST_CHECK(calculator.SetVariableValue("pi", 3.14159265));
						BOOST_CHECK(calculator.DefineFunction("radiusSquared", "radius", OperationType::MUL, "radius"));
						BOOST_CHECK(calculator.DefineFunction("circleArea", "pi", OperationType::MUL, "radiusSquared"));
						BOOST_CHECK(calculator.SetVariableValue("radius", 10));
						BOOST_CHECK_CLOSE(calculator.GetFunctionsValue("circleArea").get(), 314.15, 1e-2);
					}
					BOOST_AUTO_TEST_SUITE_END()

					BOOST_AUTO_TEST_SUITE(TestLogicTable)
					BOOST_AUTO_TEST_CASE(F_F_F_F_result_F)
					{
						BOOST_CHECK(!calculator.DefineFunction("s", "y", OperationType::ADD, "x"));
					}
					BOOST_AUTO_TEST_CASE(F_F_F_T_result_F)
					{
						BOOST_CHECK(calculator.SetVariableIdentifier("r"));
						BOOST_CHECK(calculator.DefineFunction("x", "r"));
						BOOST_CHECK(!calculator.DefineFunction("s", "y", OperationType::ADD, "x"));
					}
					BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_SMALLESTPOSVAL_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_SMALLESTPOSVAL_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/constant/register.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_smallestposval Smallestposval
 *
 * \par Description
 * Constant Smallestposval : the least non zero positive value of floating point numbers,
 * i.e. 2.225073858507201e-308 for double and  1.1754944e-38 for float
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/smallestposval.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::smallestposval_(A0)>::type
 *     Sqrtsmallestposval();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Smallestposval
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Smallestposval of functor Smallestposval
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/
    BOOST_SIMD_CONSTANT_REGISTER( Smallestposval , double, 1
                                , 0x00800000, 0x0010000000000000ULL
                                );

  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Smallestposval, Smallestposval)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// This file was modified by Oracle on 2020.
// Modifications copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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 <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/geometries/point.hpp>

#include <test_common/test_point.hpp>

template <typename T, typename CS>
bg::model::point<T, 3, CS> create_point()
{
    T t1 = 1;
    T t2 = 2;
    T t3 = 3;
    return bg::model::point<T, 3, CS>(t1, t2, t3);
}

template <typename P, typename T>
void check_point(P& to_check, T x, T y, T z)
{
    BOOST_CHECK_EQUAL(bg::get<0>(to_check), x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check), y);
    BOOST_CHECK_EQUAL(bg::get<2>(to_check), z);
}

template <typename T, typename CS>
void test_default_constructor()
{
    bg::model::point<T, 3, CS> p(create_point<T, CS>());
    check_point(p, 1, 2, 3);
}

template <typename T, typename CS>
void test_copy_constructor()
{
    bg::model::point<T, 3, CS> p = create_point<T, CS>();
    check_point(p, 1, 2, 3);
}

template <typename T, typename CS>
void test_copy_assignment()
{
    bg::model::point<T, 3, CS> p(create_point<T, CS>());
    bg::set<0>(p, 4);
    bg::set<1>(p, 5);
    bg::set<2>(p, 6);
    check_point(p, 4, 5, 6);
}

template <typename T, typename CS>
void test_concept()
{
    typedef bg::model::point<T, 3, CS> P;

    // Compilation tests, all things should compile.
    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstPoint<P>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::Point<P>) );

    typedef typename bg::coordinate_type<P>::type T1;
    boost::ignore_unused<T1>();
}

template <typename T, typename CS>
void test_constexpr()
{
    typedef bg::model::point<T, 3, CS> P;
    constexpr P p1 = P(1, 2, 3);
    constexpr P p2 = P(4, 5);
    constexpr T c1 = bg::get<0>(p1);
    constexpr T c2 = bg::get<2>(p2);
    BOOST_CHECK_EQUAL(c1, 1);
    BOOST_CHECK_EQUAL(c2, 0);
    check_point(p1, 1, 2, 3);
    check_point(p2, 4, 5, 0);
}

template <typename T, typename CS>
void test_all()
{
    test_default_constructor<T, CS>();
    test_copy_constructor<T, CS>();
    test_copy_assignment<T, CS>();
    test_concept<T, CS>();
    test_constexpr<T, CS>();
}

template <typename CS>
void test_cs()
{
    test_all<int, CS>();
    test_all<float, CS>();
    test_all<double, CS>();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    return 0;
}


#define BOOST_TEST_MODULE matrix

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/mpl/list.hpp>

#include <mla/matrix/SparseDOK.h++>

typedef boost::mpl::list<
	float,
	double
> scalar_list;

template <typename Scalar>
using MatrixType = mla::matrix::SparseDOK<Scalar>;

BOOST_AUTO_TEST_SUITE(test_matrix)

BOOST_AUTO_TEST_CASE_TEMPLATE( Declaration, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);

	BOOST_CHECK_EQUAL(m.rows(), 3);
	BOOST_CHECK_EQUAL(m.columns(), 3);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Clearing, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			BOOST_CHECK_EQUAL(m.getValue(i, j), 0.0);
		}
	}

}

BOOST_AUTO_TEST_CASE_TEMPLATE( Assigning, Scalar, scalar_list )
{
	MatrixType<Scalar> m(3,3);
	m.setZero();

	m(1,1) = 1.0;

	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);

	BOOST_CHECK_EQUAL(m.getValue(0, 1), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 1), 0.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Permute_rows, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	m.permuteRows(2,3);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( Permute_columns, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	m.permuteColumns(2,3);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( symmetric_reorder_identity_matrix, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	std::vector<size_t> new_order{0,1,2,3,4};

	m.symmetricReorder(new_order);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE( symmetric_reorder_identity_matrix_2, Scalar, scalar_list )
{
	MatrixType<Scalar> m(5,5);
	m.setZero();

	m(1,1) = 1.0;
	m(2,2) = 2.0;
	m(3,3) = 3.0;
	m(4,4) = 4.0;

	std::vector<size_t> new_order{0,2,1,3,4};

	m.symmetricReorder(new_order);

	BOOST_CHECK_EQUAL(m.getValue(0, 0), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(1, 1), 2.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 2), 1.0);
	BOOST_CHECK_EQUAL(m.getValue(2, 3), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 2), 0.0);
	BOOST_CHECK_EQUAL(m.getValue(3, 3), 3.0);
	BOOST_CHECK_EQUAL(m.getValue(4, 4), 4.0);
}

BOOST_AUTO_TEST_SUITE_END()


//============================================================================
// Name        : pcshow_lines.cpp
// Author      : adapted from Pangolin by S. Lovegrove and PangoCloud by J. Liu
// Date        : 03/2019
// License     : MIT License (Pangolin) / GNU General Public License
// Description : display 3D point cloud plus lines using Pangolin viewer
// Github      : https://github.com/stevenlovegrove/Pangolin#include
//               https://github.com/HTLife/PangoCloud
//============================================================================

#include "pcshow.h"
// libraries
#include <Eigen/Dense>
#include <pangolin/pangolin.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/gldraw.h>
#include "draw_lines.h"

/**
 * class for 3D point viewing
 */
class PointViewer3D {

    const int stride;
    const int num_points;
    GLuint vbo;

public:
    PointViewer3D(std::vector<Vec3> *points) : num_points(points->size()), stride(sizeof(Vec3)) {
        void* ptr_to_data = &(*points).front();
        glGenBuffers(1, &vbo);
        glBindBuffer(GL_ARRAY_BUFFER, vbo);
        glBufferData(GL_ARRAY_BUFFER, num_points * stride, ptr_to_data, GL_STATIC_DRAW);
        glBindBuffer(GL_ARRAY_BUFFER, 0);
    }

    virtual ~PointViewer3D() {
        glDeleteBuffers(1, &vbo);
    }

    void draw() {
        glBindBuffer(GL_ARRAY_BUFFER, vbo);
        glVertexPointer(3, GL_DOUBLE, stride, 0); // GL_FLOAT
        glEnableClientState(GL_VERTEX_ARRAY);
        glDrawArrays(GL_POINTS, 0, num_points);
        glDisableClientState(GL_VERTEX_ARRAY);
        glBindBuffer(GL_ARRAY_BUFFER, 0);
    }

};

class PointViewer3Df {

    using Vec3f = Eigen::Vector3f;

    const int stride;
    const int num_points;
    GLuint vbo;

public:
    PointViewer3Df(std::vector<Vec3f> *points) : num_points(points->size()), stride(sizeof(Vec3f)) {
        void* ptr_to_data = &(*points).front();
        glGenBuffers(1, &vbo);
        glBindBuffer(GL_ARRAY_BUFFER, vbo);
        glBufferData(GL_ARRAY_BUFFER, num_points * stride, ptr_to_data, GL_STATIC_DRAW);
        glBindBuffer(GL_ARRAY_BUFFER, 0);
    }

    virtual ~PointViewer3Df() {
        glDeleteBuffers(1, &vbo);
    }

    void draw() {
        glBindBuffer(GL_ARRAY_BUFFER, vbo);
        glVertexPointer(3, GL_FLOAT, stride, 0); // GL_FLOAT
        glEnableClientState(GL_VERTEX_ARRAY);
        glDrawArrays(GL_POINTS, 0, num_points);
        glDisableClientState(GL_VERTEX_ARRAY);
        glBindBuffer(GL_ARRAY_BUFFER, 0);
    }

};

/**
 * actual point cloud viewing function
 */
void pcshow_lines(std::vector<Vec3> &points, const std::vector<Eigen::Matrix<double, 6, 1>>& lines, std::string win_name)
{

    // Create OpenGL window in single line
    pangolin::CreateWindowAndBind(win_name, 640, 480);
    const int UI_WIDTH = 180;

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState camera(
                pangolin::ProjectionMatrix(640, 480, 480, 480, 320, 240, 0.1, 1000),
                pangolin::ModelViewLookAt(0, 0.5, 1.0, 0, 0, 0, pangolin::AxisY)
                );

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& display = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -640.0f/480.0f)
        .SetHandler(new pangolin::Handler3D(camera));

    while( !pangolin::ShouldQuit() )
    {
        // Clear entire screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Activate efficiently by object
        display.Activate(camera);

        PointViewer3D cloud(&points);
        cloud.draw();
        draw_lines(lines);

        pangolin::FinishFrame();
    }
}

void pcshow_lines(std::vector<Eigen::Vector3f> &points, const std::vector<Eigen::Matrix<double, 6, 1>>& lines, std::string win_name)
{

    // Create OpenGL window in single line
    pangolin::CreateWindowAndBind(win_name, 640, 480);
    const int UI_WIDTH = 180;

    // 3D Mouse handler requires depth testing to be enabled
    glEnable(GL_DEPTH_TEST);

    // Define Camera Render Object (for view / scene browsing)
    pangolin::OpenGlRenderState camera(
                pangolin::ProjectionMatrix(640, 480, 530, 530, 320, 240, 0.1, 1000),
                pangolin::ModelViewLookAt(0, -2.5, 12.5, 0, 0, 0, pangolin::AxisY)
                );

    // Add named OpenGL viewport to window and provide 3D Handler
    pangolin::View& display = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -640.0f/480.0f)
        .SetHandler(new pangolin::Handler3D(camera));

    while( !pangolin::ShouldQuit() )
    {
        // Clear entire screen
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Activate efficiently by object
        display.Activate(camera);

        PointViewer3Df cloud(&points);
        cloud.draw();
        draw_lines(lines);

        pangolin::FinishFrame();
    }
}


/* 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);
  }

}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm algebra elementary pow
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/type_traits.h"
#include "fern/algorithm/algebra/elementary/pow.h"

namespace fa = fern::algorithm;

template<
    class Value1,
    class Value2>
using OutOfDomainPolicy = fa::pow::OutOfDomainPolicy<Value1, Value2>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double, double> policy;
        BOOST_CHECK(policy.within_domain(3.0, 4.0));
        BOOST_CHECK(policy.within_domain(0.0, 0.0));

        BOOST_CHECK(!policy.within_domain(-3.0, 4.1));
        BOOST_CHECK(!policy.within_domain(0.0, -1.0));
    }
}

template<
    class Value1,
    class Value2,
    class Result>
using OutOfRangePolicy = fa::pow::OutOfRangePolicy<Value1, Value2, Result>;

template<
    class Value1,
    class Value2,
    class Result>
struct VerifyWithinRange
{
    bool operator()(
        Value1 const& value1,
        Value2 const& value2)
    {
        fa::SequentialExecutionPolicy sequential;

        OutOfRangePolicy<Value1, Value2, Result> policy;
        Result result;

        fa::algebra::pow(sequential, value1, value2, result);

        return policy.within_range(value1, value2, result);
    }
};

BOOST_AUTO_TEST_CASE(out_of_range_policy)
{
    {
        float max_float = fern::max<float>();

        VerifyWithinRange<float, float, float> verify;
        BOOST_CHECK_EQUAL(verify(0.0, 0.0), true);

        BOOST_CHECK_EQUAL(verify(max_float, max_float), false);
    }
}

template<
    class Value1,
    class Value2,
    class Result>
void verify_value(
    Value1 const& value1,
    Value2 const& value2,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::algebra::pow(sequential, value1, value2, result_we_get);
    BOOST_CHECK_EQUAL(result_we_get, result_we_want);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<float, float, float>( 0.0f, 0.0f, 1.0f);
    verify_value<float, float, float>( 1.0f, 1.0f, 1.0f);
    verify_value<float, float, float>( 2.0f, 2.0f, 4.0f);
}


/*
 * 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;
}


#include <boost/mpl/int.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/simd/sdk/memory/align_on.hpp>

using boost::mpl::int_;
using boost::mpl::equal_to;
using boost::simd::meta::align_on_c;

int main()
{
  BOOST_MPL_ASSERT(( equal_to<align_on_c<0,4>::type,int_<0> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on_c<1,4>::type,int_<4> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on_c<4,4>::type,int_<4> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on_c<1055,4>::type,int_<1056> >::type ));
}


/**********************************************************************
*  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