// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file pfnf_rewriter_test.cpp
/// \brief Tests for pfnf rewriter.

#define BOOST_TEST_MODULE pfnf_rewriter_test
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/pbes/detail/pbessolve.h"
#include "mcrl2/pbes/detail/pfnf_print.h"
#include "mcrl2/pbes/detail/parse.h"
#include "mcrl2/pbes/rewrite.h"
#include "mcrl2/pbes/rewriters/pfnf_rewriter.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

const std::string VARIABLE_SPECIFICATION =
  "datavar         \n"
  "  b:  Bool;     \n"
  "  b1: Bool;     \n"
  "  b2: Bool;     \n"
  "  b3: Bool;     \n"
  "                \n"
  "  n:  Nat;      \n"
  "  n1: Nat;      \n"
  "  n2: Nat;      \n"
  "  n3: Nat;      \n"
  "                \n"
  "  p:  Pos;      \n"
  "  p1: Pos;      \n"
  "  p2: Pos;      \n"
  "  p3: Pos;      \n"
  "                \n"
  "predvar         \n"
  "  X;            \n"
  "  Y: Nat;       \n"
  "  W: Bool;      \n"
  "  Z: Bool, Nat; \n"
  "  X0;           \n"
  "  X1: Bool;     \n"
  "  X2: Nat, Nat; \n"
  "  X3: Bool, Nat;\n"
  "  X4: Nat, Bool;\n"
  ;

inline
pbes_system::pbes_expression expr(const std::string& text)
{
  return pbes_system::parse_pbes_expression(text, VARIABLE_SPECIFICATION);
}

void test_pfnf_expression(const std::string& s)
{
  pbes_system::detail::pfnf_traverser visitor;
  pbes_system::pbes_expression t1 = expr(s);
  visitor.apply(t1);
  pbes_system::pbes_expression t2 = visitor.evaluate();
  data::rewriter datar;
  pbes_system::simplify_data_rewriter<data::rewriter> R(datar);
  if (R(t1) != R(t2))
  {
    BOOST_CHECK(R(t1) == R(t2));
    std::cout << "--- failed test --- " << std::endl;
    std::cout << "t1    " << pbes_system::pp(t1) << std::endl;
    std::cout << "t2    " << pbes_system::pp(t2) << std::endl;
    std::cout << "R(t1) " << pbes_system::pp(R(t1)) << std::endl;
    std::cout << "R(t2) " << pbes_system::pp(R(t2)) << std::endl;
  }
}

BOOST_AUTO_TEST_CASE(test_pfnf_visitor)
{
  test_pfnf_expression("forall m:Nat. false");
  test_pfnf_expression("X && (Y(3) || X)");
  //test_pfnf_expression("forall m:Nat. (Y(m) || exists n:Nat. Y(n))");
  //test_pfnf_expression("forall m:Nat. (Y(m) || exists m:Nat. Y(m))");
}

void test_pfnf(const std::string& pbes_spec)
{
  std::cerr << "--- test_pfnf ---" << std::endl;
  pbes p = txt2pbes(pbes_spec);
  std::cerr << "- before:" << std::endl;
  std::cerr << pbes_system::pp(p) << std::endl;
  pfnf_rewriter R;
  pbes_rewrite(p, R);
  std::cerr << "- after:" << std::endl;
  std::cerr << pbes_system::pp(p) << std::endl;
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));
  std::cerr << "-----------------" << std::endl;
}

BOOST_AUTO_TEST_CASE(test_pfnf_rewriter)
{
  using namespace pbes_system;

  std::string PFNF1 =
    "pbes                                                    \n"
    "nu X(b:Bool) = (val(b) && (X(b) || (X(!b) && X(!!b)))); \n"
    "                                                        \n"
    "init X(true);                                           \n"
    ;

  std::string PFNF2 =
    "pbes                                                        \n"
    "nu X(m:Nat) = (forall n:Nat. X(n)) && (forall j:Nat. X(j)); \n"
    "                                                            \n"
    "init X(0);                                                  \n"
    ;

  test_pfnf(PFNF1);
  test_pfnf(PFNF2);

  pfnf_rewriter R;
  pbes_expression x = parse_pbes_expression("val(n1 > 3) && forall b: Bool. forall n: Nat. val(n > 3) || exists n:Nat. val(n > 5)", VARIABLE_SPECIFICATION);
  pbes_expression y = R(x);
}

void test_pfnf_rewriter2(const std::string& text, const bool expected_result)
{
  pbes p = txt2pbes(text);
  std::cout << "\ntest_pfnf_rewriter2\n" << std::endl;
  std::cout << "--- before ---\n";
  std::cout << pbes_system::pp(p) << std::endl;

  bool solution = detail::pbessolve(p);
  BOOST_CHECK(solution == expected_result);

  pfnf_rewriter R;
  pbes_rewrite(p, R);

  std::cout << "--- after ---\n";
  std::cout << pbes_system::pp(p) << std::endl;

  BOOST_CHECK(p.is_well_typed());
  solution = detail::pbessolve(p);
  BOOST_CHECK(solution == expected_result);
}

BOOST_AUTO_TEST_CASE(test_pfnf_rewriter_2)
{
  // problematic case found by random tests 14-1-2011
  std::string text =
    "pbes nu X =                                                  \n"
    "       (exists v: Nat. true) && (forall v: Nat. val(v < 3)); \n"
    "                                                             \n"
    "init X;                                                      \n"
    ;
  test_pfnf_rewriter2(text,false);

  text =
    "pbes                                                                                \n"
    "nu X = (exists v: Nat. (val(v > 4) || Y(v))) && (forall v: Nat. (val(v < 3) && Y(v))); \n"
    "mu Y(n: Nat) = true;                                                                   \n"
    "                                                                                \n"
    "init X;                                                                                \n"
    ;
//  test_pfnf_rewriter2(text);

  // problematic case found by random tests 15-1-2011
  text =
    "pbes nu X0(m: Nat) =                                                                                \n"
    "       ((false && (forall u: Nat. false)) && false && (forall v: Nat. val(!(v < 3)) && (forall u: Nat. val(!(u < 3)) && val(!(v == u))))) || (exists u: Nat. val(!(u < 3)) || false); \n"
    "     mu X1 =                                                                                \n"
    "       true;                                                                                \n"
    "     mu X2 =                                                                                \n"
    "       true && false;                                                                                \n"
    "                                                                                \n"
    "init X0(0);                                                                                \n"
    ;
  test_pfnf_rewriter2(text,true);
}

BOOST_AUTO_TEST_CASE(test_is_pfnf)
{
  std::string text =
    "pbes nu X(n: Nat) = X(0) || X(1) || X(2);   \n"
    "     nu Y(n: Nat) = (Y(0) || X(1)) && X(2); \n"
    "     nu Z(n: Nat) = true => (X(0) || X(1)); \n"
    "     nu X3(n: Nat) = true;                  \n"
    "     nu X4(n: Nat) = val(true);             \n"
    "     nu X5(n: Nat) = (true => (X(0) || X(1))) && val(true) && X(0); \n"
    "     nu X6(n: Nat) = (forall b:Bool. true) && (true => (X(0) || X(1))) && val(true) && X(0); \n"
    "     nu X7(n: Nat) = false;                 \n"
    "     nu X8(n: Nat) = val(false);            \n"
    "     nu X9(n: Nat) = X(0);                  \n"
    "     nu X10(n:Nat) = (X(0) || X(1)) && X(0);\n"
    "     nu X11(b: Bool) = val(b) && (X11(b) || X11(!b)) && (X11(b) || X11(!!b)); \n"
    "init X(0);                                  \n"
    ;
  pbes p = txt2pbes(text, false);
  pbes_expression x;

  x = p.equations()[0].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_imp(x));

  x = p.equations()[1].formula();
  BOOST_CHECK(!pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(!pbes_system::detail::is_pfnf_imp(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf(x));

  x = p.equations()[2].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_imp(x));
  std::vector<pbes_expression> v = pbes_system::detail::pfnf_implications(x);
  BOOST_CHECK(v.size() == 1);
  BOOST_CHECK(pbes_system::pp(v[0]) == "val(true) => X(0) || X(1)");

  x = p.equations()[3].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_imp(x));

  x = p.equations()[4].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_imp(x));

  x = p.equations()[5].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_inner_and(x));

  x = p.equations()[6].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));

  x = p.equations()[7].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_inner_and(x));

  x = p.equations()[8].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_inner_and(x));

  x = p.equations()[9].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_imp(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_inner_and(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));

  x = p.equations()[10].formula();
  BOOST_CHECK(!pbes_system::detail::is_pfnf_or(x));
  BOOST_CHECK(!pbes_system::detail::is_pfnf_imp(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_inner_and(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));

  x = p.equations()[11].formula();
  BOOST_CHECK(pbes_system::detail::is_pfnf(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_outer_and(x));
  BOOST_CHECK(pbes_system::detail::is_pfnf_expression(x));

  BOOST_CHECK(pbes_system::detail::is_pfnf(p));

  text =
    "pbes nu X0(m: Nat) =                                                            \n"
    "  exists u2: Nat. forall v: Nat. forall u1: Nat. forall u: Nat. val(!(u2 < 3)); \n"
    "mu X1 =                                                                         \n"
    "  true;                                                                         \n"
    "mu X2 =                                                                         \n"
    "  false;                                                                        \n"
    "                                                                                \n"
    "init X0(0);                                                                     \n"
    ;
  p = txt2pbes(text, false);
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));

  text =
    "pbes nu X0(m: Nat) =                                                            \n"
    "  val(false);                                                                   \n"
    "                                                                                \n"
    "init X0(0);                                                                     \n"
    ;
  p = txt2pbes(text, false);
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));

  text =
    "pbes nu X(b: Bool) =                                  \n"
    "       val(b) && (X(b) || X(!b)) && (X(b) || X(!!b)); \n"
    "                                                      \n"
    "init X(true);                                         \n"
    ;
  p = txt2pbes(text, false);
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));
}

BOOST_AUTO_TEST_CASE(test_pfnf_print)
{
  std::string text =
    "pbes nu X(n: Nat) = X(0) || X(1) || X(2);   \n"
    "     nu Y(n: Nat) = forall c:Bool. forall b:Bool. (X(0) || X(1) && X(0)) && (val(n > 0) => (X(2) || X(3))); \n"
    "init X(0);                                  \n"
    ;
  pbes p = txt2pbes(text);
  pfnf_rewriter R;
  pbes_rewrite(p, R);
  std::cout << "--- pfnf print ---" << std::endl;
  std::cout << pp(p) << std::endl;
  std::cout << "---" << std::endl;
  std::cout << pfnf_pp(p) << std::endl;
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));
}

BOOST_AUTO_TEST_CASE(test_pfnf_rewriter3)
{
  std::string text =
    "sort D = struct d1 | d2 | d3;                                                \n"
    "                                                                             \n"
    "map match: Pos # D -> Bool;                                                  \n"
    "                                                                             \n"
    "pbes                                                                         \n"
    "                                                                             \n"
    "nu Y(s1: Pos, e1: D, s2: Pos, e2: D) =                                       \n"
    "    forall d: D.                                                             \n"
    "    (                                                                        \n"
    "        (val(s1 == 1 && match(1, d1))                  => X(2, d1, s2, e2))  \n"
    "     && (val(s1 == 1 && !match(1, d1))                 => X(1, d1, s2, e2))  \n"
    "     && (val(s1 == 2 && s2 == 1 && match(2, e1))       => Y(1, d1, 2, e1))   \n"
    "     && (val(s1 == 2 && s2 == 1 && !match(2, e1))      => Y(1, d1, 1, d1))   \n"
    "     && (val(s2 == 2)                                  => Y(s1, e1, 1, d1))  \n"
    "     && (val(s1 == 1 && match(1, d))                   => Y(2, d, s2, e2))   \n"
    "     && (val(s1 == 1 && !match(1, d))                  => Y(1, d1, s2, e2))  \n"
    "    );                                                                       \n"
    "                                                                             \n"
    "mu X(s1: Pos, e1: D, s2: Pos, e2: D) =                                       \n"
    "    forall d: D.                                                             \n"
    "    (                                                                        \n"
    "        (val(s1 == 2 && s2 == 1 && match(2, e1))       => X(1, d1, 2, e1))   \n"
    "     && (val(s1 == 2 && s2 == 1 && !match(2, e1))      => X(1, d1, 1, d1))   \n"
    "     && (val(e2 != d1 && s2 == 2)                      => X(s1, e1, 1, d1))  \n"
    "     && (val(s1 == 1 && match(1, d))                   => X(2, d, s2, e2))   \n"
    "     && (val(s1 == 1 && !match(1, d))                  => X(1, d1, s2, e2))  \n"
    "    );                                                                       \n"
    "                                                                             \n"
    "init Y(1, d1, 1, d1);                                                        \n"
    ;
  pbes p = txt2pbes(text, true);
  BOOST_CHECK(!pbes_system::detail::is_pfnf(p));
  pfnf_rewriter R;
  pbes_system::pbes_rewrite(p, R);
  BOOST_CHECK(pbes_system::detail::is_pfnf(p));
}


// [===========================================================================]
// [                                M o n e t a                                ]
// [---------------------------------------------------------------------------]
// [                                                                           ]
// [                          Copyright (C) 2005-2015                          ]
// [                      Rodrigo Madera <madera@acm.org>                      ]
// [                                                                           ]
// [---------------------------------------------------------------------------]
// [         Distributed under the Boost Software License, Version 1.0         ]
// [ Read accompanying LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt ]
// [===========================================================================]

#pragma once
#include <boost/mpl/fold.hpp>
#include <boost/mpl/clear.hpp>
#include <boost/mpl/pair.hpp>
#include <boost/mpl/set.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/contains.hpp>

namespace mplx {

	template <class Sequence>
	struct remove_duplicates {
		typedef typename boost::mpl::fold<
			Sequence,
			boost::mpl::pair<
				typename boost::mpl::clear<Sequence>::type,
				boost::mpl::set0<>
			>,
			boost::mpl::if_<
				boost::mpl::contains<
					boost::mpl::second<boost::mpl::_1>,
					boost::mpl::_2
				>,
				boost::mpl::_1,
				boost::mpl::pair<
					boost::mpl::push_back<
						boost::mpl::first<boost::mpl::_1>,
						boost::mpl::_2
					>,
					boost::mpl::insert<
						boost::mpl::second<boost::mpl::_1>,
						boost::mpl::_2
					>
				>
			>
		>::type::first type;
	};

}


#define BOOST_TEST_MODULE lue framework core span
#include <boost/test/unit_test.hpp>
#include "lue/framework/core/span.hpp"

BOOST_AUTO_TEST_CASE(dynamic_span_test)
{
    lue::Count const nr_rows = 60;
    lue::Count const nr_cols = 40;
    lue::Count const nr_cells = nr_rows * nr_cols;

    auto idx =
        [nr_rows, nr_cols](
            lue::Index const row,
            lue::Index const col) -> lue::Index
        {
            return row * nr_cols + col;
        };

    // Allocate a 1D array
    std::vector<int> cells(nr_cells);

    // Fill array with increasing valueѕ
    std::iota(cells.begin(), cells.end(), 0);

    // View 1D array as a 2D array
    lue::DynamicSpan<int, 2> raster(cells.data(), nr_rows, nr_cols);
    BOOST_REQUIRE_EQUAL(raster(0, 0), 0);
    BOOST_REQUIRE_EQUAL(raster(3, 4), idx(3, 4));
    BOOST_REQUIRE_EQUAL(raster(nr_rows - 1, nr_cols - 1), nr_cells - 1);

    // View part of the 2D array as a new view

    // Select a single cell
    {
        auto const cell = lue::subspan(
                raster,
                3,
                4
            );
        BOOST_CHECK_EQUAL(cell.rank(), 0);
        BOOST_CHECK_EQUAL(cell(), idx(3, 4));
    }

    // Select a single row
    {
        auto const row = lue::subspan(
                raster,
                3,
                std::experimental::all
            );
        BOOST_CHECK_EQUAL(row.rank(), 1);
        BOOST_REQUIRE_EQUAL(row.extent(0), nr_cols);
        BOOST_CHECK_EQUAL(row(0), idx(3, 0));
        BOOST_CHECK_EQUAL(row(1), idx(3, 0 + 1));
    }

    // Select a single column
    {
        auto const col = lue::subspan(
                raster,
                std::experimental::all,
                4
            );
        BOOST_CHECK_EQUAL(col.rank(), 1);
        BOOST_REQUIRE_EQUAL(col.extent(0), nr_rows);
        BOOST_CHECK_EQUAL(col(0), idx(0, 4));
        BOOST_CHECK_EQUAL(col(1), idx(0 + 1, 4));
    }

    // Select a window
    {
        lue::Count const radius = 1;
        lue::Count const window_size = 2 * radius + 1;
        auto const window = lue::subspan(
                raster,
                std::pair<std::ptrdiff_t, std::ptrdiff_t>{3, 3 + window_size},
                std::pair<std::ptrdiff_t, std::ptrdiff_t>{4, 4 + window_size}
            );
        BOOST_CHECK_EQUAL(window.rank(), 2);
        BOOST_REQUIRE_EQUAL(window.extent(0), window_size);
        BOOST_REQUIRE_EQUAL(window.extent(1), window_size);
        BOOST_CHECK_EQUAL(window(0, 0), idx(3 + 0, 4 + 0));
        BOOST_CHECK_EQUAL(window(0, 1), idx(3 + 0, 4 + 1));
        BOOST_CHECK_EQUAL(window(0, 2), idx(3 + 0, 4 + 2));
        BOOST_CHECK_EQUAL(window(1, 0), idx(3 + 1, 4 + 0));
        BOOST_CHECK_EQUAL(window(1, 1), idx(3 + 1, 4 + 1));
        BOOST_CHECK_EQUAL(window(1, 2), idx(3 + 1, 4 + 2));
        BOOST_CHECK_EQUAL(window(2, 0), idx(3 + 2, 4 + 0));
        BOOST_CHECK_EQUAL(window(2, 1), idx(3 + 2, 4 + 1));
        BOOST_CHECK_EQUAL(window(2, 2), idx(3 + 2, 4 + 2));
    }
}


/*
 *	main.cpp
 *
 * 	\brief     10. Aufgabe
 *  \details   Main. Loest das Steinergraph Problem.
 *  \author    Christopher Wyczisk
 *  \date      20.07.2017
 */
#include <iostream>
#include <boost/program_options.hpp>

#include "../header/steiner.h"
#include "../header/graph.h"
#include "../header/kommunikator.h"

using namespace std;

vector<std::pair<int, int>> ecken[100];
int ergebnisse[100];

/**
 * \brief Helper von primzahlen().
 */
bool hatTeiler(const std::vector<int>* primzahlen, int zahl) {
	bool hatTeiler = false;
	int i = 0;

	while(i < primzahlen -> size() && !hatTeiler) {
		int primzahl = primzahlen -> at(i);
		hatTeiler = ((zahl % primzahl) == 0 || primzahl / 2 >= zahl);
		i++;
	}

	return hatTeiler;
}

/**
 * \brief berechnet alle relevanten primzahlen bis AnzahlEkcneGlobal.
 */
void primzahlen(vector<int>* primzahlen, int anzahlEcken) {
	for(int i = 2; i < anzahlEcken; i++) {
		if(!hatTeiler(primzahlen, i)) {
			primzahlen -> push_back(i);
		}
	}
}

vector<int>* berechneHundertTerminals(vector<int>* terminals, kommunikator* kom) {
	vector<int>* hundertTerminals;
	primzahlen(terminals, kom -> anzahlEckenGlobal);
	if(terminals -> size() < 100) {
		hundertTerminals = terminals;
	} else {
		hundertTerminals = new vector<int>(terminals -> begin(), terminals -> begin() + 100);
	}
	return hundertTerminals;
}

/**
 * \brief berechnet unter nutzung von threads den index zu den knoten mit minimalen wert.
 */
int berechneMinimalenKnotenkey(kommunikator* kom, int anzahlThreads, vector<int>* terminals, vector<int>* hundertTerminals) {
	int minKey;
	int wert = INT_MAX;

	#pragma omp parallel for num_threads(anzahlThreads)
	for(int i = 0; i < hundertTerminals -> size(); i++) {
		steiner* steinerAlgo = new steiner(*terminals);
		ecken[i] = steinerAlgo -> loesen(*kom -> kanten, kom -> anzahlEckenGlobal, hundertTerminals -> at(i));
		ergebnisse[i] = steinerAlgo -> gibObjektivenWert();
		delete steinerAlgo;
	}

	for(int i = 0; i < hundertTerminals -> size(); i++) {
		if(ergebnisse[i] < wert){
			minKey = i;
			wert = ergebnisse[i];
		}
	}
	return minKey;
}

int main(int argc, char* argv[]) {
	kommunikator* kom = new kommunikator();
	int anzahlThreads = kom -> anzahlThreads(argv[1]);

	ifstream file;
	bool fileEingelesen = kom -> graphEinlesen(argc, argv, file);
	bool erstellt = kom -> graphErstellen(file);

	vector<int>* terminals = new vector<int>();
	vector<int>* hundertTerminals = berechneHundertTerminals(terminals, kom);

	kom -> starteZeitmessung();
	int minKey = berechneMinimalenKnotenkey(kom, anzahlThreads, terminals, hundertTerminals);

	graph g(ecken[minKey], kom -> anzahlEckenGlobal);
	if(!g.vollstaendig(hundertTerminals)) {
		cout << "Einige Terminals fehlen" << endl;
	}
	if(!g.keineZykel()){
		cout << "Kreise im Graph gefunden" << endl;
	}
	if(!g.zusammenhaengend()){
		cout << "Nicht zusammenhaengend" << endl;
	}

	//ausgabe
	kom -> ausgabe(ergebnisse[minKey], ecken[minKey]);

	kom -> ausgabeDerLaufzeit();
	delete hundertTerminals;
	delete kom;
	return EXIT_SUCCESS;
}


//Copyright (c) 2015 Zachary Kann
//
//Permission is hereby granted, free of charge, to any person obtaining a copy
//of this software and associated documentation files (the "Software"), to deal
//in the Software without restriction, including without limitation the rights
//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the Software is
//furnished to do so, subject to the following conditions:
//
//The above copyright notice and this permission notice shall be included in all
//copies or substantial portions of the Software.
//
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
//SOFTWARE.

// ---
// Author: Zachary Kann

#include <armadillo>

using namespace arma;

#ifndef _Z_TCF_HPP_
#define _Z_TCF_HPP_

template <class T>
class TCF
{
    private:
        unsigned length_, interval_, zeros_, number_, speclength_;
        unsigned corr_int_, corr_, num_corr_;
        Row<T> correlation_function_;

    public:
        TCF(unsigned length, unsigned interval = 1, unsigned zeros = 0):
            length_(length), interval_(interval), zeros_(zeros)
        {
            correlation_function_.zeros(length_+zeros_);
        }
        void correlate(const mat& vec1, const mat& vec2);
        void correlate(const mat& vec1, const mat& vec2,
            const unsigned mod);
        void correlate_one_direction(const mat& vec1, const mat& vec2);
        void correlate_one_direction(const mat& vec1, const mat& vec2,
            const unsigned mod);
        void correlate_1D(const rowvec& vec1, const rowvec& vec2);
        void correlate_1D(const rowvec& vec1, const rowvec& vec2,
            const unsigned mod);
        void correlate_1D_one_direction(const rowvec& vec1, const rowvec& vec2);
        void correlate_1D_one_direction(const rowvec& vec1, const rowvec& vec2,
            const unsigned mod);
        inline void correlate_one_direction(const rowvec& vec)
        {
            correlate_one_direction(vec, vec);
        }
        inline void correlate_one_direction(const rowvec& vec,
            const unsigned mod)
        {
            correlate_one_direction(vec, vec, mod);
        }
        inline void correlate_1D_one_direction(const rowvec& vec)
        {
            correlate_1D_one_direction(vec, vec);
        }
        inline void correlate_1D_one_direction(const rowvec& vec,
            const unsigned mod)
        {
            correlate_1D_one_direction(vec, vec, mod);
        }
        unsigned length() { return length_; }
        inline T tcf(int i) { return correlation_function_(i); }
};
#endif


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

#define TEST_NAME "base/radial_absolute_pose"
#include "util/testing.h"

#include <Eigen/Core>

#include "base/pose.h"
#include "estimators/radial_absolute_pose.h"
#include "optim/ransac.h"
#include "util/random.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestRadialP5P) {
  SetPRNGSeed(0);

  for (int trials = 0; trials < 10; ++trials) {
    Eigen::Matrix3x4d gt_pose;
    gt_pose.block<3, 3>(0, 0) =
        Eigen::Quaterniond::UnitRandom().toRotationMatrix();
    gt_pose.col(3).setRandom();

    std::vector<Eigen::Vector2d> points2D;
    std::vector<Eigen::Vector3d> points3D;

    for (int i = 0; i < 5; ++i) {
      Eigen::Vector2d x;
      x.setRandom();
      Eigen::Vector3d X = x.homogeneous();
      X *= RandomReal(0.1, 100.0);
      X = gt_pose.block<3, 3>(0, 0).transpose() * (X - gt_pose.col(3));

      points2D.push_back(x);
      points3D.push_back(X);
    }

    std::vector<Eigen::Matrix3x4d> poses =
        RadialP5PEstimator::Estimate(points2D, points3D);

    BOOST_CHECK(poses.size() > 0);

    double min_pose_diff = std::numeric_limits<double>::max();
    for (auto p : poses) {
      double diff = (gt_pose.block<2, 4>(0, 0) - p.block<2, 4>(0, 0)).norm();
      min_pose_diff = std::min(diff, min_pose_diff);

      std::vector<double> residuals;
      RadialP5PEstimator::Residuals(points2D, points3D, p, &residuals);

      for (double r : residuals) {
        BOOST_CHECK(r < 1e-6);
      }
    }

    BOOST_CHECK(min_pose_diff < 1e-2);
  }
}


/* vim:set ts=3 sw=3 sts=3 et: */
/**
 * \file       murmur3.hpp
 * \author     Marcus Holland-Moritz (marcus@last.fm)
 * \copyright  Copyright © 2008-2013 Last.fm Limited
 *
 * This file is part of libmoost.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef MOOST_HASH_MURMUR3_HPP
#define MOOST_HASH_MURMUR3_HPP

#include <vector>

#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_pod.hpp>

namespace moost { namespace hash {

/**
 * Fast murmur3 hash implementation
 *
 * This class implements the 32-bit murmur3 hash algorithm and is
 * in fact derived from the original code:
 *
 *   https://github.com/PeterScott/murmur3
 *
 * The main difference is that the template function compute32()
 * can be more easily optimised by the compiler. Especially if
 * you're hashing constant-size types (even more so if they're
 * a multiple of 32 bits in size), the code will be significantly
 * faster than the original implementation.
 */
class murmur3
{
private:
   static uint32_t fmix(uint32_t h)
   {
     h ^= h >> 16;
     h *= 0x85ebca6b;
     h ^= h >> 13;
     h *= 0xc2b2ae35;
     h ^= h >> 16;

     return h;
   }

   static uint32_t getblock(const uint32_t *p, int i)
   {
     return p[i];
   }

   static uint32_t rotl32 (uint32_t x, int8_t r)
   {
     return (x << r) | (x >> (32 - r));
   }

public:
   static uint32_t compute32(const void *key, size_t len, uint32_t seed)
   {
      const uint8_t *data = reinterpret_cast<const uint8_t *>(key);
      const int nblocks = len/4;

      uint32_t h1 = seed;

      const uint32_t c1 = 0xcc9e2d51;
      const uint32_t c2 = 0x1b873593;

      //----------
      // body

      const uint32_t *blocks = reinterpret_cast<const uint32_t *>(data + nblocks*4);

      for(int i = -nblocks; i; i++)
      {
        uint32_t k1 = getblock(blocks, i);

        k1 *= c1;
        k1 = rotl32(k1, 15);
        k1 *= c2;

        h1 ^= k1;
        h1 = rotl32(h1, 13);
        h1 = h1*5+0xe6546b64;
      }

      //----------
      // tail

      const uint8_t *tail = reinterpret_cast<const uint8_t*>(data + nblocks*4);

      uint32_t k1 = 0;

      switch (len & 3)
      {
         case 3: k1 ^= tail[2] << 16;
         case 2: k1 ^= tail[1] << 8;
         case 1: k1 ^= tail[0];
                 k1 *= c1; k1 = rotl32(k1, 15); k1 *= c2; h1 ^= k1;
      }

      //----------
      // finalization

      h1 ^= len;

      return fmix(h1);
   }

   template <typename T>
   static uint32_t compute32(const T& key, uint32_t seed)
   {
      BOOST_STATIC_ASSERT(boost::is_pod<T>::value);
      return compute32(&key, sizeof(key), seed);
   }

   static uint32_t compute32(const std::string& key, uint32_t seed)
   {
      return compute32(key.data(), key.size(), seed);
   }

   template <typename T>
   static uint32_t compute32(const std::vector<T>& key, uint32_t seed)
   {
      BOOST_STATIC_ASSERT(boost::is_pod<T>::value);
      return compute32(&key[0], sizeof(T)*key.size(), seed);
   }

   template <typename T, unsigned Seed = 0U>
   struct hash32
   {
      size_t operator() (const T& key) const
      {
         return compute32(key, Seed);
      }
   };
};

}}

#endif


#include <boost/test/unit_test.hpp>
#include "algorithms/math/hamming_weight_of_array.hpp"

BOOST_AUTO_TEST_SUITE(TestHammingWeightOfArray)

BOOST_AUTO_TEST_CASE(zero_elements)
{
    HammingWeightOfArray::Solution solution;
    BOOST_CHECK(std::vector<int>({0}) == solution.countBits(0));
}

BOOST_AUTO_TEST_CASE(non_zero)
{
    HammingWeightOfArray::Solution solution;
    BOOST_CHECK(std::vector<int>({0,1,1,2,1,2}) == solution.countBits(5));
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef GRAPHS_NODE_HPP
#define GRAPHS_NODE_HPP

#include <boost/functional/hash.hpp>
#include <boost/serialization/access.hpp>

#include "utils.hpp"

namespace graphs {
struct Node {
    explicit Node() = default;

    explicit Node(std::uint64_t id)
        : m_id(id) {};

    Node(std::uint64_t id, Angle latitude, Angle longitude)
        : m_id(id)
        , m_latitude(latitude)
        , m_longitude(longitude) {};

    Node(std::uint64_t id, Location location)
        : m_id(id)
        , m_latitude(location.first)
        , m_longitude(location.second) {};

    [[nodiscard]] std::uint64_t id() const { return m_id; }
    [[nodiscard]] Angle latitude() const { return m_latitude; }
    [[nodiscard]] Angle longitude() const { return m_longitude; }
    [[nodiscard]] auto location() const -> Location { return { m_latitude, m_longitude }; }

    friend class boost::serialization::access;
    template<typename Archive>
    void serialize(Archive& archive, const unsigned int& version) {
        (void) version;
        archive & m_id;
        archive & m_latitude;
        archive & m_longitude;
    }

    bool operator==(const Node& other) const { return other.m_id == m_id; }
    bool operator!=(const Node& other) const { return !(other == *this); }
    bool operator<(const Node& other) const { return m_id < other.m_id; }

private:
    std::uint64_t m_id = 0;
    Angle m_latitude = 0;
    Angle m_longitude = 0;
};

using Nodes = std::vector<Node>;

/**
 * Factory function for Node.
 */
inline auto make_node(const osmium::NodeRef& node) -> Node {
    return { node.positive_ref(), node.lat(), node.lon() };
}
} // namespace graphs

namespace std {
template<>
struct hash<graphs::Node> {
    size_t operator()(const graphs::Node& n) const {
        using boost::hash_value;
        using boost::hash_combine;

        size_t seed = 0;
        hash_combine(seed, hash_value(n.id()));
        hash_combine(seed, hash_value(n.longitude()));
        hash_combine(seed, hash_value(n.latitude()));
        return seed;
    }
};
} // namespace std

#endif // GRAPHS_NODE_HPP


#include "catch.hpp"

#include <UnitConvert.hpp>
#include <UnitConvert/GlobalUnitRegistry.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/io.hpp>

namespace std
{
template <typename U, typename T>
istream& operator>>(istream& in, boost::units::quantity<U,T>& val)
{
  char unit_char_array[256];
  in.getline(unit_char_array,256);
  std::string unit_string(unit_char_array);

  val = UnitConvert::getGlobalUnitRegistry().makeQuantity<T>(unit_string).template to<boost::units::si::length>();

  return in;
}
}  // namespace std

TEST_CASE("Boost.PropertyTree Integration")
{
  using namespace boost::units;
  boost::property_tree::ptree tree;

  quantity<si::length> L(10 * si::meter);

  tree.put("Length",L);

  boost::property_tree::write_json(std::cout, tree);

  quantity<si::length,double> L2 = tree.get<quantity<si::length,double>>("Length");

  std::cout << L2;

  CHECK(L2.value() == Approx(10));
}


//
//  CCCpy.cpp
//  CCC
//
//  Created by Chris Kiefer on 06/10/2019.
//

/*
https://github.com/TNG/boost-python-examples
*/

#include <stdio.h>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include "ETC.hpp"
#include "CCC.hpp"

namespace np = boost::python::numpy;
using namespace boost::python;

char const* greet()
{
   return "Cheese!";
}

double ETC(np::ndarray const & ar) {
  int64_t* input_ptr = reinterpret_cast<int64_t*>(ar.get_data());
  ivec inputVec(input_ptr, ar.shape(0), 1, 1);
  return ETC::calc(inputVec);
}

double dynamicCC(np::ndarray const & ar, size_t dx, size_t xpast, size_t step) {
  int64_t* input_ptr = reinterpret_cast<int64_t*>(ar.get_data());
  ivec inputVec(input_ptr, ar.shape(0), 1, 1);
  return CCC::dynamicCC(inputVec, dx, xpast, step);
}

double dynamicCCJoint(np::ndarray const & ar, np::ndarray const & ar2, size_t dx, size_t past, size_t step) {
  int64_t* input_ptr = reinterpret_cast<int64_t*>(ar.get_data());
  ivec inputVec(input_ptr, ar.shape(0), 1, 1);
  int64_t* input_ptr2 = reinterpret_cast<int64_t*>(ar2.get_data());
  ivec inputVec2(input_ptr2, ar.shape(0), 1, 1);
  return CCC::dynamicCCJoint(inputVec, inputVec2, dx, past, step);
}

boost::python::tuple CCCausality(np::ndarray const & ar, np::ndarray const & ar2, size_t dx, size_t past, size_t step) {
  int64_t* input_ptr = reinterpret_cast<int64_t*>(ar.get_data());
  ivec inputVec(input_ptr, ar.shape(0), 1, 1);

  int64_t* input_ptr2 = reinterpret_cast<int64_t*>(ar2.get_data());
  ivec inputVec2(input_ptr2, ar2.shape(0), 1, 1);

  auto CCC = CCC::CCCausality(inputVec, inputVec2, dx, past, step);
  return boost::python::make_tuple(get<0>(CCC), get<1>(CCC));
}

BOOST_PYTHON_MODULE(cccrt)
{
    np::initialize();

    def("greet", greet);
    def("ETC", ETC);
    def("dynamicCC", dynamicCC);
    def("dynamicCCJoint", dynamicCCJoint);
    def("CCCausality", CCCausality);
}


//============================================================================
// Name        : LBFGSSolver.cpp
// Author      : Yaser
// Version     :
// Copyright   :
// Description : LBFGS MPI implementation
//============================================================================

#include "LBFGSSolver.h"
#include <boost/mpi.hpp>
#include <iomanip>

namespace mpi = boost::mpi;

//virtual void computeAndTakeStep();
//virtual double ();

LBFGSSolver::LBFGSSolver(ProblemData problemData, OptimizationParameters _optParams) :
						QuasiNewtonSolver(problemData, _optParams) {
	mpi::communicator world;
	stepSize = optParams.stepsize;
	lbfgsHistorySize = optParams.lbfgs_memory;
	m = optParams.m;
	globalN = optParams.n;
	localN = optParams.n / world.size();
	int localNTmp = localN;
	// We are going to split data by columns. Local N tells how many columns
	//	are stored locally.
	if (world.rank() == world.size() - 1) { // If optParam.n is not divisible by world.size we need to have few extra points on last node
		localN = optParams.n - (localN * (world.size() - 1));
	}
	Alocal.resize(localN * m);
	xLocalEven.resize(localN);
	xLocalOdd.resize(localN);
	cLocal.resize(localN * lbfgsHistorySize*2);
	preMatrixGlobal.resize((2*lbfgsHistorySize+1) * (2*lbfgsHistorySize+1));
	gradientLocalEven.resize(localN);
	gradientLocalOdd.resize(localN);
	xGlobalEven.resize(globalN);
	xGlobalOdd.resize(globalN);
	pK.resize(globalN);
	bLocal.resize(m);
	// Send data to other nodes
	if (world.rank() == 0) { // Sends data to others
/*
		for (int r = 0; r < m; r++) {
			for (int c = 0; c < globalN; c++) {
				cout << problemData.A[r + c * m] << " ";
			}
			cout << endl;
		}
*/
		for (int i = 1; i < world.size(); i++) {
			int nnz = localNTmp;
			if (i == world.size() - 1) {
				nnz = optParams.n - (localNTmp * (world.size() - 1));
			}
			world.send(i, 0, &problemData.A[(i) * localNTmp * m], nnz * m);

		}
		// Also copy local data
		for (int i = 0; i < localN * optParams.m; i++) {
			Alocal[i] = problemData.A[i];
		}
		for (int i = 0; i < m; i++) {
			bLocal[i] = problemData.b[i];
			cout << bLocal[i] << " ";
		}
		cout << endl;
		if (iter%2==0){
			for (int i = 0; i < globalN; i++) {
				xGlobalEven[i] = problemData.x[i];
			}
		} else {
			for (int i = 0; i < globalN; i++) {
				xGlobalOdd[i] = problemData.x[i];
			}
		}

	} else {
		// Receive data from root
		world.recv(0, 0, &Alocal[0], localN * m);

	}
	if (iter%2==0){
		this->sendXFromRootToNodes(xGlobalEven);
	} else {
		this->sendXFromRootToNodes(xGlobalOdd);
	}

}

void LBFGSSolver::sendXFromRootToNodes(std::vector<double> xIn) {

	mpi::communicator world;
	if (world.rank() == 0) { // Sends data to others
		for (int i = 1; i < world.size(); i++) {
			int nnz = localN;
			if (i == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.send(i, 1, &xIn[(i) * localN], nnz);

		}
		// Also copy local data
		if (iter%2==0){

			for (int i = 0; i < localN; i++) {
				xLocalEven[i] = xIn[i];
			}
		} else {
			for (int i = 0; i < localN; i++) {
				xLocalOdd[i] = xIn[i];
			}
		}

	} else {
		// Receive data from root
		if (iter%2==0){
			world.recv(0, 1, &xLocalEven[0], localN);
		} else {
			world.recv(0, 1, &xLocalOdd[0], localN);

		}
	}

}

std::vector<double> LBFGSSolver::computeResidualForCurrentX() { // computes Ax-b
	mpi::communicator world;
	std::vector<double> g(m, 0);

	if (iter%2==0){

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < m; j++) {
				g[j] += xLocalEven[i] * Alocal[i * m + j];
			}
		}
	} else {

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < m; j++) {
				g[j] += xLocalOdd[i] * Alocal[i * m + j];
			}
		}
	}

	std::vector<double> gTmp(m, 0);
	mpi::reduce(world, &g[0], m, &gTmp[0], std::plus<double>(), 0);

	if (world.rank() == 0) {
		for (int i = 0; i < m; i++) {
			gTmp[i] -= bLocal[i];
		}
	}
	return gTmp;
}

double LBFGSSolver::computeCurrentObjectiveValue() {
	double objVal = 0;
	// objVal = 1/2 * \|Ax - b\|^2
	// first I multiply  g = ALocal*xLocal
	std::vector<double> residual = this->computeResidualForCurrentX();
	mpi::communicator world;

	if (world.rank() == 0) {

		for (unsigned int i = 0; i < residual.size(); i++) {
			objVal += residual[i] * residual[i];
		}
	}
	return objVal / 2;
}
std::vector<double> LBFGSSolver::computeGradientForCurrentX() { // computes A'*(Ax-b)

	std::vector<double> residual = this->computeResidualForCurrentX();
	mpi::communicator world;
	if (world.rank() == 0) {
		//cout<<"residual: "<< residual[0]<< "  iter: "<<iter<<" xLocalEven: "<<xLocalEven[0]<<" xLocalOdd: "<<xLocalOdd[0]<<endl;
	}

	broadcast(world, &residual[0], residual.size(), 0);  // now residuals is the same on each node.

	// gradient = A'*residual
	if(iter%2 == 0){
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalEven[col] = 0;
			}
		}

		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalEven[col] += residual[r] * Alocal[r + col * m];
			}
		}
	} else {
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalOdd[col] = 0;
			}
		}
		for (int r = 0; r < m; r++) {
			for (int col = 0; col < localN; col++) {
				gradientLocalOdd[col] += residual[r] * Alocal[r + col * m];
			}
		}
	}

	// gather to a root note to store full gradient
	std::vector<double> gradient;
	if (world.rank() == 0) {
		gradient.resize(globalN);
		for (int from = 1; from < world.size(); from++) {
			int nnz = localN;
			if (from == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.recv(from, 0, &gradient[localN * from], nnz);

		}
		if(iter%2 == 0){

			for (int i = 0; i < localN; i++) {
				gradient[i] = gradientLocalEven[i];
			}
		} else {
			for (int i = 0; i < localN; i++) {
				gradient[i] = gradientLocalOdd[i];
			}
		}

	} else {
		// send data to ROOT
		if(iter%2 == 0){
			world.send(0, 0, &gradientLocalEven[0], gradientLocalEven.size());
		} else {
			world.send(0, 0, &gradientLocalOdd[0], gradientLocalOdd.size());
		}
	}

	return gradient;

}

std::vector<double> LBFGSSolver::computeMatrixPre() { // calculates the b.b' (here c.c') and returns precondition matrix of alg. 3
	mpi::communicator world;
	// First time to enter this function, iter=0
	if (iter>0){
		int mod1 = (iter-1) % lbfgsHistorySize;

		int mod2 = (iter-1) % lbfgsHistorySize + lbfgsHistorySize;

		std::vector<double> preMatUpdateLocal(2*lbfgsHistorySize+1, 0);

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < 2*lbfgsHistorySize; j++) {
				preMatUpdateLocal[j] += cLocal[localN*mod1+ i] * cLocal[j * localN + i];
			}
		}

		//std::vector<double> gTmp(2*lbfgsHistorySize, 0); ////// why?
		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize,
				&preMatrixGlobal[mod1*(2*lbfgsHistorySize+1)], std::plus<double>(), 0);

		// mod1*(2*lbfgsHistorySize) is the first element in the column 3
		std::fill(preMatUpdateLocal.begin(), preMatUpdateLocal.end(), 0);     //
		if (world.rank() == 0) {    //
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[mod1 + i * (2*lbfgsHistorySize+1) ]	=
						preMatrixGlobal[mod1*(2*lbfgsHistorySize+1)+i];
			}
		}

		for (int i = 0; i < localN; i++) {
			for (int j = 0; j < 2*lbfgsHistorySize; j++) {
				preMatUpdateLocal[j] += cLocal[localN*mod2+ i] * cLocal[j * localN + i];
			}
		}

		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize, &preMatrixGlobal[mod2*(2*lbfgsHistorySize+1)], std::plus<double>(), 0);
		std::fill(preMatUpdateLocal.begin(), preMatUpdateLocal.end(), 0);     //
		// mod2(?)*(2*lbfgsHistorySize) is the first element in the column 3

		if (world.rank() == 0) {    ////////////////////////////////
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[mod2 + i * (2*lbfgsHistorySize +1)]	= 	preMatrixGlobal[mod2*(2*lbfgsHistorySize+1)+i];
			}
		}

		if (iter%2 == 0){
//			cout<<"grad @ even= [";
			for (int i = 0; i < localN; i++) {
				for (int j = 0; j < 2*lbfgsHistorySize; j++) {
					preMatUpdateLocal[j] += gradientLocalEven[i] * cLocal[j * localN + i];
				}
				preMatUpdateLocal[2*lbfgsHistorySize] += gradientLocalEven[i]*gradientLocalEven[i];
//				cout<<gradientLocalOdd[i]<<"   "<<gradientLocalEven[i];

			}
//			cout<<endl;
//			cout<<"gradNorm @ even= "<<preMatUpdateLocal[2*lbfgsHistorySize]<<endl;

		}
		else {
//			cout<<"grad @ odd= [";
			for (int i = 0; i < localN; i++) {
				for (int j = 0; j < 2*lbfgsHistorySize; j++) {
					preMatUpdateLocal[j] += gradientLocalOdd[i] * cLocal[j * localN + i];
				}
				preMatUpdateLocal[2*lbfgsHistorySize] += gradientLocalOdd[i]*gradientLocalOdd[i];

			}
		}

		//std::vector<double> gTmp(2*lbfgsHistorySize, 0);  ////// why?
		mpi::reduce(world, &preMatUpdateLocal[0], 2*lbfgsHistorySize+1,
				&preMatrixGlobal[(2*lbfgsHistorySize+1)*(2*lbfgsHistorySize)], std::plus<double>(), 0);
		// mod1*(2*lbfgsHistorySize) is the first element in the column 3

		if (world.rank() == 0) {    ////////////////////////////////
			for (int i = 0; i < 2*lbfgsHistorySize; i++) {
				preMatrixGlobal[2*lbfgsHistorySize + i * (2*lbfgsHistorySize+1) ]	=
						preMatrixGlobal[(2*lbfgsHistorySize+1)*(2*lbfgsHistorySize)+i];
			}
		}
	}
	return preMatrixGlobal;

}

void LBFGSSolver::findPk() { // finds p_k
	std::vector<double> preMatrix = this->computeMatrixPre();
	mpi::communicator world;

	std::vector<double> delta(2*lbfgsHistorySize+1, 0);

	if (world.rank() == 0) {

		std::vector<double> alpha(lbfgsHistorySize, 0);
		delta[2*lbfgsHistorySize] = -1;
		int last_i = max(iter - lbfgsHistorySize, 0);
		// first loop

//		cout<<"first loop: prematrix access"<<endl;
		for (int i = iter - 1; i >= last_i; i--) {
//			int j = i - (iter-lbfgsHistorySize) + 1;
			int j = i%lbfgsHistorySize;

			for (int l = 0; l<2*lbfgsHistorySize+1;l++){
				alpha[j] += preMatrix[(2*lbfgsHistorySize+1)*j+l]*delta[l];
//				cout<<"[j,l] = ["<<j<<","<<l<<"]   ";

			}
			alpha[j] = alpha[j]/preMatrix[j*(2*lbfgsHistorySize+1)+(lbfgsHistorySize+j)];
//			cout<<"\ns[j,m+j] = ["<<j<<","<<lbfgsHistorySize+j<<"]   ";
			delta[lbfgsHistorySize+j] -=  alpha[j];

//			cout<<endl<<"next mem";

		}

		double scalarMultiplier = preMatrix[((iter - 1)%lbfgsHistorySize)*(2*lbfgsHistorySize+1)+
		                                    (iter - 1)%lbfgsHistorySize+lbfgsHistorySize]/preMatrix[((iter - 1)%lbfgsHistorySize+lbfgsHistorySize)*(2*lbfgsHistorySize+1)+((iter - 1)%lbfgsHistorySize+lbfgsHistorySize)];

		for (int i = 0; i<2*lbfgsHistorySize+1; i++) {
			delta[i] *= scalarMultiplier;
		}

		// second loop
		double bbeta = 0;
//		cout<<"second loop: prematrix access"<<endl;
		int first_i = max(0, iter - lbfgsHistorySize);
		for (int i = first_i; i < iter; i++) {
			bbeta = 0;
//			int j = i - (iter-lbfgsHistorySize) + 1;
			int j = i%lbfgsHistorySize;

			for (int l = 0; l<2*lbfgsHistorySize+1;l++){
				bbeta += preMatrix[(2*lbfgsHistorySize+1)*(lbfgsHistorySize+j)+l]*delta[l];
			}
			bbeta =  bbeta /preMatrix[j*(2*lbfgsHistorySize+1)+(lbfgsHistorySize+j)];
			delta[j] +=  alpha[j]-bbeta;

		}

	}
	broadcast(world, &delta[0], delta.size(), 0);  // now delta is the same on all nodes.
	std::vector<double> pLocal (localN);
	for (int i = 0; i < localN ; i++){
		for (int j = 0; j< 2*lbfgsHistorySize ; j++){
			pLocal[i] += delta[j] * cLocal[j*localN+i];
		}
	}

	if (iter%2 == 0){
		for (int i = 0; i < localN ; i++){
			pLocal[i] += delta[2*lbfgsHistorySize] * gradientLocalEven[i];
		}
	} else {
		for (int i = 0; i < localN ; i++){
			pLocal[i] += delta[2*lbfgsHistorySize] * gradientLocalOdd[i];
		}
	}

//	std::vector<double> pK ;

	if (world.rank() == 0) {
//		pK.resize(globalN);
		for (int from = 1; from < world.size(); from++) {
			int nnz = localN;
			if (from == world.size() - 1) {
				nnz = globalN - (localN * (world.size() - 1));
			}
			world.recv(from, 0, &pK[localN * from], nnz);

		}

		for (int i = 0; i < localN; i++) {
			pK[i] = pLocal[i];
		}

	} else {
		// send data to ROOT
		world.send(0, 0, &pLocal[0], localN);
		if (world.rank()==1){
//			cout<<endl<<endl<<endl<<"pLocal on node "<<world.rank()<<" is:   "<<pLocal[0]<<endl;
		}

	}

}

void LBFGSSolver::updateYk() { // finds y_k after each iteration
	mpi::communicator world;
	int modd = iter % lbfgsHistorySize;
	if (iter%2 == 0){
		for(int i =0 ; i< localN; i++){
			cLocal[(lbfgsHistorySize+modd)*localN + i] = gradientLocalOdd[i] - gradientLocalEven[i];
		}
	} else {
		for(int i =0 ; i< localN; i++){
			cLocal[(lbfgsHistorySize+modd)*localN + i] = gradientLocalEven[i] - gradientLocalOdd[i];
		}
	}
}

void LBFGSSolver::updateSk() { // finds s_k after each iteration
	mpi::communicator world;
	// First time to enter this function, iter=0
	int modd = iter % lbfgsHistorySize;
	if (iter%2 == 0){
		for(int i =0 ; i< localN; i++){
			cLocal[modd*localN + i] = xLocalOdd[i] - xLocalEven[i];	// bb local is stack of y_k, s_k and grad_k
		}
	} else {
		for(int i =0 ; i< localN; i++){
			cLocal[modd*localN + i] = xLocalEven[i] - xLocalOdd[i];
		}
	}
}

void LBFGSSolver::computeAndTakeStep() {
	mpi::communicator world;

	if(iter == 0){
		gradientEven = this->computeGradientForCurrentX();
	}
	/*        two loop recursion	 */
	if (iter>=0){
		this->findPk();
	}

	/*      Take the step and update s_k and y_k   */

	if(iter%2 == 0){

		if (world.rank() == 0) {
			for (int i = 0; i < globalN; i++) {
				if (iter==0){
					xGlobalOdd[i] = xGlobalEven[i] - stepSize * gradientEven[i];
				}else{
					xGlobalOdd[i] = xGlobalEven[i] + stepSize * pK[i];
				}
			}
			//cout<<endl<<"gradientEven: "<< gradientEven[0]<< "  iter: "<<iter<<" xGlobal even cur: "<<xGlobalEven[0]<<" xGlobal odd next: "<<xGlobalOdd[0]<<endl;
		}

		iter +=1;
		this->sendXFromRootToNodes(xGlobalOdd);

		iter -=1;
		this->updateSk();
		iter +=1;

		gradientOdd = this->computeGradientForCurrentX();

		iter -=1;
		this->updateYk();
		iter +=1;

	} else {
		if (world.rank() == 0) {

			for (int i = 0; i < globalN; i++) {
				if (iter==0){
					xGlobalEven[i] = xGlobalOdd[i]- stepSize * gradientOdd[i];
				}else{
					xGlobalEven[i] = xGlobalOdd[i]+ stepSize * pK[i];
				}
			}

		}

		iter +=1;
		this->sendXFromRootToNodes(xGlobalEven);

		iter -=1;
		this->updateSk();
		iter +=1;
		gradientEven = this->computeGradientForCurrentX();
		iter -=1;
		this->updateYk();
		iter +=1;

	}

}


#pragma once

#include <Eigen/Dense>

#include "aux/eigen2hdf.hpp"
#include "grid/grid_tools.hpp"

namespace boltzmann {

/**
 * @brief Write mesh to hdf5
 *
 * @param dh DoF handler
 *
 */
template <typename DOFHANDLER>
void
export_mesh(const DOFHANDLER& dh, const char* fname = "mesh.hdf5")
{
  auto d2v = dof_to_vertex_index(dh);
  auto v2d = vertex_to_dof_index(dh);
  const auto& vertices = dh.get_triangulation().get_vertices();
  unsigned int L = vertices.size();
  const unsigned int DIM = DOFHANDLER::dimension;
  typedef Eigen::Matrix<double, Eigen::Dynamic, DIM> point_array_t;

  point_array_t X(L, DIM);

  // didx: dof index
  for (unsigned int didx = 0; didx < dh.n_dofs(); ++didx) {
    auto point = vertices[d2v[didx]];
    for (unsigned int d = 0; d < DIM; ++d) {
      X(didx, d) = point[d];
    }
  }

  const unsigned int dofs_per_cell = dh.get_fe().dofs_per_cell;
  typedef Eigen::Matrix<unsigned int, Eigen::Dynamic, Eigen::Dynamic> cell_array_t;
  cell_array_t M(dh.get_triangulation().n_active_quads(), dofs_per_cell);

  auto& tria = dh.get_triangulation();

  std::vector<unsigned int> ccw = {0, 1, 3, 2};

  unsigned int i = 0;
  for (auto cell : tria.active_cell_iterators()) {
    int counter = 0;
    for (auto j : ccw) {
      M(i, counter++) = v2d[cell->vertex_index(j)];
    }
    i++;
  }

  hid_t fh5 = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  eigen2hdf::save(fh5, "nodes", X);
  eigen2hdf::save(fh5, "cells", M);

  H5Fclose(fh5);
}

}  // boltzmann


// The MIT License (MIT)
//
// Copyright (c) 2015 Markus Herb
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
#ifndef KALMAN_MATRIX_HPP_
#define KALMAN_MATRIX_HPP_

#include <cmath>

#include <Eigen/Dense>

#define KALMAN_VECTOR(NAME, T, N)                                                       \
    typedef Kalman::Vector<T, N> Base;                                                  \
    using typename Base::Scalar;                                                        \
    using Base::RowsAtCompileTime;                                                      \
    using Base::ColsAtCompileTime;                                                      \
    using Base::SizeAtCompileTime;                                                      \
                                                                                \
    NAME(void) : Kalman::Vector<T, N>() {}                                              \
                                                                                \
    template<typename OtherDerived>                                                     \
    NAME(const Eigen::MatrixBase<OtherDerived>& other) : Kalman::Vector<T, N>(other) {} \
                                                                                \
    template<typename OtherDerived>                                                     \
    NAME& operator= (const Eigen::MatrixBase <OtherDerived>& other)                     \
    {                                                                                \
        this->Base::operator=(other);                                                   \
        return *this;                                                                   \
    }

namespace Kalman {
    const int Dynamic = Eigen::Dynamic;

    /**
     * @class Kalman::Matrix
     * @brief Template type for matrices
     * @param T The numeric scalar type
     * @param rows The number of rows
     * @param cols The number of columns
     */
    template<typename T, int rows, int cols>
    using Matrix = Eigen::Matrix<T, rows, cols>;

    /**
     * @brief Template type for vectors
     * @param T The numeric scalar type
     * @param N The vector dimension
     */
    template<typename T, int N>
    class Vector : public Matrix<T, N, 1>
    {
    public:
        //! Matrix base type
        typedef Matrix<T, N, 1> Base;

        using typename Base::Scalar;
        using Base::RowsAtCompileTime;
        using Base::ColsAtCompileTime;
        using Base::SizeAtCompileTime;

        Vector(void) : Matrix<T, N, 1>() {}

        /**
         * @brief Copy constructor
         */
        template<typename OtherDerived>
        Vector(const Eigen::MatrixBase<OtherDerived>& other)
            : Matrix<T, N, 1>(other)
        { }
        /**
         * @brief Copy assignment constructor
         */
        template<typename OtherDerived>
        Vector& operator= (const Eigen::MatrixBase <OtherDerived>& other)
        {
            this->Base::operator=(other);
            return *this;
        }
    };

    /**
     * @brief Cholesky square root decomposition of a symmetric positive-definite matrix
     * @param _MatrixType The matrix type
     * @param _UpLo Square root form (Eigen::Lower or Eigen::Upper)
     */
    template<typename _MatrixType, int _UpLo = Eigen::Lower>
    class Cholesky : public Eigen::LLT< _MatrixType, _UpLo >
    {
    public:
        Cholesky() : Eigen::LLT< _MatrixType, _UpLo >() {}

        /**
         * @brief Construct cholesky square root decomposition from matrix
         * @param m The matrix to be decomposed
         */
        Cholesky(const _MatrixType& m ) : Eigen::LLT< _MatrixType, _UpLo >(m) {}

        /**
         * @brief Set decomposition to identity
         */
        Cholesky& setIdentity()
        {
            this->m_matrix.setIdentity();
            this->m_isInitialized = true;
            return *this;
        }

        /**
         * @brief Check whether the decomposed matrix is the identity matrix
         */
        bool isIdentity() const
        {
            eigen_assert(this->m_isInitialized && "LLT is not initialized.");
            return this->m_matrix.isIdentity();
        }

        /**
         * @brief Set lower triangular part of the decomposition
         * @param matrix The lower part stored in a full matrix
         */
        template<typename Derived>
        Cholesky& setL(const Eigen::MatrixBase <Derived>& matrix)
        {
            this->m_matrix = matrix.template triangularView<Eigen::Lower>();
            this->m_isInitialized = true;
            return *this;
        }

        /**
         * @brief Set upper triangular part of the decomposition
         * @param matrix The upper part stored in a full matrix
         */
        template<typename Derived>
        Cholesky& setU(const Eigen::MatrixBase <Derived>& matrix)
        {
            this->m_matrix = matrix.template triangularView<Eigen::Upper>().adjoint();
            this->m_isInitialized = true;
            return *this;
        }
    };
}

#endif


#include "globalregression.h"
#include <iostream>
#include <fstream>
#include <boost/lexical_cast.hpp>
#include <stdlib.h>

#define Malloc(type,n) (type *)malloc((n)*sizeof(type))

void print_null(const char *s) {}

cRegression::cRegression(sTrainData* data, sParams* params)
{
	m_TrainData = data;
	m_Params = params;
	m_Evaluate = new cEvaluate;
}

cv::Mat_<double> cRegression::GlobalRegression(const cv::Mat_<int>&binaryfeatures, int stage)
{
	cv::Mat_<double> deltashapes = cv::Mat::zeros(m_TrainData->__dbsizes, 2 * m_TrainData->__numlandmarks, CV_64FC1);
	std::cout << "compute residual." << std::endl;
	for (int i = 0;i < m_TrainData->__dbsizes; i++){
		cv::Mat_<double> residua_col0 = m_TrainData->__data[i].__shapes_residual.col(0).t();
		cv::Mat_<double> residua_col1 = m_TrainData->__data[i].__shapes_residual.col(1).t();

		residua_col0.copyTo(deltashapes.row(i).colRange(0, m_TrainData->__numlandmarks));
		residua_col1.copyTo(deltashapes.row(i).colRange(m_TrainData->__numlandmarks, 2 * m_TrainData->__numlandmarks));
	}
	std::cout << "compute residual finished." << std::endl;

	std::cout << "linear regression." << std::endl;
	cv::Mat_<double> W_liblinear = cv::Mat::zeros(binaryfeatures.cols, deltashapes.cols,CV_64FC1);
	// std::cout << "convert to sparse feature." << std::endl;
	cv::SparseMat_<int> binaryfeatures_sparse(binaryfeatures);
	// std::cout << binaryfeatures_sparse.size(1) << std::endl;
	// std::cout << "convert to sparse feature finished." << std::endl;
    #pragma omp parallel for
	for (int i = 0; i < deltashapes.cols; i++){
		cv::Mat_<double> tmp = __train_regressor(deltashapes.col(i), binaryfeatures_sparse);
		tmp.copyTo(W_liblinear.col(i));
	}
	std::cout << "linear regression finished." << std::endl;

	cv::Mat_<double> binaryfeatures_d;
	binaryfeatures.convertTo(binaryfeatures_d,CV_64FC1);
	cv::Mat_<double> deltashapes_bar = binaryfeatures_d * W_liblinear;

	cv::Mat_<double> deltashapes_bar_x = deltashapes_bar.colRange(0, deltashapes_bar.cols/2);//54 X 68
	cv::Mat_<double> deltashapes_bar_y = deltashapes_bar.colRange(deltashapes_bar.cols / 2, deltashapes_bar.cols);

	std::vector<cv::Mat_<double>> preshapes(0);
	std::vector<cv::Mat_<double>> gtshapes(0);

	std::cout << "update stage." << std::endl;
	for (int i=0; i < m_TrainData->__dbsizes; i++){
		cv::Mat_<double> delta_shape_interm_coord = cv::Mat::zeros(deltashapes_bar_x.cols,2,CV_64FC1);//68 X 2
		cv::Mat_<double> deltashapes_bar_x_t, deltashapes_bar_y_t;

		deltashapes_bar_x_t = deltashapes_bar_x.t();
		deltashapes_bar_y_t = deltashapes_bar_y.t();

		deltashapes_bar_x_t.col(i).copyTo(delta_shape_interm_coord.col(0));
		deltashapes_bar_y_t.col(i).copyTo(delta_shape_interm_coord.col(1));

		delta_shape_interm_coord.col(0) *= m_TrainData->__data[i].__intermediate_bboxes[stage].width;
		delta_shape_interm_coord.col(1) *= m_TrainData->__data[i].__intermediate_bboxes[stage].height;

		cv::Mat_<double> shape_newstage = m_TrainData->__data[i].__intermediate_shapes[stage] + delta_shape_interm_coord;

		m_TrainData->__data[i].__intermediate_shapes[stage + 1] = shape_newstage.clone();
		m_TrainData->__data[i].__intermediate_bboxes[stage + 1] = m_TrainData->__data[i].__intermediate_bboxes[stage];

		preshapes.push_back(m_TrainData->__data[i].__intermediate_shapes[stage + 1].clone());
		gtshapes.push_back(m_TrainData->__data[i].__shape_gt.clone());

		/*
		cv::Mat img = m_TrainData->__data[i].__img_gray.clone();
		for (int k = 0; k < preshapes[i].rows; ++k) {
			cv::circle(img, cv::Point(m_TrainData->__data[i].__intermediate_shapes[stage](k, 0),
				m_TrainData->__data[i].__intermediate_shapes[stage](k, 1)),
				1, CV_RGB(128, 128, 128), 1);
			cv::circle(img, cv::Point(preshapes[i](k, 0), preshapes[i](k, 1)), 1, CV_RGB(255, 255, 255), 1);
			cv::circle(img, cv::Point(gtshapes[i](k, 0), gtshapes[i](k, 1)), 1, CV_RGB(0, 0, 0), 1);
		}
		cv::rectangle(img, m_TrainData->__data[i].__bbox_gt, CV_RGB(255, 255, 255), 1);
		cv::imshow("img", img);
		cv::waitKey(0);
		*/

		//calculate transform
		/*cv::Mat_<double> shape_newstage_x = shape_newstage.col(0);
		cv::Mat_<double> shape_newstage_y = shape_newstage.col(1);
		cv::Scalar cx = cv::mean(shape_newstage_x);
		cv::Scalar cy = cv::mean(shape_newstage_y);
		shape_newstage_x = shape_newstage_x - cx[0];
		shape_newstage_y = shape_newstage_y - cy[0];

		cv::Rect         bbox   = m_TrainData->__data[i].__intermediate_bboxes[stage + 1];
		cv::Mat_<double> mean = m_Utils.Reshape(m_TrainData->__meanface_one_row, bbox, m_Params);
		cv::Mat_<double> mean_x = mean.colRange(0, mean.cols / 2).t();
		cv::Mat_<double> mean_y = mean.colRange(mean.cols / 2, mean.cols).t();

		cv::Scalar mx = cv::mean(mean_x);
		cv::Scalar my = cv::mean(mean_y);
		mean_x = mean_x - mx[0];
		mean_y = mean_y - my[0];
		cv::Mat_<double> mean_newstage = cv::Mat::zeros(mean_x.rows,2,CV_64FC1);
		mean_x.copyTo(mean_newstage.col(0));
		mean_y.copyTo(mean_newstage.col(1));

		m_TrainData->__data[i].__tf2meanshape = m_Utils.Calc_Point_Transform(shape_newstage, mean_newstage);
		m_TrainData->__data[i].__meanshape2tf = m_Utils.Calc_Point_Transform(mean_newstage, shape_newstage);*/

		cv::Mat_<double> residual_col0, residual_col1, residual;
		residual = m_TrainData->__data[i].__shape_gt - shape_newstage;

		residual.col(0) = residual.col(0) / (double)m_TrainData->__data[i].__intermediate_bboxes[stage + 1].width;
		residual.col(1) = residual.col(1) / (double)m_TrainData->__data[i].__intermediate_bboxes[stage + 1].height;

		//residual.col(0) = residual.col(0) * m_TrainData->__data[i].__tf2meanshape.at<double>(0, 0) + residual.col(1) * m_TrainData->__data[i].__tf2meanshape.at<double>(0, 1) + m_TrainData->__data[i].__tf2meanshape.at<double>(0, 2);
		//residual.col(1) = residual.col(1) * m_TrainData->__data[i].__tf2meanshape.at<double>(1, 0) + residual.col(1) * m_TrainData->__data[i].__tf2meanshape.at<double>(1, 1) + m_TrainData->__data[i].__tf2meanshape.at<double>(1, 2);

		residual.copyTo(m_TrainData->__data[i].__shapes_residual);
	}
	std::wcout << "update stage finished." << std::endl;

	std::cout << "Mean Square Root Error: " << m_Evaluate->compute_error(preshapes, gtshapes) << std::endl;

	// std::cout << "regression stage finished." << std::endl;
	return W_liblinear;
}

cv::Mat_<double> cRegression::__train_regressor(const cv::Mat_<double>& label_vec, const cv::SparseMat_<int>& instance_mat)
{
	void(*print_func)(const char*) = &print_null;
	const char *error_msg;

	struct parameter param;
	struct problem   problem;
	struct feature_node *x_space = NULL;

	srand(1);
	// std::cout << "initialize liblinear parameter." << std::endl;
	param.solver_type = L2R_L2LOSS_SVR_DUAL;
	param.C = 1.0 / (double)label_vec.rows;
	param.eps = 0.1;
	param.p = 0;
	param.nr_weight = 0;
	param.weight_label = NULL;
	param.weight = NULL;
	// std::cout << "initialize liblinear parameter finished." << std::endl;
	set_print_string_function(print_func);

	std::vector<int>*  prob_x = NULL;
	prob_x = new std::vector<int>[label_vec.rows]; // number of samples = label_vec.rows

	cv::SparseMatConstIterator_<int> it, end;
	// std::cout << "copy feature." << std::endl;
	for (it = instance_mat.begin(), end = instance_mat.end(); it != end; ++it){
		const cv::SparseMat::Node* node = it.node();
		prob_x[node->idx[0]].push_back(node->idx[1]);
	}
	// std::cout << "copy feature finished." << std::endl;

	//sort the vector
	for (int i = 0; i < label_vec.rows; i++){
		std::sort(prob_x[i].begin(), prob_x[i].end());
	}

	problem.l = label_vec.rows;
	problem.n = instance_mat.size(1);
	problem.bias = -1;

	size_t nzcount = instance_mat.nzcount();
	int elements = (int)(nzcount + problem.l);

	problem.y = Malloc(double, problem.l);
	problem.x = Malloc(struct feature_node *, problem.l);
	x_space   = Malloc(struct feature_node, elements);

	int j = 0;
	for (int i = 0; i < problem.l; i++){
		problem.y[i] = label_vec(i, 0);
		problem.x[i] = &x_space[j];

		for (int k = 0; k < prob_x[i].size(); k++){
			x_space[j].index = prob_x[i][k] + 1;
			x_space[j].value = 1;
			j++;
		}
		x_space[j++].index = -1;
	}

	delete[] prob_x;

	error_msg = check_parameter(&problem, &param);
	if (error_msg){
		fprintf(stderr, "ERROR: %s\n", error_msg);
	}

	// std::cout << "train model." << std::endl;
	struct model *model = NULL;
	model = train(&problem, &param);
	// std::cout << "train model finished." << std::endl;

	cv::Mat_<double> weight = cv::Mat::zeros(model->nr_feature, 1, CV_64FC1);
	for (int i = 0; i < model->nr_feature; i++){
		weight(i, 0) = model->w[i];
	}

	free_and_destroy_model(&model);
	destroy_param(&param);

	free((void*)(problem.y));
	free((void*)(problem.x));
	free((void*)(x_space));
	return weight;
}

cv::Mat_<double> cRegression::__train_regressor(const cv::Mat_<double>& label_vec, const cv::Mat_<int>& instance_mat)
{
	void(*print_func)(const char*) = &print_null;
	const char *error_msg;

	struct parameter param;
	struct problem   problem;
	struct feature_node *x_space = NULL;

	srand(1);
	// std::cout << "initialize liblinear parameter." << std::endl;
	param.solver_type = L2R_L2LOSS_SVR_DUAL;
	param.C = 1.0 / (double)label_vec.rows;
	param.eps = 0.1;
	param.p = 0;
	param.nr_weight = 0;
	param.weight_label = NULL;
	param.weight = NULL;
	// std::cout << "initialize liblinear parameter finished." << std::endl;
	set_print_string_function(print_func);

	std::vector<int>*  prob_x = NULL;
	prob_x = new std::vector<int>[label_vec.rows]; // number of samples = label_vec.rows

	size_t nzcount = 0;
	// std::cout << "copy feature." << std::endl;
	for (int i = 0; i < instance_mat.rows; ++i) {
	    for (int j = 0; j < instance_mat.cols; ++j) {
		    int elem = instance_mat(i, j);
			if (elem != 0) {
				prob_x[i].push_back(j);
				++nzcount;
			}
		}
	}
	// std::cout << "copy feature finished." << std::endl;

	//sort the vector
	for (int i = 0; i < label_vec.rows; i++){
		std::sort(prob_x[i].begin(), prob_x[i].end());
	}

	problem.l = label_vec.rows;
	problem.n = instance_mat.cols;
	problem.bias = -1;

	int elements = (int)(nzcount + problem.l);

	problem.y = Malloc(double, problem.l);
	problem.x = Malloc(struct feature_node *, problem.l);
	x_space = Malloc(struct feature_node, elements);

	int j = 0;
	for (int i = 0; i < problem.l; i++){
		problem.y[i] = label_vec(i, 0);
		problem.x[i] = &x_space[j];

		for (int k = 0; k < prob_x[i].size(); k++){
			x_space[j].index = prob_x[i][k] + 1;
			x_space[j].value = 1;
			j++;
		}
		x_space[j++].index = -1;
	}

	delete[] prob_x;

	error_msg = check_parameter(&problem, &param);
	if (error_msg){
		fprintf(stderr, "ERROR: %s\n", error_msg);
	}

	// std::cout << "train model." << std::endl;
	struct model *model = NULL;
	model = train(&problem, &param);
	// std::cout << "train model finished." << std::endl;

	cv::Mat_<double> weight = cv::Mat::zeros(model->nr_feature, 1, CV_64FC1);
	for (int i = 0; i < model->nr_feature; i++){
		weight(i, 0) = model->w[i];
		// std::cout << weight(i, 0) << " "; // std::endl;
	}

	free_and_destroy_model(&model);
	destroy_param(&param);

	free((void*)(problem.y));
	free((void*)(problem.x));
	free((void*)(x_space));
	return weight;
}


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

void GridFiller::fillGrid(Grid* grid, const string fileName){

    cube* gr = grid->getRealGrid();
    const size_t dimX = gr->n_rows;
    const size_t dimY = gr->n_cols;
    const size_t dimZ = gr->n_slices;

    const size_t totElem = dimX*dimY*dimZ;

    size_t c = 0;

    string line;
    ifstream file(fileName);
    if (file.is_open()) {
        while (getline(file, line)) {
            const vector<string> spl = StringUtils::split(line);
            for(size_t x = 0; x < spl.size(); ++x){
#ifdef __GNUC__
                stringstream ss(line);
                double d;
                ss >> d;
                gr->at(c) = d;
#else
                gr->at(c) = stod(spl[x]);
#endif
                ++c;
            }
        }
        file.close();
    } else {
        throw runtime_error("Unable to open file " + fileName + " for reading grid in!");
    }

    if(c != totElem){
        cerr << "total elements " << totElem << endl;
        cerr << "read elements " << c << endl;
        throw runtime_error("Not the right number of elements got read.");
    }

    grid->complete(gr);
}

void GridFiller::fillEmptyGrid(Grid* grid){

    cube* gr = grid->getRealGrid();
    gr->fill(0.0);
    grid->complete(gr);
}

void GridFiller::fillGridRandomly(Grid* grid){

    cube* gr = grid->getRealGrid();
    gr->randu();
    grid->complete(gr);
}


/*
 * transformation.hpp
 *
 *  Created on: Apr 24, 2012
 *      Author: wjiang2
 */

#ifndef TRANSFORMATION_HPP_
#define TRANSFORMATION_HPP_

#define CALTBL 0
#define LOG 1
#define LIN 2
#define FLIN 3
#define FASINH 4
//#define LOGICLE 1
#define BIEXP 5

#include <map>
#include <string>
#include <vector>
#include <stdexcept>
#include "calibrationTable.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/compare.hpp>
#include <boost/regex.hpp>

#include "global.hpp"

using namespace std;

struct coordinate
{
//	friend class boost::serialization::access;

	double x,y;
	coordinate(double _x,double _y){x=_x;y=_y;};
	coordinate(){};
	void convertToPb(pb::coordinate & coor_pb){
		coor_pb.set_x(x);
		coor_pb.set_y(y);
	};
	coordinate(const pb::coordinate & coor_pb):x(coor_pb.x()),y(coor_pb.y()){};
};
bool compare_x(coordinate i, coordinate j);
bool compare_y(coordinate i, coordinate j);
//typedef map<string,double> ARGLIST;

class transformation{
protected:
	calibrationTable calTbl; //no longer have to store calTbl since we now can compute it from biexp even for mac workspace
	bool isGateOnly;
	unsigned short type;//could have been avoided if it is not required by R API getTransformation that needs to extract concrete transformation
	string name;
	string channel;
	bool isComputed;//this flag allow lazy computCalTbl/interpolation
public:
	transformation();
	transformation(bool _isGate,unsigned short _type);
	virtual ~transformation(){};
	virtual void transforming(valarray<double> & input);
	virtual void computCalTbl(){};//dummy routine that does nothing
	virtual Spline_Coefs getSplineCoefs(){return calTbl.getSplineCoefs();};
	virtual void setCalTbl(calibrationTable _tbl);
	virtual calibrationTable getCalTbl(){return calTbl;};
	virtual void interpolate(){calTbl.interpolate();};
	virtual bool isInterpolated(){return calTbl.isInterpolated();}
	virtual bool gateOnly(){return isGateOnly;};
	virtual void setGateOnlyFlag(bool _flag){isGateOnly=_flag;};
	virtual bool computed(){return isComputed;};
	virtual void setComputeFlag(bool _flag){isComputed=_flag;};
	virtual string getName(){return name;};
	virtual void setName(string _name){name=_name;};
	virtual string getChannel(){return channel;};
	virtual void setChannel(string _channel){channel=_channel;};
	virtual unsigned short getType(){return type;};
	virtual void setType(unsigned short _type){type=_type;};
	virtual transformation * clone(){return new transformation(*this);};
	virtual void convertToPb(pb::transformation & trans_pb);
	transformation(const pb::transformation & trans_pb);
	virtual boost::shared_ptr<transformation> getInverseTransformation();
	virtual void setTransformedScale(int scale){throw(domain_error("setTransformedScale function not defined!"));};
	virtual int getTransformedScale(){throw(domain_error("getTransformedScale function not defined!"));};
	virtual int getRawScale(){throw(domain_error("getRawScale function not defined!"));};
};
/* case insensitive compare predicate*/
struct ciLessBoost : std::binary_function<std::string, std::string, bool>
{
    bool operator() (const std::string & s1, const std::string & s2) const {
        return lexicographical_compare(s1, s2, boost::is_iless());
    }
};

typedef map<string,transformation *, ciLessBoost> trans_map;/* we always do case-insensitive searching for transformation lookup
due to some of channel name discrepancies occured in flowJo workspaces*/
typedef std::map<std::string, std::string, ciLessBoost> CHANNEL_MAP;
struct PARAM{
		string param;
		bool log;
		unsigned range;
		unsigned highValue;
		unsigned calibrationIndex;
		//EDIT: can't trust this info from xml
//		double timestep;//only meaningful for time channel which is used to scale time channel (only for data, not for gates since gates are already stored at scaled value)
		PARAM(){};
		void updateChannels(const CHANNEL_MAP & chnl_map){
			CHANNEL_MAP::const_iterator itChnl = chnl_map.find(param);
			if(itChnl!=chnl_map.end())
				param = itChnl->second;
		};
		PARAM(const pb::PARAM & param_pb){
			param = param_pb.param();
			log = param_pb.log();
			range = param_pb.range();
			highValue = param_pb.highvalue();
			calibrationIndex = param_pb.calibrationindex();
//			timestep = param_pb.timestep();
		};
		 void convertToPb(pb::PARAM & param_pb){
			 param_pb.set_param(param);
			 param_pb.set_log(log);
			 param_pb.set_range(range);
			 param_pb.set_highvalue(highValue);
			 param_pb.set_calibrationindex(calibrationIndex);
//			 param_pb.set_timestep(timestep);
		 };
		};
typedef vector<PARAM> PARAM_VEC;

PARAM_VEC::const_iterator findTransFlag(const PARAM_VEC & pVec, const string & name, const string & , const string & );

class trans_local{
private:
	trans_map tp;
public:
	trans_map getTransMap(){return tp;};
	void setTransMap(trans_map _tp){tp=_tp;};
	transformation * getTran(string);
	trans_map cloneTransMap();
	void addTrans(string tName,transformation* trans){tp[tName]=trans;};
	virtual void convertToPb(pb::trans_local & lg_pb, pb::GatingSet & gs_pb);
	trans_local(){};
	trans_local(const pb::trans_local & tg_pb, map<intptr_t, transformation *> & trans_tbl);
	virtual void convertToPb(pb::trans_local & lg_pb);
	void updateChannels(const CHANNEL_MAP & chnl_map);
};

class trans_global:public trans_local{
private:
	string groupName;
	vector<int> sampleIDs;
public:
	void setSampleIDs(vector<int> _sampleIDs){sampleIDs=_sampleIDs;}
	vector<int> getSampleIDs(){return sampleIDs;}
	string getGroupName(){return groupName;}
	void setGroupName(string _groupName){groupName=_groupName;};
	void convertToPb(pb::trans_local & tg_pb, pb::GatingSet & gs_pb);
	trans_global(){};
	trans_global(const pb::trans_local & tg_pb, map<intptr_t, transformation *> & trans_tbl);

};

typedef vector<trans_global> trans_global_vec;

class biexpTrans:public transformation{
public:
	int channelRange;
	double pos, neg, widthBasis, maxValue;
public:
	biexpTrans();
	void computCalTbl();
	biexpTrans * clone(){return new biexpTrans(*this);};
	void convertToPb(pb::transformation & trans_pb);
	biexpTrans(const pb::transformation & trans_pb);
	void setTransformedScale(int scale){
		channelRange = scale;
		//recompute cal table
		computCalTbl();
		interpolate();
	};
	int getTransformedScale(){return channelRange;};
	int getRawScale(){return maxValue;};

};

class fasinhTrans:public transformation{
public:
	double maxRange;
	double length;//unused at this moment
	double T, A, M;
public:
	fasinhTrans();
	fasinhTrans(double , double , double , double , double );
	virtual void transforming(valarray<double> & input);
	fasinhTrans * clone(){return new fasinhTrans(*this);};
	void convertToPb(pb::transformation & trans_pb);
	fasinhTrans(const pb::transformation & trans_pb);
	boost::shared_ptr<transformation> getInverseTransformation();
	void setTransformedScale(int scale){maxRange = scale;};
	int getTransformedScale(){return maxRange;};
	int getRawScale(){return T;};
};
/*
 * inverse transformation of fasinhTrans
 */
class fsinhTrans:public fasinhTrans{

public:
	fsinhTrans();
	fsinhTrans(double , double , double , double , double );
	void transforming(valarray<double> & input);
	boost::shared_ptr<transformation> getInverseTransformation(){throw(domain_error("inverse function not defined!"));};
//	fsinhTrans * clone(){return new fasinhTrans(*this);};
//	void convertToPb(pb::transformation & trans_pb);
//	fasinhTrans(const pb::transformation & trans_pb);
//	boost::shared_ptr<transformation> getInverseTransformation();
//	void setTransformedScale(int scale){length = scale;};
};

/*
 * TODO:right now set two flags to TRUE in the contructor to avoid doing cal table stuff,
 * we should consider redesign the classes so that logTrans does not share this extra feature from parent class
 */
class logTrans:public transformation{
public:
		double offset;
		double decade;
		unsigned scale;
		unsigned T; //top value; derived from keyword $PnR for each channel
public:
	logTrans();//deprecated
	logTrans(double _offset,double _decade, unsigned _scale, unsigned T);
	double flog(double x,double _max,double _min);
	void transforming(valarray<double> & input);
	logTrans * clone(){return new logTrans(*this);};
	void convertToPb(pb::transformation & trans_pb);
	logTrans(const pb::transformation & trans_pb);

	boost::shared_ptr<transformation> getInverseTransformation();
	void setTransformedScale(int _scale){scale = _scale;};
	int getTransformedScale(){return scale;};
	int getRawScale(){return T;};
};

class logInverseTrans:public logTrans{
public:
	logInverseTrans(double _offset,double _decade, unsigned _scale, unsigned _T):logTrans(_offset, _decade, _scale, _T){};
	void transforming(valarray<double> & input);

};

class linTrans:public transformation{
public:
        linTrans();
        void transforming(valarray<double> & input);
        linTrans * clone(){return new linTrans(*this);};
        void convertToPb(pb::transformation & trans_pb);
        linTrans(const pb::transformation & trans_pb);
        boost::shared_ptr<transformation> getInverseTransformation(){throw(domain_error("inverse function not defined!"));};
        void setTransformedScale(int scale){throw(domain_error("setTransformedScale function not defined!"));};

};

/*
 * This class is dedicated to scale the EllipsoidGate
 */
class scaleTrans:public linTrans{
	int t_scale; //transformed scale
	int r_scale; // raw scale

public:
	scaleTrans();
	scaleTrans(int,int);
	void transforming(valarray<double> & input);
	scaleTrans * clone(){return new scaleTrans(*this);};
	boost::shared_ptr<transformation> getInverseTransformation();
	void setTransformedScale(int _scale){t_scale = _scale;};

};

class flinTrans:public transformation{
	double min;
	double max;
public:
	flinTrans();
	flinTrans(double _minRange, double _maxRange);
	double flin(double x);
	void transforming(valarray<double> & input);
	flinTrans * clone(){return new flinTrans(*this);};
	void convertToPb(pb::transformation & trans_pb);
	flinTrans(const pb::transformation & trans_pb);
	boost::shared_ptr<transformation> getInverseTransformation(){throw(domain_error("inverse function not defined!"));};
	void setTransformedScale(int scale){throw(domain_error("setTransformedScale function not defined!"));};

};

#endif /* TRANSFORMATION_HPP_ */


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

// introductory Boost.BloomFilter program
#include <boost/bloom_filter/basic_bloom_filter.hpp>
#include <sstream>
#include <string>
#include <iostream>
using namespace boost::bloom_filters;
using namespace std;

const string gen_url(const size_t num)
{
  static const string start_url("https://www.");
  static const string end_url(".com/");
  static stringstream stringer;
  string result;

  stringer << num;
  stringer >> result;
  stringer.clear();

  return start_url + result + end_url;
}

int main () {
  static const size_t INSERT_MAX = 5000;
  static const size_t CONTAINS_MAX = 10000;
  static const size_t NUM_BITS = 32768; // 8KB

  basic_bloom_filter<string, NUM_BITS> bloom;
  size_t collisions = 0;

  for (size_t i = 0; i < INSERT_MAX; ++i) {
    bloom.insert(gen_url(i));
  }

  for (size_t i = INSERT_MAX; i < CONTAINS_MAX; ++i) {
    if (bloom.probably_contains(gen_url(i))) ++collisions;
  }

  cout << "collisions: " << collisions << endl;

  return 0;
}