// error_policies_example.cpp

// Copyright Paul A. Bristow 2007.
// Copyright John Maddock 2007.

// Use, modification and distribution are 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 <boost/math/distributions/normal.hpp>
  using boost::math::normal_distribution;

#include <boost/math/distributions/students_t.hpp>
   using boost::math::students_t;  // Probability of students_t(df, t).
   using boost::math::students_t_distribution;

//  using namespace boost::math;
//.\error_policy_normal.cpp(30) : error C2872: 'policy' : ambiguous symbol
//        could be 'I:\Boost-sandbox\math_toolkit\boost/math/policies/policy.hpp(392) : boost::math::policies::policy'
//        or 'boost::math::policies'

  // So can't use this using namespace command.
// Suppose we want a statistical distribution to return infinities,
// rather than throw exceptions (the default policy), then we can use:

// std
#include <iostream>
   using std::cout;
   using std::endl;

using boost::math::policies::policy;
// Possible errors
using boost::math::policies::overflow_error;
using boost::math::policies::underflow_error;
using boost::math::policies::domain_error;
using boost::math::policies::pole_error;
using boost::math::policies::denorm_error;
using boost::math::policies::evaluation_error;

using boost::math::policies::ignore_error;

// Define a custom policy to ignore just overflow:
typedef policy<
overflow_error<ignore_error>
      > my_policy;

// Define another custom policy (perhaps ill-advised?)
// to ignore all errors: domain, pole, overflow, underflow, denorm & evaluation:
typedef policy<
domain_error<ignore_error>,
pole_error<ignore_error>,
overflow_error<ignore_error>,
underflow_error<ignore_error>,
denorm_error<ignore_error>,
evaluation_error<ignore_error>
      > my_ignoreall_policy;

// Define a new distribution with a custom policy to ignore_error
// (& thus perhaps return infinity for some arguments):
typedef boost::math::normal_distribution<double, my_policy> my_normal;
// Note: uses default parameters zero mean and unit standard deviation.

// We could also do the same for another distribution, for example:
using boost::math::students_t_distribution;
typedef students_t_distribution<double, my_ignoreall_policy> my_students_t;

int main()
{
  cout << "quantile(my_normal(), 0.05); = " << quantile(my_normal(), 0.05) << endl; // 0.05 is argument within normal range.
  cout << "quantile(my_normal(), 0.); = " << quantile(my_normal(), 0.) << endl; // argument zero, so expect infinity.
  cout << "quantile(my_normal(), 0.); = " << quantile(my_normal(), 0.F) << endl; // argument zero, so expect infinity.

  cout << "quantile(my_students_t(), 0.); = " << quantile(my_students_t(-1), 0.F) << endl; // 'bad' argument negative, so expect NaN.

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
  // Construct a (0, 1) normal distribution that ignores all errors,
  // returning NaN, infinity, zero, or best guess,
  // and NOT setting errno.
  normal_distribution<long double, my_ignoreall_policy> my_normal2(0.L, 1.L); // explicit parameters for distribution.
  cout << "quantile(my_normal2(), 0.); = " << quantile(my_normal2, 0.01) << endl; // argument 0.01, so result finite.
  cout << "quantile(my_normal2(), 0.); = " << quantile(my_normal2, 0.) << endl; // argument zero, so expect infinity.
#endif

  return 0;
}

/*

Output:

quantile(my_normal(), 0.05); = -1.64485
quantile(my_normal(), 0.); = -1.#INF
quantile(my_normal(), 0.); = -1.#INF
quantile(my_students_t(), 0.); = 1.#QNAN
quantile(my_normal2(), 0.); = -2.32635
quantile(my_normal2(), 0.); = -1.#INF

*/


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file sigma_point_additive_update_policy.hpp
 * \date July 2015
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <fl/util/meta.hpp>
#include <fl/util/traits.hpp>
#include <fl/util/descriptor.hpp>
#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/quadrature/sigma_point_quadrature.hpp>

namespace fl
{

// Forward declarations
template <typename...> class SigmaPointUpdatePolicy;

template <
    typename SigmaPointQuadrature,
    typename AdditiveSensorFunction
>
class SigmaPointUpdatePolicy<
          SigmaPointQuadrature,
          Additive<AdditiveSensorFunction>>
    : public Descriptor
{
public:
    typedef typename AdditiveSensorFunction::State State;
    typedef typename AdditiveSensorFunction::Obsrv Obsrv;

    enum : signed int
    {
        NumberOfPoints =
            SigmaPointQuadrature::number_of_points(SizeOf<State>::Value)
    };

    typedef PointSet<State, NumberOfPoints> StatePointSet;
    typedef PointSet<Obsrv, NumberOfPoints> ObsrvPointSet;

    template <
        typename Belief
    >
    void operator()(const AdditiveSensorFunction& obsrv_function,
                    const SigmaPointQuadrature& quadrature,
                    const Belief& prior_belief,
                    const Obsrv& obsrv,
                    Belief& posterior_belief)
    {
        auto&& h = [&](const State& x)
        {
           return obsrv_function.expected_observation(x);
        };

        quadrature.propergate_gaussian(h, prior_belief, X, Z);

        auto&& prediction = Z.center();
        auto&& Z_c = Z.points();
        auto&& W = X.covariance_weights_vector();
        auto&& X_c = X.centered_points();

        auto innovation = (obsrv - prediction).eval();
        auto cov_xx = (X_c * W.asDiagonal() * X_c.transpose()).eval();
        auto cov_yy = (Z_c * W.asDiagonal() * Z_c.transpose()
                       + obsrv_function.noise_covariance()).eval();
        auto cov_xy = (X_c * W.asDiagonal() * Z_c.transpose()).eval();
        auto K = (cov_xy * cov_yy.inverse()).eval();

        posterior_belief.dimension(prior_belief.dimension());
        posterior_belief.mean(X.mean() + K * innovation);
        posterior_belief.covariance(cov_xx - K * cov_yy * K.transpose());
    }

    virtual std::string name() const
    {
        return "SigmaPointUpdatePolicy<"
                + this->list_arguments(
                       "SigmaPointQuadrature",
                       "Additive<AdditiveSensorFunction>")
                + ">";
    }

    virtual std::string description() const
    {
        return "Sigma Point based filter update policy for observation model"
               " with additive noise";
    }

protected:
    StatePointSet X;
    ObsrvPointSet Z;
};

}


#include <Eigen/Dense>
#include <map>
#include <algorithm>
#include <vector>

int atom(int ao_index)
{
    int orbitals_per_atom=4;
    return ao_index / orbitals_per_atom;
}

int indexOf(std::vector<std::string> my_list, std::string element)
{
    for(size_t i=0;i<my_list.size();i++)
    {
        if(element==my_list[i])
        {
            return i;
        }
    }
    return -1;
}

bool list_contains(std::vector<std::string>  my_list, std::string element)
{
    bool found = 0;
    for(size_t i=0;i<my_list.size();i++)
    {
        if(element==my_list[i])
        {
            found = 1;
        }
    }
    return found;
}

std::string orb(int ao_index, std::vector<std::string> orbital_types)
{
    int orbitals_per_atom=4;
    int orb_index = ao_index % orbitals_per_atom;
    return orbital_types[orb_index];
}

int ao_index(int atom_p, std::string orb_p, std::vector<std::string>  orbital_types)
{
        int orbitals_per_atom=4;
        int p = atom_p * orbitals_per_atom;
        p += indexOf(orbital_types,orb_p);
        return p;
}

float chi_on_atom(std::string o1, std::string o2, std::string o3, std::vector<std::string> p_orbitals,double dipole)
{
    if(o1 == o2 && o3 == "s")
        return 1.0;
    bool o3_in_p_orbitals = list_contains(p_orbitals,o3);
    if((o1 == o3 && o3_in_p_orbitals) && (o2 == "s"))
        return dipole;
    if ((o2 == o3 && o3_in_p_orbitals) && (o1 == "s"))
        return dipole;
    return 0.0;
}

Eigen::MatrixXd fast_fock_matrix(Eigen::MatrixXd hamiltonian, Eigen::MatrixXd interaction, Eigen::MatrixXd rho,double dipole)
{
    std::vector<std::string> orbital_types ={"s","px", "py", "pz"};
    std::vector<std::string>  p_orbitals = {"px", "py", "pz"};
    size_t ndof = hamiltonian.rows();
    Eigen::MatrixXd fock_mat = hamiltonian;
    //Hartree potential term
    for(size_t p=0;p<ndof;p++)
    {
        for(auto orb_q: orbital_types)
        {
            int q = ao_index(atom(p), orb_q,orbital_types); // p & q on same atom
            for(auto orb_t : orbital_types)
            {
                int t = ao_index(atom(p), orb_t,orbital_types); // p & t on same atom
                float chi_pqt = chi_on_atom(orb(p,orbital_types), orb_q, orb_t,p_orbitals,dipole);
                for(size_t r=0;r<ndof;r++)
                {
                    for(auto orb_s : orbital_types)
                    {
                        int s = ao_index(atom(r), orb_s,orbital_types); // r & s on same atom
                        for(auto orb_u : orbital_types)
                        {
                            int u = ao_index(atom(r), orb_u,orbital_types); // r & u on same atom
                            float chi_rsu = chi_on_atom(orb(r,orbital_types), orb_s, orb_u, p_orbitals,dipole);
                            fock_mat(p,q) += 2.0 * chi_pqt * chi_rsu * interaction(t,u) * rho(r,s);
                        }
                    }
                }
            }
        }
    }
    //Fock exchange term
    for(size_t p=0;p<ndof;p++)
    {
        for(auto orb_s: orbital_types)
        {
            int s = ao_index(atom(p), orb_s,orbital_types); // p & s on same atom
            for(auto orb_u : orbital_types)
            {
                int u = ao_index(atom(p), orb_u,orbital_types); //p & u on same atom
                float chi_psu = chi_on_atom(orb(p,orbital_types), orb_s, orb_u,p_orbitals,dipole);
                for(size_t q=0; q<ndof; q++)
                {
                    for(auto orb_r : orbital_types)
                    {
                        int r = ao_index(atom(q), orb_r,orbital_types); // q & r on same atom
                        for(auto orb_t : orbital_types)
                        {
                            int t = ao_index(atom(q), orb_t,orbital_types); //q & t on same atom
                            float chi_rqt = chi_on_atom(orb_r, orb(q,orbital_types), orb_t, p_orbitals,dipole);
                            fock_mat(p,q) -= chi_rqt * chi_psu * interaction(t,u) * rho(r,s);
                        }
                    }
                }
            }
        }
    }
    return fock_mat;

}

int main(void)
{
    return 0;
}


#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
using namespace std;
using namespace boost::multiprecision;

cpp_int dp[55][4004];

int main() {
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    int n, k, x;
    while (cin >> k >> n >> x) {
        cpp_int nom = 0;
        if (x <= k * n && x >= n) {
            for (int i = 0; i <= 50; i++)
                for (int j = 0; j <= 4000; j++)
                    dp[i][j] = 0;

            dp[0][0] = 1;

            for (int i = 1; i <= k; i++)
                dp[1][i] = 1;

            for (int i = 2; i <= n; i++) {
                cpp_int ps = 0;
                for (int j = i; j <= k * i; j++) {
                    ps += dp[i - 1][j - 1];
                    if (j - i >= k)ps -= dp[i - 1][j - k - 1];
                    dp[i][j] = ps;
                }
            }
            nom = dp[n][x];
        }

        cpp_int denom = 1;
        for (int i = 1; i <= n; i++)denom *= k;

        cout << nom << "/" << denom << endl;
    }

    return 0;
}

Mathematics/Probability Theory/Standard/Throw the Dice.cpp


#include <fstream>
#define _USE_MATH_DEFINES
#include <cmath>
#include <boost/range/iterator_range.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

inline uint8_t scale8u(uint8_t value, uint8_t scale) {
	return (value * scale + value) >> 8;
}

void cos(boost::filesystem::path outputPath) {
	std::ofstream f((outputPath / "cos8u10.h").string());
	f << "#pragma once" << std::endl;

	// unsigned cosine with 8 bit output and 10 bit input, shifted to positive values
	f << "uint16_t FLASH cos8Table[] = {" << std::endl;
	for (int j = 0; j < 64; ++j) {
		f << "\t";
		for (int i = 0; i < 16; ++i) {
			double x = double(i + 16*j) / 512.0 * M_PI;
			//int y = (i | j) == 0 ? 255 : 0;
			int y = int(round((cos(x) + 1.0) * 127.5));
			f << y << ", ";
		}
		f << std::endl;
	}
	f << "};" << std::endl;
	f << "inline uint8_t cos8u10(uint16_t x) {return cos8Table[x];}" << std::endl;
}

void exp(boost::filesystem::path outputPath) {
	{
		// write exp table with 5 bit input and 16 bit output
		std::ofstream f((outputPath / "exp16u5.h").string());
		f << "#pragma once" << std::endl;
		f << "uint16_t FLASH exp16u5Table[] = {" << std::endl;
		f << "\t0";
		for (int i = 1; i < 32; ++i) {
			// x^31 = 65535 -> x = 65535^(1/31)
			int y = int(round(pow(65535.0, i / 31.0)));
			f << ", " << y;
		}
		f << std::endl;
		f << "};" << std::endl;
		f << "inline uint16_t exp16u5(uint8_t x) {return exp16u5Table[x];}" << std::endl;
	}
	{
		// write exp table with 8 bit input and 16 bit output
		std::ofstream f((outputPath / "exp16u8.h").string());
		f << "#pragma once" << std::endl;
		f << "uint16_t FLASH exp16u8Table[] = {" << std::endl;
		f << "\t0";
		for (int i = 1; i < 256; ++i) {
			// x^255 = 65535 -> x = 65535^(1/255)
			int y = int(round(pow(65535.0, i / 255.0)));
			f << ", " << y;
		}
		f << std::endl;
		f << "};" << std::endl;
		f << "inline uint16_t exp16u8(uint8_t x) {return exp16u8Table[x];}" << std::endl;
	}
}

void permute(boost::filesystem::path outputPath) {
	// generate permute table
	srand(1337);
	int permute[256];
	for (int i = 0; i < 256; ++i) {
		permute[i] = i;
	}
	for (int i = 0; i < 10000; ++i) {
		int x = rand() & 0xff;
		int y = rand() & 0xff;
		std::swap(permute[x], permute[y]);
	}

	// write permutation table and function
	std::ofstream f((outputPath / "permute8.h").string());
	f << "#pragma once" << std::endl;
	f << "uint8_t FLASH permute8Table[] = {" << std::endl;
	f << "\t";
	for (int i = 0; i < 256; ++i) {
		if (i != 0)
			f << ", ";
		f << permute[i];
	}
	f << std::endl;
	f << "};" << std::endl;
	f << "inline uint8_t permute8(uint8_t x) {return permute8Table[x];}" << std::endl;
}

void ledSerial(boost::filesystem::path outputPath) {
	// generate pattern to send via serial port to led strip of WS2812b etc.

	// 8 bit are transferred like this (shown inverted) where first and last bit of each block are start and stop bit:
	// 1xx001xx00 1xx001xx00 1xx001xx00 1xx001xx00

	// write lookup table
	std::ofstream f((outputPath / "ledSerial.h").string());
	f << "#pragma once" << std::endl;
	f << "uint32_t FLASH ledSerialTable[] = {" << std::endl;
	f << "\t";
	for (int v = 0; v < 256; ++v) {
		if (v != 0)
			f << ", ";

		// square the value to emulate gamma of 2
		int value = scale8u(v, v);

		// all ones in the middle of each block, omitting start bits (inverted)
		uint32_t pattern = ~0x08080808;
		for (int i = 0; i < 8; ++i) {
			if (value & (1 << i)) {
				// offsets of the "xx" are 1, 6, 9, 14, 17, 22, 25, 30
				int offset = 1 + 4 * i + (i & 1 ? 1 : 0);
				pattern &= ~(3 << offset);
			}
		}

		f << pattern;
	}
	f << std::endl;
	f << "};" << std::endl;
}

struct IndexColor {
	int index;

	int red;
	int green;
	int blue;

	IndexColor() = default;
	IndexColor(int index, uint8_t red, uint8_t green, uint8_t blue)
		: index(index), red(red), green(green), blue(blue) {}
};

inline int toInt(std::string const & s) {
	int base = 10;
	int i = 0;
	if (boost::starts_with(s, "0x")) {
		base = 16;
		i = 2;
	}

	int value = 0;
	for (; i < s.length(); ++i) {
		value *= base;
		char ch = s[i];

		if (ch >= '0' && ch <= '9') {
			value += ch - '0';
		} else if (base == 16) {
			if (ch >= 'A' && ch <= 'F')
				value += ch - 'A' + 10;
			else if (ch >= 'a' && ch <= 'f')
				value += ch - 'a' + 10;
		}
	}
	return value;
}

void interpolate(std::ofstream & f, const std::string &indent, int index1, int index2, int value1, int value2) {
	f << value1;
	if (value2 != value1) {
		f << (value2 > value1 ? " + " : " - ");
		f << '(';
		if ((index1 & 0xff) == 0)
			f << "(uint8_t)x";
		else
			f << "(uint8_t)(x - " << index1 << ")";
		f << " * " << std::abs(value2 - value1);
		int d = index2 - index1;
		int shift = 0;
		while (d > 1) {
			++shift;
			d >>= 1;
		}
		f << " >> " << shift;
		f << ')';
	}
}

void subdivide(std::ofstream & os, const std::string& indent, std::vector<IndexColor> const & palette, int begin, int end) {
	if (end - begin > 1) {
		int mid = (begin + end) / 2;

		os << indent << "if (x < 0x" << std::hex << palette[mid].index << std::dec << ") {" << std::endl;
		subdivide(os, indent + '\t', palette, begin, mid);
		os << indent << "} else {" << std::endl;
		subdivide(os, indent + '\t', palette, mid, end);
		os << indent << "}" << std::endl;
	} else {
		// add index as comment
		os << indent << "// 0x" << std::hex << palette[begin].index << std::dec << std::endl;

		// interpolate colors
		IndexColor color1 = palette[begin];
		IndexColor color2 = palette[begin + 1];
		os << indent << "color.red = "; interpolate(os, indent, color1.index, color2.index, color1.red, color2.red); os << ";" << std::endl;
		os << indent << "color.green = "; interpolate(os, indent, color1.index, color2.index, color1.green, color2.green); os << ";" << std::endl;
		os << indent << "color.blue = "; interpolate(os, indent, color1.index, color2.index, color1.blue, color2.blue); os << ";" << std::endl;
	}
}

void palette(boost::filesystem::path path, boost::filesystem::path outputPath) {
	std::vector<IndexColor> palette;

	// read palette
	std::ifstream is(path.string());
	std::string line;
	while (std::getline(is, line)) {
		// remove comment
		size_t pos = line.find("//");
		if (pos != std::string::npos)
			line = line.substr(0, pos);

		// trim
		boost::algorithm::trim(line);

		// split
		std::vector<std::string> elements;
		boost::split(elements, line, boost::algorithm::is_any_of("\t "), boost::token_compress_on);

		if (elements.size() >= 2) {

			int index = toInt(elements[0]);

			if (elements.size() == 2) {
				if (elements[1] == "loop") {
					IndexColor ic = palette.front();
					ic.index = index;
					palette.push_back(ic);
				}
			} else if (elements.size() == 4) {
				int r = toInt(elements[1]);
				int g = toInt(elements[2]);
				int b = toInt(elements[3]);

				palette.push_back(IndexColor(index, r, g, b));
			}
		}
	}

	std::string name = path.stem().string();
	std::ofstream os((outputPath / (name + ".h")).string());
	os << "#pragma once" << std::endl;
	os << "inline RGB " << name << "(uint16_t x) {" << std::endl;

	//os << "\tuint8_t r, g, b;" << std::endl;
	os << "\tRGB color;" << std::endl;

	subdivide(os, "\t", palette, 0, palette.size()-1);

	//os << "\tRGB color;" << std::endl;
	//os << "\tcolor.red = r;" << std::endl;
	//os << "\tcolor.green = g;" << std::endl;
	//os << "\tcolor.blue = b;" << std::endl;
	os << "\treturn color;" << std::endl;

	os << "}" << std::endl;
	os.close();
}

int main(int argc, const char **argv) {
	boost::filesystem::path outputPath = "generated";
	boost::filesystem::create_directory(outputPath);

	// generate tables
	cos(outputPath);
	exp(outputPath);
	permute(outputPath);
	ledSerial(outputPath);

	// convert palettes in current directory
	boost::filesystem::path p = ".";
	for (auto & entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {})) {
		boost::filesystem::path path = entry.path();
		std::string ext = path.extension().string();
		if (ext == ".palette") {
			palette(path, outputPath);
		}
	}

	return 0;
}


/**
 * @file mtgp32-calc-poly.cpp
 *
 * @brief calculate characteristic polynomial for 32bit mtgp.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (Hiroshima University)
 *
 * Copyright (c) 2012 Mutsuo Saito, Makoto Matsumoto, Hiroshima
 * University and University of Tokyo. All rights reserved.
 *
 * The new BSD License is applied to this software, see LICENSE.txt
 */
#include <stdint.h>
#include <inttypes.h>
#include <time.h>
#include <string.h>
#include <string>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>
#include "mtgp32-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp32-fast.h"

using namespace std;
using namespace NTL;

/**
 * calculate the characteristic polynomial for given 32-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] poly calculated characteristic polynomial.
 * @param[in] mtgp64 generator
 */
void calc_characteristic(GF2X& poly, mtgp32_fast_t * mtgp32)
{
    vec_GF2 seq;
    int mexp = mtgp32->params.mexp;
    seq.SetLength(2 * mexp);
    for (int i = 0; i < 2 * mexp; i++) {
	seq[i] = mtgp32_genrand_uint32(mtgp32) & 1;
    }
    MinPolySeq(poly, seq, mexp);
}

/**
 * calculate the characteristic polynomial for given 32-bit MTGP.
 * MinPolySeq is defined in NTL.
 * @param[out] str calculated characteristic polynomial in string format.
 * @param[in] mtgp64 generator
 */
void calc_characteristic(string& str, mtgp32_fast_t * mtgp32)
{
    GF2X poly;
    calc_characteristic(poly, mtgp32);
    polytostring(str, poly);
}

void calc_characteristic(uint32_t array[], int size, mtgp32_fast_t * mtgp32)
{
    GF2X poly;
    calc_characteristic(poly, mtgp32);
    polytoarray(array, size, poly);
}

#if defined(MAIN)
/**
 * main function for executable.
 * @param[in] argc number of arguments.
 * @param[in] argv an array of arguments.
 * @return 0 if normal, other abnormal.
 */
int main(int argc, char *argv[]) {
    int mexp;
    int no;
    uint32_t seed = 1;
    mtgp32_params_fast_t *params;
    mtgp32_fast_t mtgp32;
    int rc;

    if (argc <= 2) {
	printf("%s: mexp no.\n", argv[0]);
	return 1;
    }
    mexp = strtol(argv[1], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 2;
    }
    no = strtol(argv[2], NULL, 10);
    if (errno) {
	printf("%s: mexp no.\n", argv[0]);
	return 3;
    }
    switch (mexp) {
    case 11213:
	params = mtgp32_params_fast_11213;
	break;
    case 23209:
	params = mtgp32_params_fast_23209;
	break;
    case 44497:
	params = mtgp32_params_fast_44497;
	break;
    default:
	printf("%s: mexp no.\n", argv[0]);
	printf("mexp shuould be 11213, 23209 or 44497\n");
	return 4;
    }
    if (no >= 128 || no < 0) {
	printf("%s: mexp no.\n", argv[0]);
	printf("no must be between 0 and 127\n");
	return 5;
    }
    params += no;
    rc = mtgp32_init(&mtgp32, params, seed);
    if (rc) {
	printf("failure in mtgp32_init\n");
	return -1;
    }
    mtgp32_print_idstring(&mtgp32, stdout);
#if 0
    string s;
    calc_characteristic(s, &mtgp32);
    printf("%s\n", s.c_str());
#else
#endif
    mtgp32_free(&mtgp32);
    return 0;
}
#endif


//
// Created by eliezer on 07.11.16.
//

#include <cmath>
#include "BatchPoissonPure.h"
#include <math.h>
#include <limits>
#include <algorithm>
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <boost/math/special_functions/digamma.hpp>
#include <chrono>
#include <boost/math/special_functions/gamma.hpp>
#ifndef M_PIl
/** The constant Pi in high precision */
#define M_PIl 3.1415926535897932384626433832795029L
#endif
#ifndef M_GAMMAl
/** Euler's constant in high precision */
#define M_GAMMAl 0.5772156649015328606065120900824024L
#endif
#ifndef M_LN2l
/** the natural logarithm of 2 in high precision */
#define M_LN2l 0.6931471805599453094172321214581766L
#endif

double gamma_term(double a, double b, double a_latent, double b_latent, double e_latent, double elog_latent) {
    return lgamma(a_latent)-lgamma(a)+a*log(b)+a_latent*(1-log(b_latent))-b*e_latent+(a-a_latent)*elog_latent;
}

void compute_gama_expected(Arrayf& a_x, Arrayf& b_x, Arrayf& e_x,
                           Arrayf& elog_x) {
    for ( long i=0; i< a_x.rows();i++ ){
        for( long k=0; k< a_x.cols();k++){
            e_x(i,k) = a_x(i,k)/b_x(k);
            //elog_x(i,k) = (double)(digammal(a_x(i,k))-log(b_x(k))); // using standalone implementation
            elog_x(i, k) = boost::math::digamma(a_x(i, k)) - log(b_x(k)); // using boost implementation

        }
    }
}

size_t sizes_var(vars v,size_t n_ratings, size_t n_wd_entries, size_t n_users, size_t n_items,size_t k_feat, size_t n_words, size_t n_max_neighbors){
    size_t result=0;
    switch(v){
        case vars::a_beta:
        case vars::e_beta:
        case vars::elog_beta:
            result= k_feat*n_words;
            break;

        case vars::a_epsilon:
        case vars::e_epsilon:
        case vars::elog_epsilon:
        case vars::a_theta:
        case vars::e_theta:
        case vars::elog_theta:
            result= k_feat*n_items;
            break;

        case vars::e_eta:
        case vars::elog_eta:
        case vars::a_eta:
            result= k_feat*n_users;
            break;

        case vars::e_tau:
        case vars::elog_tau:
        case vars::a_tau:
            result= n_max_neighbors*n_users;
            break;

        case vars::phi:
            result=k_feat*n_wd_entries;
            break;
        case vars::xi_M:
        case vars::xi_N:
            result= k_feat*n_ratings;
            break;
        case vars::xi_S:
            result= n_max_neighbors*n_ratings;
            break;

        case vars::b_theta:
        case vars::b_eta:
        case vars::b_epsilon:
        case vars::b_beta:
            result= k_feat;
            break;

        case vars::b_tau:
            result= n_max_neighbors;
            break;
    }
    //std::cout << EnumStrings[(int)v]<<"= "<<result<<'\n';
    return result;
}

size_t total_memory(size_t n_ratings, size_t n_wd_entries, size_t n_users, size_t n_items,size_t k_feat, size_t n_words, size_t n_max_neighbors){
    size_t sum=0;

    for(vars i : vec_vars){
        sum+=sizes_var(i,n_ratings,n_wd_entries, n_users,  n_items,k_feat, n_words,n_max_neighbors);
    }
    return sum;
}

BatchPoissonNewArray::BatchPoissonNewArray(size_t n_ratings, size_t n_wd_entries, size_t n_users, size_t n_items,
                                           size_t k_feat, size_t n_words, size_t n_max_neighbors, double a, double b,
                                           double c, double d, double e, double f, double g, double h, double k, double l) :
        arrman(new ArrayManager<double>(2*total_memory(n_ratings,n_wd_entries, n_users,  n_items,k_feat, n_words,n_max_neighbors))),
        _n_users(n_users), _n_items(n_items), _k_feat(k_feat),
        _n_words(n_words), _n_ratings(n_ratings),
        _n_wd_entries(n_wd_entries), _n_max_neighbors(n_max_neighbors),
        a(a), b(b), c(c), d(d), e(e), f(f), g(g), h(h), k(k), l(l),
        beta(arrman,n_words,k_feat,a,b),
        theta(arrman,n_items,k_feat,c,d),
        epsilon(arrman,n_items,k_feat,g,h),
        eta(arrman,n_users,k_feat,e,f),
        tau(arrman,n_users,n_max_neighbors,k,l),
        phi(arrman->makeArray(n_wd_entries,k_feat)),
        xi_M(arrman->makeArray(n_ratings,k_feat)),
        xi_N(arrman->makeArray(n_ratings,k_feat)),
        xi_S(arrman->makeArray(n_ratings,n_max_neighbors)),
        user_items_map(pairmap(n_ratings)),
        user_items_index(vector< pair<size_t,size_t>>(n_users)),
        user_items_neighboors(vector< list <  pair<size_t, size_t > > >(n_ratings))
{

    mem_use= total_memory(n_ratings,n_wd_entries, n_users,  n_items,k_feat, n_words,n_max_neighbors);
    std::cout << "total = " << mem_use <<"\n";
    std::cout << "n_users = " << n_users <<"\n";
    std::cout << "n_items = " << n_items <<"\n";
    std::cout << "n_words = " << n_words <<"\n";
    std::cout << "n_max_neighbors = " << n_max_neighbors <<"\n";
    std::cout << "n_ratings = " << n_ratings <<"\n";
    std::cout << "n_wd_entries = " << n_wd_entries <<"\n";
    std::cout << "k_feat = " << k_feat <<"\n";

}

void BatchPoissonNewArray::train(size_t n_iter, double tol) {
    try {
        std::cout << "n_iter = " << n_iter <<"\n";
        std::cout << "tol = " << tol <<"\n";
        init_aux_latent();
        double old_elbo=-std::numeric_limits<double>::infinity();
        double elbo=0;

        for(auto i=0;i<n_iter;i++){

                std::cout << "############ITERATION "<<i<<" of "<<n_iter<<endl;
                std::cout << "Begin update latent variables"<<endl;
                update_latent();
                std::cout << "Begin update auxiliary variables"<<endl;
                update_aux_latent();
                elbo = compute_elbo();

                elbo_lst.push_back(elbo);
                std::cout << "Old ELBO="<<old_elbo<<"  ---- new ELBO="<< elbo<< " improvement = " << abs((elbo-old_elbo)/old_elbo) << endl;

                if(abs((elbo-old_elbo)/old_elbo) < tol)
                    break;
                else
                    old_elbo=elbo;

        }
        std::cout << "List os ELBO values";
        std::copy(elbo_lst.begin(),
                  elbo_lst.end(),
                  std::ostream_iterator<double>(std::cout, " , "));
    } catch (const std::bad_alloc& e) {
        std::cout << "Allocation failed: " << e.what() << '\n';
        exit(-1);
    }
}

 void BatchPoissonNewArray::init_train(vector<tuple<size_t, size_t, size_t>> r_entries,
                                      vector<tuple<size_t, size_t, size_t>> w_entries,
                                      vector< vector<size_t> > user_neighboors) {
    this->r_entries=r_entries;
    this->w_entries=w_entries;
    this->user_neighboors=user_neighboors;
    cout << "init_train " << endl;
    std::cout << "r_entries.size = " << r_entries.size() <<"\n";
    std::cout << "w_entries.size = " << w_entries.size() <<"\n";
    std::cout << "user_neighboors.size = " << user_neighboors.size() <<"\n";

    pair<size_t,size_t> temp=make_pair(0,0);

    size_t ud;
    for(ud=0; ud< _n_ratings;ud++) {
        auto user_u = std::get<0>(r_entries[ud]);
        auto item_i = std::get<1>(r_entries[ud]);
        temp.first=user_u;
        temp.second=item_i;
        user_items_map[temp]=  ud;

    }
    temp=make_pair(0,0);
    size_t current_user=0;
    for(ud = 0; ud < _n_ratings; ud++) {
        auto user_u = std::get<0>(r_entries[ud]);
        auto item_i = std::get<1>(r_entries[ud]);
        tau.b_latent(user_u)+=std::get<2>(r_entries[ud]); // b_tau_user_i = l + \sum_d r_{user_i,d}

        if(current_user!=user_u){
            // generate a index with beginning index and end index for item rated by user in the user_item_rating matrix
            //
            temp.second=ud;
            user_items_index[current_user]=temp;
            temp=make_pair(ud,0);
            current_user=user_u;
            //cout << "("<<user_u << ", " << item_i << "," << std::get<2>(r_entries[ud])<<"),";

        }
        size_t neigh_ord=0;
        for(auto user_i : this->user_neighboors[user_u]){
            // test which neighboor of user_u has item_i rated and point to the index on the rating matrix
            pairmap::iterator ifind = user_items_map.find(make_pair(user_i,item_i));
            if ( ifind != user_items_map.end() ){
                // fill vector< list <  pair<size_t, size_t > > > user_items_neighboors;
                // list<<pair<user_neighbor_i,index_in_r_entries>>
                user_items_neighboors[ud].push_back(make_pair(user_i,ifind->second));

            }
            neigh_ord++;
        }
    }
    temp.second=_n_ratings;
    user_items_index[current_user]=temp;
    //cout << "###########" <<endl;
    //for(size_t i=0;i<user_items_index.size();i++)
    //{
    //    cout << "("<<i<<","<<user_items_index[i].first <<","<<user_items_index[i].second  << "),";
    //}
    beta.init_b_latent();
    theta.init_b_latent();
    epsilon.init_b_latent();
    eta.init_b_latent();
    cout << "tau_b="<< tau.b_latent << endl;
}

void BatchPoissonNewArray::init_aux_latent() {
    phi.init_gamma_row_normalized();
    xi_M.init_gamma_row_normalized();
    xi_N.init_gamma_row_normalized();
    xi_S.init_gamma_row_normalized();
    //update_latent();
    //update_expected();
    //xi_S=0;
    //xi_M=0;
    //xi_N=0;
    //xi_S=0;
    //update_aux_latent();

}

void BatchPoissonNewArray::update_latent() {
    beta.init_a_latent();
    theta.init_a_latent();
    epsilon.init_a_latent();
    eta.init_a_latent();
    tau.init_a_latent();
    cout << "INIT#theta" << theta <<endl;
    //cout << "INIT#eta" << eta <<endl;
    //cout << "INIT#tau" << tau <<endl;
    //cout << "INIT#epsilon" << epsilon <<endl;
    //cout << "INIT#beta" << epsilon <<endl;

    for(size_t ud=0; ud< _n_ratings;ud++){
        auto user_u = std::get<0>(r_entries[ud]);
        auto item_i = std::get<1>(r_entries[ud]);
        auto r_ud= std::get<2>(r_entries[ud]);

        for(size_t k=0; k< _k_feat;k++){
            auto rudk_M=r_ud*xi_M(ud,k);
            auto rudk_N=r_ud*xi_N(ud,k);
            epsilon.a_latent(item_i,k)+=rudk_N;
            theta.a_latent(item_i,k)+=rudk_M;
            eta.a_latent(user_u,k)+=rudk_M+rudk_N;
        }

        for(auto neighb : user_items_neighboors[ud]) {
            tau.a_latent(user_u,neighb.first)+=r_ud*xi_S(ud,neighb.first);
        }
    }
    double temp_w;
    for(size_t dv=0; dv< _n_wd_entries;dv++){

        auto word_w = std::get<1>(w_entries[dv]);
        auto item_i = std::get<0>(w_entries[dv]);
        auto wdv= std::get<2>(w_entries[dv]);
        for(size_t k=0; k< _k_feat;k++) {
            temp_w=wdv*phi(dv,k);
            beta.a_latent(word_w,k)+=temp_w;
            theta.a_latent(item_i,k)+=temp_w;

        }
    }

    beta.init_b_latent();
    eta.init_b_latent();
    for(size_t k=0; k< _k_feat;k++) {
        double sum_d_epsilon = epsilon.e_expected.col_sum(k);
        double sum_d_theta = theta.e_expected.col_sum(k);
        beta.b_latent(k)+= sum_d_theta;
        eta.b_latent(k) += sum_d_theta + sum_d_epsilon;
    }
    beta.update_expected();
    eta.update_expected();

    theta.init_b_latent();
    epsilon.init_b_latent();
    for(size_t k=0; k< _k_feat;k++) {
        double sum_u_eta = eta.e_expected.col_sum(k);
        double sum_v_beta = beta.e_expected.col_sum(k);
        epsilon.b_latent(k) += sum_u_eta;
        theta.b_latent(k) += sum_u_eta + sum_v_beta;
    }
    theta.update_expected();
    epsilon.update_expected();
    tau.update_expected();

    //cout << "END#theta" << theta <<endl;
    //cout << "AFTERTHETA#xi_M" << xi_M << endl;
    //cout << "END#eta" << eta <<endl;
    //cout << "END#tau" << tau <<endl;
    //cout << "END#epsilon" << epsilon <<endl;
    //cout << "END#beta" << epsilon <<endl;
}

void BatchPoissonNewArray::update_aux_latent() {
    double sum_k=0;
    for(auto ud=0; ud< _n_ratings;ud++) {
        // TODO: implement LOG-SUM
        sum_k=0;
        auto user_u = std::get<0>(r_entries[ud]);
        auto item_i = std::get<1>(r_entries[ud]);
        for (auto k = 0; k < _k_feat; k++) {
            // self.xi_M = np.exp(self.Elogeta[:, np.newaxis, :] + self.Elogtheta[:, :, np.newaxis])
            xi_M(ud, k) = exp(eta.elog_expected(user_u, k) + theta.elog_expected(item_i, k));

            // self.xi_N = np.exp(self.Elogeta[:, np.newaxis, :] + self.Elogepsilon[:, :, np.newaxis])
            xi_N(ud, k) = exp(eta.elog_expected(user_u, k) + epsilon.elog_expected(item_i, k));
            sum_k += xi_M(ud,k) + xi_N(ud,k);
        }
        xi_S.row(ud) = 0;
        for (auto neighb : user_items_neighboors[ud]) {
            // user_items_neighboors[ud].push_back(make_pair(user_i,ifind->second));
            xi_S(ud,neighb.first) = std::get<2>(r_entries[neighb.second])
                                     * exp(tau.elog_expected(user_u,neighb.first));
            sum_k += xi_S(ud,neighb.first);

        }
        {
            xi_M.row(ud) /= sum_k;
        }
        {
            xi_N.row(ud) /= sum_k;
        }
        {
            xi_S.row(ud) /= sum_k;
        }
    }
    cout << endl;

    for(auto dv=0; dv< _n_wd_entries;dv++){
        sum_k=0;
        auto word_w = std::get<1>(w_entries[dv]);
        auto item_i = std::get<0>(w_entries[dv]);
        for(auto k=0; k< _k_feat;k++){
            // self.phi = np.exp(self.Elogbeta[:, np.newaxis, :] + self.Elogtheta[:, :, np.newaxis])
            phi(dv,k)=exp(beta.elog_expected(word_w,k)+theta.elog_expected(item_i,k));
            sum_k += phi(dv,k);
        }
        {
            phi.row(dv)/=sum_k;
        }

    }
    cout << "#END UPDATE_AUX" << endl;
    cout << "#xi_M" << xi_M.row(11685) << endl;
    cout << "#xi_N" << xi_N.row(11685) << endl;
    //cout << "#xi_S" << xi_S.row(11685) << endl;
    //cout << "#xi_N" << xi_N << endl;
    //cout << "#xi_S" << xi_S << endl;
    cout << "#phi" << phi.row(0) << endl;

}

double BatchPoissonNewArray::compute_elbo() {
    double total_sum;
    total_sum = 0.0;
    double log_sum=0;
    // poisson termo of the ELBO for user-document ratings
    // sum_u,d,k{ Eq[log p(r_ud|*) ] }
    cout << r_entries.size() << " nrat " << _n_ratings;
    for(size_t  ud=0; ud< _n_ratings;ud++) {
        size_t  user_u = std::get<0>(r_entries[ud]);
        size_t  item_i = std::get<1>(r_entries[ud]);
        size_t  r_ud = std::get<2>(r_entries[ud]);
        log_sum=0;
        for (size_t  k = 0; k < _k_feat; k++) {
            if(xi_M(ud,k) > 0)
                log_sum += xi_M(ud,k)*(eta.elog_expected(user_u,k)+theta.elog_expected(item_i,k)-log(xi_M(ud,k)));
            if(xi_N(ud,k) > 0)
                log_sum += xi_N(ud,k)*(eta.elog_expected(user_u,k)+epsilon.elog_expected(item_i,k)-log(xi_N(ud,k)));
            if(log_sum!=log_sum)
            {
                cout << "(NAN-logsum: ud="<<ud<<", k="<<k<<" xi_M(ud,k)="<<xi_M(ud,k)<<" xi_N(ud,k)="<<xi_N(ud,k)
                     <<" E_q[log eta_uk]="<<eta.elog_expected(user_u,k)<<" E_q[log the_dk]="<<theta.elog_expected(item_i,k)
                     <<" E_q[log eta_uk]="<<epsilon.elog_expected(user_u,k);
            }
        }
        if(ud==0)
            cout << "logsum = " << log_sum << " ";
        for (pair<size_t,size_t> neighb : user_items_neighboors[ud]) {
            //neighb is user_i in N(user_u), neighb.first is its index in the trust tau variable
            // user_items_neighboors[ud].push_back(make_pair(user_i,ifind->second));
            size_t  r_id = std::get<2>(r_entries[neighb.second]);
            if(xi_S(ud,neighb.first) > 0)
                log_sum += xi_S(ud,neighb.first)*(tau.elog_expected(user_u,neighb.first)+log(r_id )
                                                       -log(xi_S(ud,neighb.first)));
        }
        if(ud==0)
            cout << "logsum = " << log_sum << " ";
        total_sum+=r_ud*log_sum-boost::math::lgamma(r_ud+1);
        if(boost::math::isnan( total_sum))
            cout << "##LOG_SUM ud="<<ud<<", user_u="<<user_u<<"item_i="<<item_i<<" r_ud="<<r_ud<<"##";
        /** TODO:
         * - sum_u,d,k over Eq[latent variables] (Eq without log probability)
         */
    }
    // poisson termo of the ELBO for word-document count
    // sum_v,d,k{ Eq[log p(w_dv|*) ] }
    //cout << xi_M;
    //cout << theta;
   // cout << endl;
    //cout << "r entries "<< std::get<0>(r_entries[0]) << " " << std::get<1>(r_entries[0]) << " " << boost::math::lgamma(std::get<2>(r_entries[0])+1) << " " <<endl;
    cout << "partial elbo 1 "<<total_sum;
    cout << endl;
    for(size_t dv=0; dv< _n_wd_entries;dv++){
        size_t  word_w = std::get<1>(w_entries[dv]);
        size_t item_i = std::get<0>(w_entries[dv]);
        size_t  w_dv = std::get<2>(w_entries[dv]);
        log_sum=0;
        for(size_t k=0; k< _k_feat;k++){
            log_sum += phi(dv,k)*(beta.elog_expected(word_w,k)+theta.elog_expected(item_i,k)- log(phi(dv,k)));
            if(boost::math::isnan( log_sum))
                cout << "##LOG_SUM dv="<<dv<<", k="<<k<<", word_w="<<word_w<<"item_i="<<item_i<<" phi(dv,k)="<<phi(dv,k)<<", beta.elog_expected(word_w,k)="
                     <<beta.elog_expected(word_w,k)
                     <<", beta.elog_expected(word_w,k)="<<beta.elog_expected(word_w,k)
                     <<",theta.elog_expected(item_i,k)="<<theta.elog_expected(item_i,k)
                     <<",log(phi(dv,k)))="<<log(phi(dv,k))
                        <<"##";

        }

        total_sum+=(((double)w_dv)*log_sum)-boost::math::lgamma(w_dv+1);
        if(boost::math::isnan( total_sum))
            cout << "##LOG_SUM dv="<<dv<<", word_w="<<word_w<<"item_i="<<item_i<<" w_dv="<<w_dv<<"##";
        /* if(w_dv >= 1){
            try {
                double x=boost::math::lgamma(w_dv+1);
                //cout << "boot lgamma =" << x << " wdv=" << w_dv << endl ;
                if(boost::math::isnan( log_sum))
                    cout << "##LOG_SUM dv="<<dv<<", word_w="<<word_w<<"item_i="<<item_i<<" w_dv="<<w_dv<<"log-fact="<<x<<"##";
                total_sum-=x;
                //cout << "total_sum =" << total_sum << endl ;
            } catch (const std::bad_alloc& e) {
                std::cout << "Allocation failed: " << e.what() << '\n';
                exit(-1);
            }

        }*/

        /** TODO:
         * - sum_v,d,k over Eq[latent variables] (Eq without log probability)
         */

    }

    //term with sum of multiplication of expected-value of latent variables
    // -sum_k,d,v E[theta_dk]*E[beta_vk]
    total_sum+=theta.elbo_term_prod_linear_expectations(vector<gamma_latent*>({&beta}));
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM theta*beta";
    // -sum_k,d,u E[theta_dk]*E[eta_uk]+E[epsilon_dk]*E[eta_uk]
    total_sum+=eta.elbo_term_prod_linear_expectations(vector<gamma_latent*>({&theta,&epsilon}));
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM theta*eta+epsilon*eta";
    total_sum+=tau_elbo_expected_linear_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM tau";

    // Gamma terms for the latent variables
    total_sum+=beta.elbo_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM gamma beta";
    total_sum+=theta.elbo_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM gamma theta";
    total_sum+=epsilon.elbo_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM gamma epsilon";
    total_sum+=eta.elbo_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM  gamma eta";
    total_sum+=tau.elbo_term();
    if(boost::math::isnan( total_sum ))
        cout << "##TOTAL_SUM  gamma tau";
    return total_sum;
}
/*
 * TODO:
 * - Change everything about tau variable. There is two options:
 * 1) kdim of tau variable is = n_users;
 * 2) kdim of tau variable is = n_users_that_are_neighbors.
 * In beginning to think that option 1) is better, but it will need some changing in other places
 *
 * UPDATE: implemented option 1
 */

double BatchPoissonNewArray::tau_elbo_expected_linear_term() {
    double total_sum=0;
    for(size_t u=0;u < _n_users ;u++) {
        for(size_t i : user_neighboors[u]) {
            for(size_t d=user_items_index[i].first; d<user_items_index[i].second;d++) {
                size_t r_id = std::get<2>(r_entries[d]);
                total_sum+=tau.e_expected(u,i)*r_id;
            }
        }
    }
    return -total_sum;
}

vector<vector<double>>  BatchPoissonNewArray::estimate() {
    cout << "begin estimate" <<endl;
    vector<vector<double>> ret(_n_users);
    for(size_t user_u=0;user_u < _n_users; user_u++){
        for(size_t item_i=0;item_i < _n_items ; item_i++){
            double r_ui = 0;
            for(size_t k=0;k<_k_feat;k++){
                r_ui += eta.e_expected(user_u,k)*(epsilon.e_expected(item_i,k)+theta.e_expected(item_i,k));
            }
            for(size_t user_i : user_neighboors[user_u]){
                pairmap::iterator ifind = user_items_map.find(make_pair(user_i,item_i));
                if ( ifind != user_items_map.end() )
                    r_ui += tau.e_expected(user_u,user_i)*ifind->second;
            }
            ret[user_u].push_back(r_ui);
        }
    }
    cout << "end estimate" <<endl;
    return ret;
}

struct predicate
{
    bool operator()(const std::pair<double,size_t> &left, const std::pair<double,size_t> &right)
    {
        return left.first < right.first;
    }
};

vector<vector<size_t>> BatchPoissonNewArray::recommend(size_t m) {
    cout << "begin recommend" <<endl;

    vector< vector<double>> ret= estimate();
    vector<vector<size_t>> rec;
    ;

    for(size_t user_u=0;user_u < _n_users; user_u++)
    {
        vector<pair<double,size_t>> scores;
        for(size_t item_i=0;item_i < _n_items ; item_i++){
            // recommend only items that are not already rated by the user
            if(user_items_map.count(make_pair(user_u,item_i))<=0)
                scores.push_back(make_pair(ret[user_u][item_i],item_i));
        }
        std::sort(scores.begin(),scores.end());
        std::reverse(scores.begin(),scores.end());
        vector<size_t> temp;
        for(size_t i=0; i < m ; i++)
        {
            //cout << "(" <<scores[i].first <<","<<scores[i].second<<")";
            temp.push_back(scores[i].second);
        }
        //cout << endl;
        rec.push_back(temp);
    }
    return rec;
}

void BatchPoissonNewArray::recommend(std::ostream &output, size_t m) {
    cout << "begin recommend save" <<endl;

    vector<vector<double>> ret= estimate();

    for(size_t user_u=0;user_u < _n_users; user_u++)
    {
        vector<pair<double,size_t>> scores;
        for(size_t item_i=0;item_i < _n_items ; item_i++){
            // recommend only items that are not already rated by the user
            if(user_items_map.count(make_pair(user_u,item_i))<=0)
                scores.push_back(make_pair(ret[user_u][item_i],item_i));
        }
        std::sort(scores.begin(),scores.end());
        std::reverse(scores.begin(),scores.end());
        vector<size_t> temp;
        for(size_t i=0; i < m ; i++)
        {
            //cout << "(" <<scores[i].first <<","<<scores[i].second<<")";
            temp.push_back(scores[i].second);
        }
        std::copy (temp.begin(), temp.end(), std::ostream_iterator<size_t>(output, "\t"));
        output << endl;
    }
}

BatchPoissonNewArray::~BatchPoissonNewArray() {
    arrman->~ArrayManager();
}

vars operator++(vars &x) { return x = (vars)(((int)(x) + 1)); }

void gamma_latent::update_expected() {
    for ( size_t i=0; i< a_latent.rows();i++ ){
        for( size_t k=0; k< a_latent.cols();k++){
            e_expected(i,k) = a_latent(i,k)/b_latent(k);
            if(i==k && i==0)
            {
                if(boost::math::isnan( e_expected(i,k)))
                    cout << "NAN-EXP "<<i<<" "<<k<<" a_latent="<<a_latent(i,k)<<" b_latent="<<b_latent(k);
            }

            //elog_x(i,k) = (double)(digammal(a_x(i,k))-log(b_x(k))); // using standalone implementation
            elog_expected(i, k) = boost::math::digamma(a_latent(i, k)) - log(b_latent(k)); // using boost implementation
            if(i==k && i==0) {
                if (boost::math::isnan(elog_expected(i, k)))
                    cout << "NAN-LOGEXP " << i << " " << k << " digama a_latent="
                         << boost::math::digamma(a_latent(i, k)) << " b_latent=" << b_latent(k) << " log b"
                         << log(b_latent(k));
            }
        }
    }
}

double gamma_latent::elbo_term() {
    double total_sum=0;
    for( size_t  d=0;d<a_latent.nrow;d++){
        for( size_t  k=0; k<a_latent.ncol;k++){
            total_sum+=gamma_term(a,b,a_latent(d,k),b_latent(k),e_expected(d,k),elog_expected(d,k));
        }
    }
    return total_sum;
}

double gamma_latent::elbo_term(vector<gamma_latent*> vars) {
    double total_sum=0;
    for( size_t d=0;d<a_latent.nrow;d++){
        for( size_t  k=0; k<a_latent.ncol;k++){
            total_sum+=gamma_term(a,b,a_latent(d,k),b_latent(k),e_expected(d,k),elog_expected(d,k));
            for(gamma_latent* var : vars){
                if(var)
                    total_sum+=gamma_term(var->a,var->b,var->a_latent(d,k),var->b_latent(k),var->e_expected(d,k),var->elog_expected(d,k));
            }
        }
    }
    return total_sum;
}

gamma_latent::gamma_latent(const Arrayf &a_latent, const Arrayf &b_latent, const Arrayf &e_expected,
                           const Arrayf &elog_expected, double a, double b) :
        a_latent(a_latent), b_latent(b_latent),
        e_expected(e_expected),
        elog_expected(elog_expected), a(a), b(b),
        nvars(a_latent.nrow),kdim(a_latent.ncol){}

void gamma_latent::init_b_latent() {
    b_latent=b;
}

void gamma_latent::init_a_latent() {
    a_latent=a;

}

double gamma_latent::elbo_term_prod_linear_expectations(vector<gamma_latent *> vars) {
    double total_sum=0;

    for(gamma_latent* var : vars)
    {
        if(var)
        {

            for( size_t  j=0;j<(var->nvars);j++)
            {
                for (int k = 0; k < kdim; ++k)
                {
                    auto expjk=var->e_expected(j,k);
                    for( size_t  i=0;i<nvars;i++)
                        total_sum+=e_expected(i,k)*expjk;
                }
            }
        }
    }

    return (-total_sum);
}

gamma_latent::gamma_latent( ArrayManager<double>* arrman, size_t nrows, size_t ncols, double a, double b):
        a(a), b(b), a_latent(arrman->makeArray(nrows,ncols,a)),b_latent(arrman->makeArray(ncols,b)),
        e_expected(arrman->makeArray(nrows,ncols)),elog_expected(arrman->makeArray(nrows,ncols)),
        nvars(nrows),kdim(ncols)
{

}


/*
 * world = K [R | t]
 */
#include <Eigen/Dense>
#include <iostream>
#include <math.h>

struct rgbpoint
{
	rgbpoint(): r(0),g(0),b(0) {}
	char r,g,b;
} __attribute__((packed));

// [sx 0 cx]
// [0 sy cy]
// [0 0 1]
//
// to 4x4
void intrinsics2m44(Eigen::Matrix4d & out, Eigen::Matrix3d & in)
{
	out.setZero();
	out.block<3,3>(0,0) = in;
	out(3,3) = 1;

//	out << in(0,0),in(0,1),0,in(0,2)    ,in(1,0),in(1,1),0,in (1,2),   0,0,1,0,   0,0,0,1;
}

void composerotot(Eigen::Matrix4d& out, const Eigen::Matrix3d & R, const Eigen::Vector3d & T)
{
		out.setZero();
		out.block<3,3>(0,0) = R;
		out.block<3,1>(0,3) = T;
		out(3,3) = 1;

}

#ifdef __MINGW32__
#define DE __declspec(dllexport)
#else
#define DE

#endif

extern "C"
{
	void DE initlibanyregistration(){}
	/// output is color computed as depth in sizes
	void DE register2color(char * outrgb, int cw, int ch, const char * rgb, int dw, int dh, const uint16_t * depth, const double depthK[9], const double colorK[9], const double inrotation[9], const double position[3])
	{
		static_assert(sizeof(rgbpoint) == 3,"rgpoint should be 3 bytes");

		float maxdistance_mm = 10000;

		Eigen::Matrix3d rotation = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(inrotation);
		Eigen::Vector3d translation = Eigen::Vector3d::Map(position);
		Eigen::Matrix3d depth_matrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(depthK);
		Eigen::Matrix3d rgb_matrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(colorK);

		std::cout << "Rotation is\n " << rotation << std::endl;
		std::cout << "Kdepth is\n" << depth_matrix << std::endl;

		Eigen::Matrix4d rototranslationD2R;
		Eigen::Matrix4d odepth_matrix;
		Eigen::Matrix4d orgb_matrix;

		intrinsics2m44(odepth_matrix,depth_matrix);
		intrinsics2m44(orgb_matrix,rgb_matrix);
		composerotot(rototranslationD2R,rotation,translation);

		std::cout << "Kdepth4 is\n" << odepth_matrix << std::endl;

		std::cout << "rototraslation(W->D)\n" << rototranslationD2R << std::endl;

		Eigen::Matrix4d depth2rgb4 = orgb_matrix * rototranslationD2R * odepth_matrix.inverse();
//		Eigen::Matrix4d depth2rgb4 = orgb_matrix * (odepth_matrix*rototranslationD2R).inverse();

		std::cout << "depth2rgb4\n" << depth2rgb4 << std::endl;

		const  uint16_t * inputdepth_mm = depth;
		rgbpoint * out = (rgbpoint*)outrgb;
		const rgbpoint * inputcolor = (rgbpoint*)rgb;

		memset(out,0,dw*dh*3);

		for(int y = 0; y < dh; ++y)
		{
			for(size_t x = 0; x < dw; ++x, inputdepth_mm++,out++)
			{
				if(!*inputdepth_mm)
					continue;

				// Check for invalid measurements
				if(*inputdepth_mm >= maxdistance_mm)
					continue;

				const float depth_value = *inputdepth_mm / 1000.0f;

				//Eigen::Vector4d psd(x, y, 1.0, 1.0/depth_value);
				//Eigen::Vector4d psddiv = psd * depth_value;
				//Eigen::Vector4d pworld = depth2world * psddiv;
				//Eigen::Vector4d rgb_img_homo = world2rgb * pworld;

				Eigen::Vector4d psd(x, y, 1.0, 1.0/depth_value);
				Eigen::Vector4d rgb_img_homo = depth2rgb4 * (psd * depth_value);
				Eigen::Vector4d rgb_img = rgb_img_homo / rgb_img_homo.z();

				int ix = rgb_img.x();
				int iy = rgb_img.y();

				if(ix >= 0 && ix < cw && iy >= 0 && iy < ch)
				{
					*out = inputcolor[iy*cw+ix];
				}
			}
		}
	}

	// NOT WORKING DUE TO FILLING
	/// output is depth computed as color in size
	void DE register2depth(uint16_t * outdepth, int cw, int ch, const char * rgb, int dw, int dh, const uint16_t * depth, const double depthK[9], const double colorK[9], const double inrotation[9], const double position[3])
	{
		float maxdistance = 10;

		Eigen::Matrix3d rotation = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(inrotation);
		Eigen::Vector3d translation = Eigen::Vector3d::Map(position);
		Eigen::Matrix3d depth_matrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(depthK);
		Eigen::Matrix3d rgb_matrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>::Map(colorK);

		Eigen::Matrix4d rototranslationD2R;
		Eigen::Matrix4d orgb_matrix,odepth_matrix;

		intrinsics2m44(odepth_matrix,depth_matrix);
		intrinsics2m44(orgb_matrix,rgb_matrix);
		composerotot(rototranslationD2R,rotation,-translation);

		//odepth_matrix.block<3,3>(0,0) = depth_matrix;
		//odepth_matrix(3,3) = 1;

		//orgb_matrix.block<3,3>(0,0) = rgb_matrix;
		//orgb_matrix(3,3) = 1;

		std::cout << rototranslationD2R << std::endl;

		Eigen::Matrix4d depth2rgb4 = orgb_matrix * rototranslationD2R * odepth_matrix.inverse();
		Eigen::Matrix3d rgb2depth = depth2rgb4.block<3,3>(0,0).inverse();

		memset(outdepth,0,cw*ch*2); // cleanup output (max value)
		auto po = outdepth;

		for(int y = 0; y < ch; ++y)
		{
			for(size_t x = 0; x < cw; ++x,++po)
			{

				Eigen::Vector3d rgb_i(x,y,1); // 2D homo
				Eigen::Vector3d depth_sub_i = rgb2depth * rgb_i; // to 2D homo
				int ix = depth_sub_i.x();
				int iy = depth_sub_i.y();

				// if inside, and valid and nearer than the current
				if(ix >= 0 && ix  < dw && iy >= 0 && iy < dh )
				{
					 int off = iy*dw+ix;
					 uint16_t current = depth[off];
					 *po = current;
					 //uint16_t *target = outdepth + off; // target output
					 //if(current > 0 && (*target == 0 || *target > current))
					 //*target = current;
				}

				/*
					To obtain world coords
					Eigen::Vector4d psd(depth_sub_i.x(),depth_sub_i.y(), 1.0, 1.0/newdepth);
					Eigen::Vector4d psddiv = psd * newdepth; // (x*newdepth,y*newdepth,newdepth,1)
					Eigen::Vector4d pworld =  depth2world  * psddiv;
				*/
			}
		}
	}
}


#include <armadillo>

using namespace std;
using namespace arma;

namespace enjambre {

class Individuo {
public:
    Individuo(const vector<pair<double, double>>& limites);

    vec getPosicion() const { return posicion; }
    void setPosicion(const vec& value);

    vec getVelocidad() const { return velocidad; }
    void setVelocidad(const vec& value);

    vec getMejorLocal() const { return mejorLocal; }
    void setMejorLocal(const vec& value);

    double aptitudMejorLocal;

    Individuo& operator=(const Individuo& other);

private:
    const vector<pair<double, double>> m_limites;
    vec posicion;
    vec velocidad;
    vec mejorLocal;
};

Individuo::Individuo(const vector<pair<double, double>>& limites)
    : aptitudMejorLocal{-numeric_limits<double>::max()}
    , m_limites{limites}
{
    const unsigned int nVariables = m_limites.size();

    posicion = randu(nVariables);

    if (m_limites.size() != nVariables)
        throw runtime_error("Número de límites no concuerda con la cantidad de variables");

    for (unsigned int i = 0; i < posicion.n_elem; ++i) {
        double rangoVariable = m_limites.at(i).second - m_limites.at(i).first;

        posicion(i) = posicion(i) * rangoVariable + m_limites.at(i).first;
    }

    velocidad = zeros(nVariables);
    mejorLocal = posicion;
}

void Individuo::setPosicion(const vec& value)
{
    if (value.n_elem != this->posicion.n_elem)
        throw runtime_error("La dimensión del vector es incorrecta");

    for (unsigned int i = 0; i < m_limites.size(); ++i) {
        if (value.at(i) > m_limites.at(i).second)
            posicion.at(i) = m_limites.at(i).second; // Nos pasamos de largo por derecha
        else if (value.at(i) < m_limites.at(i).first)
            posicion.at(i) = m_limites.at(i).first; // Nos pasamos de largo por izquierda
        else
            posicion.at(i) = value.at(i); // No nos pasamos de largo
    }
}

void Individuo::setVelocidad(const vec& value)
{
    if (value.n_elem != this->velocidad.n_elem)
        throw runtime_error("La dimensión del vector es incorrecta");

    velocidad = value;
}

void Individuo::setMejorLocal(const vec& value)
{
    if (value.n_elem != this->posicion.n_elem)
        throw runtime_error("La dimensión del vector es incorrecta");

    mejorLocal = value;
}

Individuo& Individuo::operator=(const Individuo& other)
{
    if (other.m_limites != this->m_limites)
        throw runtime_error("Los individuos tienen distintos limites en las variables");

    this->posicion = other.posicion;
    this->velocidad = other.velocidad;
    this->mejorLocal = other.mejorLocal;
    this->aptitudMejorLocal = other.aptitudMejorLocal;

    return *this;
}

class Enjambre {
public:
    Enjambre(function<double(Individuo)> fitness,
             const vector<pair<double, double>>& limites,
             const double c1,
             const double c2,
             unsigned int nIndividuos,
             int umbral);

    bool evaluarPoblacion();
    void epoca();
    const vector<Individuo>& individuos() const { return m_individuos; }
    const Individuo& mejorGlobal() const { return m_mejorGlobal; }
    double fitnessPromedio() const { return m_fitnessPromedio; }
    bool termino() const { return m_termino; }

private:
    vector<Individuo> m_individuos;
    Individuo m_mejorGlobal;
    double m_mejorAptitud;
    double m_fitnessPromedio;
    function<double(Individuo)> m_fitness;
    const double m_c1, m_c2;
    int m_umbral;
    int m_epocasSinMejora;
    bool m_termino;
};

Enjambre::Enjambre(function<double(Individuo)> fitness,
                   const vector<pair<double, double>>& limites,
                   const double c1,
                   const double c2,
                   unsigned int nIndividuos,
                   int umbral)
    : m_mejorGlobal{limites}
    , m_mejorAptitud{-numeric_limits<double>::max()}
    , m_fitness{fitness}
    , m_c1{c1}
    , m_c2{c2}
    , m_umbral{umbral}
    , m_epocasSinMejora{0}
    , m_termino{false}
{
    for (unsigned int i = 0; i < nIndividuos; ++i) {
        m_individuos.push_back(Individuo{limites});

        evaluarPoblacion();
    }
}

bool Enjambre::evaluarPoblacion()
{
    bool mejoro = false;
    double sumaFitness = 0;

    for (Individuo& ind : m_individuos) {
        double aptitud = m_fitness(ind);
        sumaFitness += aptitud;

        if (aptitud > ind.aptitudMejorLocal) {
            ind.setMejorLocal(ind.getPosicion());
            ind.aptitudMejorLocal = aptitud;

            if (aptitud > m_mejorAptitud) {
                m_mejorGlobal = ind;
                m_mejorAptitud = aptitud;
                mejoro = true;
            }
        }
    }

    m_fitnessPromedio = sumaFitness / m_individuos.size();

    return mejoro;
}

void Enjambre::epoca()
{
    const int nVariables = m_individuos.front().getPosicion().n_elem;

    for (Individuo& ind : m_individuos) {
        // Cálculo de la nueva velocidad
        // Componente cognitiva
        const vec r1 = randu(nVariables);
        const vec distanciaMejorLocal = ind.getMejorLocal() - ind.getPosicion();

        const vec compCognitiva = m_c1 * r1 % distanciaMejorLocal;

        // Componente social
        const vec r2 = randu(nVariables);
        const vec distanciaMejorGlobal = m_mejorGlobal.getPosicion() - ind.getPosicion();

        const vec compSocial = m_c2 * r2 % distanciaMejorGlobal;

        // Actualización
        ind.setVelocidad(ind.getVelocidad() + compCognitiva + compSocial);
        ind.setPosicion(ind.getPosicion() + ind.getVelocidad());
    }

    if (evaluarPoblacion())
        m_epocasSinMejora = 0;
    else {
        if (m_epocasSinMejora == m_umbral)
            m_termino = true;

        ++m_epocasSinMejora;
    }
}
} // namespace enjambre


/** \file   cmr_t2_mapping.cpp
    \brief  Implement CMR T1 mapping for 2D acquisition
    \author Hui Xue
*/

#include "cmr_t2_mapping.h"
#include "log.h"

#include "hoNDArray_reductions.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_math.h"
#include "hoNDArray_linalg.h"

#include "simplexLagariaSolver.h"
#include "twoParaExpDecayOperator.h"
#include "curveFittingCostFunction.h"

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

namespace Gadgetron {

template <typename T>
CmrT2Mapping<T>::CmrT2Mapping() : BaseClass()
{
    max_iter_ = 150;
    max_fun_eval_ = 1000;
    thres_fun_ = 1e-4;

    max_map_value_ = 2500;
}

template <typename T>
CmrT2Mapping<T>::~CmrT2Mapping()
{
}

template <typename T>
void CmrT2Mapping<T>::get_initial_guess(const VectorType& ti, const VectorType& yi, VectorType& guess)
{
    if (guess.size() != this->get_num_of_paras())
    {
        guess.resize(this->get_num_of_paras(), 0);
    }

    // do a log linear fit
    size_t numpts = yi.size();
    GADGET_CHECK_THROW(numpts>0);

    T default_A = *std::max_element(yi.begin(), yi.end());
    T default_T2 = ti[ti.size() / 2];;

    try
    {
        size_t i;
        for (i = 0; i < numpts; i++)
        {
            if(yi[i]<=0)
            {
                guess[0] = default_A;
                guess[1] = default_T2;
                return;
            }
        }

        hoNDArray<T> log_yi(numpts), bi_est(2);
        hoNDArray<T> xi(numpts);
        for (size_t i = 0; i < numpts; i++)
        {
            xi[i] = ti[i];
            log_yi[i] = std::log(yi[i]);
        }

        T a, b;
        Gadgetron::linFit(xi, log_yi, a, b);

        guess[0] = std::exp(b);
        guess[1] = -1.0 / a;
    }
    catch(...)
    {
        guess[0] = default_A;
        guess[1] = default_T2;
    }

    if(guess[1]<0)
    {
        guess[0] = default_A;
        guess[1] = default_T2;
    }
}

template <typename T>
void CmrT2Mapping<T>::compute_map(const VectorType& ti, const VectorType& yi, const VectorType& guess, VectorType& bi, T& map_v)
{
    try
    {
        bi = guess;
        map_v = 0;

        typedef Gadgetron::twoParaExpDecayOperator< std::vector<T> > SignalType;
        typedef Gadgetron::leastSquareErrorCostFunction< std::vector<T> > CostType;

        // define solver
        Gadgetron::simplexLagariaSolver< VectorType, SignalType, CostType > solver;

        // define signal model
        SignalType t2;

        // define cost function
        CostType lse;

        solver.signal_model_ = &t2;
        solver.cf_ = &lse;

        solver.max_iter_ = max_iter_;
        solver.max_fun_eval_ = max_fun_eval_;
        solver.thres_fun_ = thres_fun_;

        solver.x_ = ti;
        solver.y_ = yi;

        solver.solve(bi, guess);

        if (bi[0] > 0 && bi[1] > 0)
        {
            map_v = bi[1];
            if (map_v >= max_map_value_) map_v = hole_marking_value_;
            if (map_v <= min_map_value_) map_v = hole_marking_value_;
        }
    }
    catch (...)
    {
        GADGET_THROW("Exceptions happened in CmrT2Mapping<T>::compute_map(...) ... ");
    }
}

template <typename T>
void CmrT2Mapping<T>::compute_sd(const VectorType& ti, const VectorType& yi, const VectorType& bi, VectorType& sd, T& map_sd)
{
    try
    {
        sd.clear();
        sd.resize(bi.size(), 0);

        map_sd = 0;

        typedef Gadgetron::twoParaExpDecayOperator< std::vector<T> > SignalType;
        SignalType t2;

        // compute fitting values
        VectorType y;
        t2.magnitude(ti, bi, y);

        // compute residual
        VectorType res(y), abs_res(y);

        size_t num = ti.size();
        size_t N = this->get_num_of_paras();

        size_t n;
        for (n = 0; n < num; n++)
        {
            res[n] = y[n] - yi[n];
            abs_res[n] = std::abs(res[n]);
        }

        hoNDArray<T> grad;
        grad.create(N, num);
        Gadgetron::clear(grad);

        VectorType gradVec(N);
        for (n = 0; n < num; n++)
        {
            t2.gradient(ti[n], bi, gradVec);
            memcpy(grad.begin() + n*N, &gradVec[0], sizeof(T)*N);
        }

        GADGET_CATCH_THROW(this->compute_sd_impl(ti, yi, bi, abs_res, grad, sd));

        map_sd = sd[1];
        if (map_sd > max_map_value_) map_sd = this->hole_marking_value_;
    }
    catch (...)
    {
        GADGET_THROW("Exceptions happened in CmrT2Mapping<T>::compute_map(...) ... ");
    }
}

template <typename T>
size_t CmrT2Mapping<T>::get_num_of_paras() const
{
    return 2; // A and T2
}

// ------------------------------------------------------------
// Instantiation
// ------------------------------------------------------------

template class EXPORTCMR CmrT2Mapping< float >;

}


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

#ifndef MTL_ORTH_INCLUDE
#define MTL_ORTH_INCLUDE

#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/operation/size1D.hpp>
#include <boost/numeric/mtl/operation/entry1D.hpp>
#include <boost/numeric/mtl/operation/dot.hpp>
#include <boost/numeric/mtl/operation/two_norm.hpp>
#include <boost/numeric/mtl/operation/is_negative.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vec {

    namespace impl {

	template <typename VVector>
	inline void orth(VVector& v, typename mtl::Collection<VVector>::size_type j, tag::vector)
	{
		vampir_trace<2018> tracer;
	    using mtl::two_norm; using mtl::size1D;
	    MTL_DEBUG_THROW_IF(is_negative(j) || j >= size1D(v), index_out_of_range());

	    typedef typename mtl::Collection<VVector>::size_type  Size;
	    for (Size i= 0; i < j; ++i)
		entry1D(v, j)-= dot(entry1D(v, i), entry1D(v, j)) * entry1D(v, i);
	    entry1D(v, j)/= two_norm(entry1D(v, j));
	}

	template <typename VVector>
	inline void orth(VVector& v, tag::vector)
	{
	    typedef typename mtl::Collection<VVector>::size_type  Size;
	    using mtl::size1D;
	    for (Size j= 0; j < size1D(v); ++j)
		orth(v, j, tag::vector());
	}

	template <typename VVector>
	mtl::mat::dense2D<typename mtl::Collection
		   <typename mtl::Collection<VVector>::value_type
		    >::value_type, mat::parameters<> >
	inline orthogonalize_factors(VVector& v, tag::vector)
	{
	    vampir_trace<2019> tracer;
	    using ::mtl::two_norm; using math::zero; using mtl::size1D;
	    typedef typename mtl::Collection<VVector>::size_type  Size;
	    typedef typename mtl::Collection<VVector>::value_type Vector;
	    typedef typename mtl::Collection<Vector>::value_type  Scalar;

	    mtl::mat::dense2D<Scalar, mat::parameters<> > tau(size1D(v), size1D(v));
	    tau= zero(Scalar());

	    if (size1D(v) == 0)
		return tau;

	    tau[0][0]= dot(entry1D(v, 0), entry1D(v, 0));

	    for (Size j= 1; j < size1D(v); ++j) {
#ifdef MTL_WITH_FUSED_ORTHOGONALIZATION
		Scalar t= dot(entry1D(v, 0), entry1D(v, j)) / tau[0][0], t2;
		tau[0][j]= t;
		for (Size i= 1; i < j; ++i) {
		    (lazy(entry1D(v, j))-= t * entry1D(v, i-1)) || (lazy(t2)= lazy_dot(entry1D(v, i), entry1D(v, j)));
		    t= tau[i][j]= t2 / tau[i][i];
		}
		entry1D(v, j)-= t * entry1D(v, j-1);
#else
		for (Size i= 0; i < j; ++i) {
		    Scalar t= dot(entry1D(v, i), entry1D(v, j)) / tau[i][i];
		    tau[i][j]= t;
		    entry1D(v, j)-= t * entry1D(v, i);
		}
#endif
		tau[j][j]= dot(entry1D(v, j), entry1D(v, j));
	    }
	    return tau;
	}

    } // impl

/*! Orthonormalize a vector of vectors.

    The outer type must be a random access collection and
    the vector type must provide a dot function.
    For instance dense_vector<dense_vector<double> > or
    std::vector<dense_vector<std::complex<double> > > are eligible.
    It is planned to implement the function for matrices as well
    where the columns will be ortho-normalized.
**/
template <typename Value>
inline void orth(Value& value)
{
    impl::orth(value, typename traits::category<Value>::type());
}

/*! Orthonormalize the i-th entry of a vector of vectors.

    The i-th vector is orthogonalized w.r.t. to the preceeding ones and
    consecutively normalized.
    The outer type must be a random access collection and
    the vector type must provide a dot function.
    For instance dense_vector<dense_vector<double> > or
    std::vector<dense_vector<std::complex<double> > > are eligible.
    It is planned to implement the function for matrices as well
    where the columns will be ortho-normalized.
**/
template <typename Value>
inline void orth(Value& value, typename mtl::Collection<Value>::size_type i)
{
    impl::orth(value, i, typename traits::category<Value>::type());
}

/*! Orthogonalize a vector of vectors.

    Opposed to orth the vectors are not normalized.
    An upper matrix with the factors used in the orthogonalization is returned.
    The diagonal contains dot(v[i], v[i]).
    The returned factors are for instance used in bicgstab_ell.
    The outer type must be a random access collection and
    the vector type must provide a dot function.
    For instance dense_vector<dense_vector<double> > or
    std::vector<dense_vector<std::complex<double> > > are eligible.
**/
template <typename Value>
mtl::mat::dense2D<typename mtl::Collection
	<typename mtl::Collection<Value>::value_type
	 >::value_type, mat::parameters<>  >
inline orthogonalize_factors(Value& v)
{
    return impl::orthogonalize_factors(v, typename traits::category<Value>::type());
}

} // namespace vector

namespace mat {

    // If other matrix types will be supported within a template function, it needs reimplementation!!!
    template <typename Vector>
    inline void orth(multi_vector<Vector>& A)
    {
	mtl::vec::impl::orth(A, mtl::tag::vector());
    }
}

using vec::orth;
using vec::orthogonalize_factors;

} // namespace mtl

#endif // MTL_ORTH_INCLUDE


#define DEBUG 1
/**
 * File    : E.cpp
 * Author  : Kazune Takahashi
 * Created : 4/23/2020, 7:08:47 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint &operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint &operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- Point -----

using Point = tuple<int, int>;

ostream &operator<<(ostream &os, Point const &pt)
{
  return os << get<0>(pt) << " " << get<1>(pt);
}

// ----- Cycle -----

class Cycle
{
  int num;
  bool pos;

public:
  Cycle(int num, bool pos) : num{num}, pos{pos} {}

  vector<Point> path() const
  {
    vector<Point> ans;
    for (auto i = 0; i <= num; ++i)
    {
      ans.push_back(Point(i, 0));
    }
    if (pos)
    {
      ans.push_back(Point(num + 1, 0));
      ans.push_back(Point(num + 1, 1));
      ans.push_back(Point(num, 1));
    }
    else
    {
      ans.push_back(Point(num, 1));
      ans.push_back(Point(num + 1, 1));
      ans.push_back(Point(num + 1, 0));
    }
    for (auto i = num; i >= 1; --i)
    {
      ans.push_back(Point(i, 0));
    }
    return ans;
  }

  Cycle inv() const
  {
    return Cycle(num, !pos);
  }
};

class Chain
{
  vector<Cycle> V;

public:
  Chain() {}
  Chain(Cycle c) : V{c} {}
  Chain(vector<Cycle> V) : V(V) {}

  Chain(int mask)
  {
    int cnt{0};
    while ((mask >> cnt) != 0)
    {
      if ((mask >> cnt) & 1)
      {
        merge(cnt);
      }
      ++cnt;
    }
  }

  vector<Point> path() const
  {
    vector<Point> ans;
    for (auto const &e : V)
    {
      auto tmp{e.path()};
      copy(tmp.begin(), tmp.end(), back_inserter(ans));
    }
    return ans;
  }

  vector<Cycle> const &seq() const
  {
    return V;
  }

  void operator+=(Chain const &other)
  {
    copy(other.seq().begin(), other.seq().end(), back_inserter(V));
  }

private:
  void merge(int n)
  {
    if (V.empty())
    {
      V.push_back(Cycle{n, true});
    }
    else
    {
      auto inverse{inv()};
      *this += Chain{Cycle{n, true}};
      *this += inverse;
      *this += Chain{Cycle{n, false}};
    }
  }

  Chain inv() const
  {
    vector<Cycle> W;
    for (auto it = V.rbegin(); it != V.rend(); ++it)
    {
      W.push_back(it->inv());
    }
    return Chain(W);
  }
};

// ----- Solve -----

class Solve
{
  int N;
  vector<bool> V;
  Chain C;
  bool possible;

public:
  Solve(int N, string A) : N{N}, V(1 << N), possible{true}
  {
    for (auto i = 0; i < 1 << N; ++i)
    {
      V[i] = (A[i] == '1');
    }
  }

  void answer()
  {
    if (check())
    {
      construct();
    }
    else
    {
      possible = false;
    }
    flush();
  }

private:
  bool check() const
  {
    if (!V[0])
    {
      return false;
    }
    for (auto i = 0; i < 1 << N; ++i)
    {
      for (auto j = 0; j < 1 << N; ++j)
      {
        if (i != j && (i & j) == j)
        {
          if (!V[j] && V[i])
          {
            return false;
          }
        }
      }
    }
    return true;
  }

  void construct()
  {
    for (auto i = 0; i < 1 << N; ++i)
    {
      if (V[i])
      {
        continue;
      }
      bool create{true};
      for (auto j = 0; j < 1 << N; ++j)
      {
        if (i != j && (i & j) == j && !V[j])
        {
          create = false;
          break;
        }
      }
      if (create)
      {
        C += Chain{i};
      }
    }
  }

  void flush() const
  {
    if (possible)
    {
      cout << "Possible" << endl;
    }
    else
    {
      cout << "Impossible" << endl;
      return;
    }
    auto tmp{C.path()};
    cout << tmp.size() << endl;
    tmp.push_back(Point(0, 0));
    for (auto const &e : tmp)
    {
      cout << e << endl;
    }
  }
};

// ----- main() -----

int main()
{
  int N;
  cin >> N;
  string A;
  cin >> A;
  Solve solve(N, A);
  solve.answer();
}


// inverting A
// using getrf() & getri()

//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <cstddef>
#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level3.hpp>
#include <boost/numeric/bindings/lapack/computational/getri.hpp>
#include <boost/numeric/bindings/lapack/computational/getrf.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>
#include "utils.h"

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

using std::size_t;
using std::cout;
using std::endl;

typedef std::complex<double> cmpx;

#ifndef F_ROW_MAJOR
typedef ublas::matrix<double, ublas::column_major> m_t;
typedef ublas::matrix<cmpx, ublas::column_major> cm_t;
#else
typedef ublas::matrix<double, ublas::row_major> m_t;
typedef ublas::matrix<cmpx, ublas::row_major> cm_t;
#endif

int main() {

  cout << endl;
  cout << "real matrix:" << endl << endl;

  size_t n = 5;
  m_t a (n, n);
  init_symm (a);
  //     [n   n-1 n-2  ... 1]
  //     [n-1 n   n-1  ... 2]
  // a = [n-2 n-1 n    ... 3]
  //     [        ...       ]
  //     [1   2   ...  n-1 n]
  print_m (a, "A");
  cout << endl;

  m_t aa (a);  // copy of a, for later use

  std::vector<int> ipiv (n);   // pivot vector
  lapack::getrf (a, ipiv);  // no lu_factor() alias for getrf() available
  lapack::getri (a, ipiv);  // no lu_invert() alias for getrf() available

  m_t i1 (n, n), i2 (n, n);
  blas::gemm (1.0, a, aa, 0.0, i1);   // i1 should be (almost) identity matrix
  blas::gemm (1.0, aa, a, 0.0, i2);   // i2 should be (almost) identity matrix

  print_m (i1, "I = A^(-1) * A");
  cout << endl;
  print_m (i2, "I = A * A^(-1)");
  cout << endl;

  cout << endl;

  //////////////////////////////////////////////////////

  cout << "complex matrix:" << endl << endl;
  cm_t ca (3, 3);

  ca (0, 0) = cmpx (3, 0);
  ca (0, 1) = cmpx (4, 2);
  ca (0, 2) = cmpx (-7, 5);
  ca (1, 0) = cmpx (4, -2);
  ca (1, 1) = cmpx (-5, 0);
  ca (1, 2) = cmpx (0, -3);
  ca (2, 0) = cmpx (-7, -5);
  ca (2, 1) = cmpx (0, 3);
  ca (2, 2) = cmpx (2, 0);
  print_m (ca, "CA");
  cout << endl;

  cm_t caa (ca);

  std::vector<int> ipiv2 (3);

  int ierr = lapack::getrf (ca, ipiv2);
  if (ierr == 0) {
    lapack::getri (ca, ipiv2);
    cm_t ii (3, 3);
    blas::gemm (1.0, ca, caa, 0.0, ii);
    print_m (ii, "I = CA^(-1) * CA");
    cout << endl;
    blas::gemm (1.0, caa, ca, 0.0, ii);
    print_m (ii, "I = CA * CA^(-1)");
    cout << endl;
  }
  else
    cout << "matrix is singular" << endl;

  cout << endl;

}


// BLAS level 3

//#define F_USE_STD_VECTOR

//#define BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
//#define BOOST_NO_FUNCTION_TEMPLATE_ORDERING

#include <iostream>
#include <boost/numeric/bindings/atlas/cblas3.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#ifdef F_USE_STD_VECTOR
#include <vector>
#include <boost/numeric/bindings/traits/std_vector.hpp>
#endif
#include "utils.h"

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

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

#ifndef F_USE_STD_VECTOR
typedef ublas::matrix<double, ublas::row_major> m_t;
#else
typedef ublas::matrix<double, ublas::column_major, std::vector<double> > m_t;
#endif

int main() {

  cout << endl;

  m_t a (4, 4);
  init_m (a, kpp (1));
  print_m (a, "a");
  cout << endl;

  m_t b (4, 6);
  init_m (b, cls1());
  print_m (b, "b");
  cout << endl;

  m_t c (4, 6);

  // c = a b
  atlas::gemm (a, b, c);
  print_m (c, "c = a b");
  cout << endl;
  atlas::gemm (CblasNoTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "c = a b");
  cout << endl;

  init_m (c, const_val<double> (1));
  print_m (c, "c");
  cout << endl;
  // c = 2 a b + 0.5 c
  atlas::gemm (2.0, a, b, 0.05, c);
  print_m (c, "c = 2 a b + 0.05 c");
  cout << endl;

  m_t d (6, 4);

  // d = b^T a^T
  atlas::gemm (CblasTrans, CblasTrans, 1.0, b, a, 0.0, d);
  print_m (d, "d = b^T a^T");
  cout << endl;

  // c = a^T b
  atlas::gemm (CblasTrans, CblasNoTrans, 1.0, a, b, 0.0, c);
  print_m (c, "c = a^T b");
  cout << endl;

  // d = b^T a
  atlas::gemm (CblasTrans, CblasNoTrans, 1.0, b, a, 0.0, d);
  print_m (d, "d = b^T a");
  cout << endl;

  init_m (d, const_val<double> (0));
  ublas::matrix_range<m_t> br (b, ublas::range (0, 4), ublas::range (0, 4));
  ublas::matrix_range<m_t> dr (d, ublas::range (1, 5), ublas::range (0, 4));

  // d[1..5][0..4] = a b[0..4][0..4]
  atlas::gemm (a, br, dr);
  print_m (d, "d[1..5][0..4] = a b[0..4][0..4]");
  cout << endl;

  // d[1..5][0..4] = b[0..4][0..4] a
  atlas::gemm (br, a, dr);
  print_m (d, "d[1..5][0..4] = b[0..4][0..4] a");
  cout << endl;

  // d[1..5][0..4] = b[0..4][0..4] a^T
  atlas::gemm (CblasNoTrans, CblasTrans, 1.0, br, a, 0.0, dr);
  print_m (d, "d[1..5][0..4] = b[0..4][0..4] a^T");
  cout << endl;

  // d[1..5][0..4] = a b[0..4][0..4]^T
  atlas::gemm (CblasNoTrans, CblasTrans, 1.0, a, br, 0.0, dr);
  print_m (d, "d[1..5][0..4] = a b[0..4][0..4]^T");
  cout << endl;

}


#include <iostream>
#include <Eigen/Dense>
#include <ctime>
#include <cassert>
#include "chrono.h"

double dot1(const Eigen::VectorXd &vec1, const Eigen::VectorXd &vec2) {
    assert(vec1.size() == vec2.size() && "Vectors must have the same size");
    double result = 0.0f;
    for (size_t i = 0; i < vec1.size(); ++i) {
        result += vec1[i] * vec2[i];
    }
    return result;
}

Eigen::MatrixXd matProduct(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) {
    assert(mat1.size() == mat2.size() && "Matrices must have the same size");
    Eigen::MatrixXd result = Eigen::MatrixXd::Zero(mat1.rows(), mat1.cols());

    for (size_t i = 0; i < result.rows(); i++) {
        for (size_t j = 0; j < result.cols(); j++) {
            for (size_t k = 0; k < mat1.cols(); k++) {
                result(i, j) += mat1(i, k) * mat2(k, j);
            }
        }
    }

    return result;
}

int main() {
    const size_t dimension = 500;
    TP_CPP_IMAC2::Chrono chrono;

    Eigen::MatrixXd A = Eigen::MatrixXd::Random(dimension, dimension);
    Eigen::MatrixXd B = Eigen::MatrixXd::Random(dimension, dimension);

    chrono.start();
    Eigen::MatrixXd eingenResult = A * B;
    std::cout << "Eignen mean : " << chrono.timeSpan() << std::endl;
    chrono.stop();

    chrono.start();
    Eigen::MatrixXd myResult = matProduct(A, B);
    std::cout << "My mean : " << chrono.timeSpan() << std::endl;
    chrono.stop();

    std::cout << (eingenResult - myResult).norm() << std::endl;

    /*

    chrono.start();
    std::cout << "mean : " << chrono.timeSpan() << std::endl;
    chrono.stop();

    Eigen::VectorXd vec1 = Eigen::VectorXd::Random(dimension);
    Eigen::VectorXd vec2 = Eigen::VectorXd::Random(dimension);

    std::cout << "My dot1 : " << dot1(vec1, vec2) << std::endl;
    std::cout << "My dot2 : " << dot2(vec1, vec2) << std::endl;
    std::cout << "Eignen dot : " << vec1.dot(vec2) << std::endl;
    */

    /*
    Eigen::VectorXd v1(5);
    v1 << 1, 2, 3, 4, 5;
    std::cout << "v1 : " << v1.transpose() << std::endl
              << std::endl;

    Eigen::Vector4f v2 = Eigen::Vector4f::Zero();
    std::cout << "v2 : " << v2.transpose() << std::endl
              << std::endl;

    v2 = Eigen::Vector4f::Ones();
    std::cout << "v2 : " << v2.transpose() << std::endl
              << std::endl;

    Eigen::Vector4f v3 = Eigen::Vector4f::Random();
    std::cout << "v3 : " << v3.transpose() << std::endl
              << std::endl;
    v3 = v3 + v2;
    std::cout << "v3 : " << v3.transpose() << std::endl
              << std::endl;

    Eigen::MatrixXd A = Eigen::MatrixXd::Random(3, 4);
    std::cout << "A :\n"
              << A << std::endl
              << std::endl;

    Eigen::Matrix4d B = Eigen::Matrix4d::Random(4, 4);
    std::cout << "B :\n"
              << B << std::endl
              << std::endl;

    Eigen::MatrixXd C(3, 4);

    clock_t begin = clock();
    C = A * B;
    clock_t end = clock();
    double tempsCalc = double(end - begin) / CLOCKS_PER_SEC;

    std::cout << "temps calcul du produit matriciel: " << tempsCalc << "s " << std::endl;
    std::cout << "A + 2*A :\n"
              << A + 2 * A << std::endl
              << std::endl;
    std::cout << "A * B :\n"
              << A * B << std::endl
              << std::endl;

              */

    return 0;
}


#if HAVE_CONFIG_H
# include <config.h>
#endif

#include "HigherStat.h"
#include "PowerSpectrum.h"
#include "LinearPS.h"
#include "Quadrature.h"
#include "SpecialFunctions.h"
#include "array.h"
#include "Spline.h"
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_deriv.h>

#include <cassert>
#include <cmath>
#include <cstdio>
#include <cerrno>
#include <cstdlib>
#include <stdlib.h>
#include <sys/stat.h>
#include <math.h>
#include <vector>
#include <boost/bind.hpp>
using boost::cref;

const real KMIN_e = 0.0001;
const real KMAX_e = 30.;

// Load Linear PS
HigherStat::HigherStat(const Cosmology& C, const PowerSpectrum& P_l, real epsrel_)
: C(C), P_l(P_l),kintegral(*this)
{
MonteCarloIntegral::Range domain[5] = {
    { KMIN_e, KMAX_e },
    { KMIN_e, KMAX_e },
    { KMIN_e, KMAX_e },
    { -0.999999, 0.999999 },
    { -0.999999, 0.999999 },
};
kintegral.SetDomain(domain);
epsrel = epsrel_;
}
HigherStat::~HigherStat() {
}

//Filters
inline double w_filter(double k, int a){
  switch (a) {
  case 1:
    return 3./k*SphericalBesselJ1(k); //TOPHAT
    break;
  case 2:
	  return exp(-pow2(k)/2.); //GAUSSIAN
    break;
  case 3:
	  return 1.; // NO FILTER
    break;
    default:
     warning("Filter: invalid indices");
     return 0;
    }
}

/*Gamma 1 and Gamma 2 (logarithmic derivatives of variance) */

// Create splines functions of sigma^2
Spline sig2_spline;
void HigherStat::sigsqr_init(int a) const{
  int loop_N = 1000;
  double RMAX =250;
  double RMIN = 1.;
  double Rval, sigval, vars[4];
  vector<double> Rval_tab, sigval_tab;
for(int i = 0; i< loop_N; i++){
    Rval = RMIN + i*(RMAX-RMIN)/(loop_N-1);//RMIN * exp(i*log(RMAX/RMIN)/(loop_N-1));
    sigval = sig_sqr(Rval,a,1,vars);
    Rval_tab.push_back(Rval);
    sigval_tab.push_back(sigval);
      }
  sig2_spline = LinearSpline(Rval_tab,sigval_tab);
}

//function for derivative

static double mysigsqr(double R,void * params){
  (void)(params);
  return sig2_spline(R);
}

static double gam1(double R, void * params1){
  (void)(params1);
gsl_function F;
double result, abserr;
  F.params = 0;
  F.function = &mysigsqr;
  gsl_deriv_central(&F, R, 1e-3, &result, &abserr);
return R*result/sig2_spline(R);
}

static double gam2(double R){
gsl_function F;
double result, abserr;
  F.params = 0;
  F.function = &gam1;
  gsl_deriv_central(&F, R, 1e-4, &result, &abserr);
return R*result;
}

// b selects analytic (1) or numerical (2)
// a selects window function form

/* Variance */
static double var_integranda(const PowerSpectrum& P_l, double R, int a,double k){
	return pow2(k*w_filter(k*R,a)*D_spt/dnorm_spt)*P_l(k);
}

static double gam1_test(double R, const PowerSpectrum& P_l){
double step = 0.00001;
double top = Integrate(bind(var_integranda, cref(P_l), R + step, 1, _1), KMIN_e , KMAX_e, 1e-4)/(2.*pow2(M_PI));
double bot = Integrate(bind(var_integranda, cref(P_l), R - step, 1, _1), KMIN_e , KMAX_e, 1e-4)/(2.*pow2(M_PI));
double mid = Integrate(bind(var_integranda, cref(P_l), R, 1, _1), KMIN_e , KMAX_e, 1e-4)/(2.*pow2(M_PI));
return R*(top-bot)/(2.*step)/mid;
}

static double gam2_test(double R, double step, const PowerSpectrum& P_l){
double top = gam1_test(R+step, cref(P_l));
double bot = gam1_test(R-step, cref(P_l));
double mid = gam1_test(R, cref(P_l));
return R*(top-bot)/(2.*step)/mid;
}

static double var_integrandn(const PowerSpectrum& P_l, double R, int a, double vars[], double k){
  IOW iow;
  iow.initn_lin(0, vars[4], k, vars[0], vars[1], vars[2], vars[3]);
	return pow2(k*w_filter(k*R,a)*F1_nk/dnorm_spt)*P_l(k);
}

// vars is for numerical skewness only : vars[0]=omega0, vars[1-3] = MG params and vars[4]=scale factor
// a chooses filter, b chooses analytic or numerical (1 or 2 resp)
double HigherStat::sig_sqr(double R, int a, int b, double vars[]) const{
  switch (b) {
    case 1:
        return  Integrate(bind(var_integranda, cref(P_l), R, a, _1), KMIN_e , KMAX_e, epsrel)/(2.*pow2(M_PI));
    break;
    case 2:
         return  Integrate(bind(var_integrandn, cref(P_l), R, a, vars, _1), KMIN_e , KMAX_e, epsrel)/(2.*pow2(M_PI));
    break;
  }

}

/* Skewness */
// analytic (GR AND nDGP)
static double s3_num_integranda(const PowerSpectrum& P_l, double R, int a, double k1, double k2, double x){
   double k1s = pow2(k1);
   double k2s = pow2(k2);
	return k1s*k2s*w_filter(k1*R,a)*w_filter(k2*R,a)*w_filter(sqrt(k1s+k2s+2.*k1*k2*x)*R,a)*(pow2(D_spt)*F2eds(k1,k2,x) + F_spt*(1.-x*x))*pow2(D_spt)/pow4(dnorm_spt)*P_l(k1)*P_l(k2);
}
// numerical
static double s3_num_integrandn(const PowerSpectrum& P_l, double R, int a, double vars[], double k1, double k2, double x){
  double kargs[4],kv[3],xv[3], p22,p13,d;
  double k1s = pow2(k1);
  double k2s = pow2(k2);
  IOW iow;
        kv[0] = k1;
        kv[1] = k2;
        kv[2] = kv[1];
        xv[0] = -0.99999999;
        xv[1] = x;
        xv[2] = -x;
        kargs[0] = sqrt(kv[1]*kv[1]+kv[0]*kv[0]-2.*kv[1]*kv[0]*xv[1]);
        kargs[2] = sqrt(kv[1]*kv[1]+2.*kv[1]*kv[2]*xv[0]+kv[2]*kv[2]);
        kargs[1] = sqrt(kv[2]*kv[2]+2.*kv[2]*kv[0]*xv[2]+kv[0]*kv[0]);
        kargs[3] = sqrt(kv[0]*kv[0]+2.*kv[0]*kv[1]*xv[1]+kv[1]*kv[1]);
        iow.initn2(vars[4],kv,xv,kargs,vars[0],vars[1],vars[2],vars[3]);
	return k1s*k2s*w_filter(k1*R,a)*w_filter(k2*R,a)*w_filter(sqrt(k1s+k2s+2.*k1*k2*x)*R,a)*F2A_nk[0]*F1p_nk[0]*F1_nk/pow4(dnorm_spt)*P_l(k1)*P_l(k2);
}

// vars is for numerical skewness only : vars[0]=omega0, vars[1-3] = MG params and vars[4]=scale factor
// a chooses window functions (see w_filter)
// b chooses (1) analytic (2) numerical (3) gamma1 approx for top-hat filter
double HigherStat::skewness(double R, int a, int b, double vars[]) const{
  double c[3] = {KMIN_e,KMIN_e,-0.99999999};
  double d[3] = {KMAX_e,KMAX_e,0.99999999};
  void * params;

  switch (b) {
    case 1:
	     return  3./4./pow4(M_PI) * Integrate<3>(bind(s3_num_integranda, cref(P_l), R, a, _1, _2, _3), c,d, epsrel);
    break;
    case 2:
      return  3./4./pow4(M_PI) * Integrate<3>(bind(s3_num_integrandn, cref(P_l), R, a, vars, _1, _2, _3), c,d, epsrel);
    break;
    case 3:
      return (34./7. + gam1(R,params)) * pow2(sig2_spline(R));
    break;
  }
}

/* Analytic Kurtosis */

static double s4_num_integranda1(const PowerSpectrum& P_l, double R, int a, double args[], double k[], double x12, double x23, double x13){
   double k1 = k[0];
   double k2 = k[1];
   double k3 = k[2];
   double k1s = args[0];
   double k2s = args[1];
   double k3s = args[2];
   double filt1 = args[3];
   double filt2 = args[4];
   double filt3 = args[5];
   double k12 = sqrt(k1s+k2s+2.*k1*k2*x12);
   double k23 = sqrt(k2s+k3s-2.*k2*k3*x23);
   double k13 = sqrt(k1s+k3s+2.*k1*k3*x13);
   double filt12  = w_filter(k12*R,a);
   double filt23 = w_filter(k23*R,a);
   double term1, term2;

   term1 = F2eds(k1,k2,x12)*F2eds(k2,k3,-x23)*filt23*filt12*filt1*filt3;
   term2 = 0.;
   if (a>=2) {
     double k123 = sqrt(k1s + k2s + k3s + 2.*k1*k2*x12 + 2.*k2*k3*x23 + 2.*k1*k3*x13);
     double filt123 = w_filter(k123*R,a);
     term2 = 0.5*filt1*filt2*filt3*F3edsb(k1,k2,k3,k23,k12,k13,x23,x12,x13)*filt123;
   }
	return term1 + term2;
}

static double s4_num_integranda2(const PowerSpectrum& P_l, double R, int a, double k1, double k2, double k3){
   double c[3] = {-1.,-1.,-1.};
   double d[3] = {1.,1.,1.};
   double k[3];
   k[0] = k1;
   k[1] = k2;
   k[2] = k3;
   double args[6];
   args[0]= pow2(k1);
   args[1]= pow2(k2);
   args[2]= pow2(k3);
   args[3]= w_filter(k1*R,a);
   args[4]= w_filter(k2*R,a);
   args[5]= w_filter(k3*R,a);
	return args[0]*args[1]*args[2]*P_l(k1)*P_l(k2)*P_l(k3)*Integrate<3>(bind(s4_num_integranda1, cref(P_l), R, a,args,k, _1, _2, _3), c,d, 1e-4);
}

// vars is for numerical kurtosis only : vars[0]=omega0, vars[1-3] = MG params and vars[4]=scale factor
// a chooses window functions (see w_filter)
// b chooses (1) analytic (2) numerical (3) gamma1 + gamma2 approx for top-hat filter

HigherStat::kurtosis_integral::kurtosis_integral(const HigherStat& hs_)
    : MonteCarloIntegral(5), hs(hs_)
{
    maxeval = 10000000;
}

void HigherStat::kurtosis_integral::Integrand(const double x[], double* f, double* param) const {
  double R = *param;
  double k1 = x[0];
  double k2 = x[1];
  double k3 = x[2];
  double x12 = x[3];
  double x23 = x[4];
  double k1s= pow2(k1);
  double k2s = pow2(k2);
  double k3s =  pow2(k3);
  double filt1 = w_filter(k1*R,1);
  double filt3 = w_filter(k3*R,1);
  double k12 = sqrt(k1s+k2s+2.*k1*k2*x12);
  double k23 = sqrt(k2s+k3s+2.*k2*k3*x23);
  double filt12  = w_filter(k12*R,1);
  double filt23  = w_filter(k23*R,1);
  double term1 =  F2eds(k1,k2,x12)*F2eds(k2,k3,x23)*filt23*filt12*filt1;
  f[0] = 2.*k1s*k2s*k3s*hs.P_l(k1)*hs.P_l(k2)*hs.P_l(k3)*filt3*(term1);
}

/* kbar definition (EQ. A.25 OF 9312026)*/
static double kbar_integrand(const PowerSpectrum& P_l, double R, double k){
	return pow2(pow2(k)*w_filter(k*R,1)*D_spt/dnorm_spt)*P_l(k);
}

double HigherStat::kurtosis(double R, int a, int b, double vars[]) const{
  double c[3] = {KMIN_e,KMIN_e,KMIN_e};
  double d[3] = {KMAX_e,KMAX_e,KMAX_e};
  double term4 = 0.;
  double kbar;
  double sigsqr = sig_sqr(R, 1, 1, vars);
  if (b==1 && a ==1) {
    kbar = Integrate(bind(kbar_integrand, cref(P_l), R, _1), KMIN_e , KMAX_e, epsrel)/(2.*pow2(M_PI)*sigsqr);
    term4 =  2./189.*pow3(sigsqr)*(1364. + 9.*gam1_test(R,cref(P_l)) * (60. + 7. * gam1_test(R,cref(P_l))) - 126. * kbar);
  }
  switch (b) {
    case 1:
      return  term4;//0.75 * pow6(D_spt/(M_PI*dnorm_spt)) * Integrate<3>(bind(s4_num_integranda2, cref(P_l), R, a, _1, _2, _3), c,d, epsrel); //+ term4;
    case 2:
      return (60712./1323. + 62./3.*gam1_test(R,cref(P_l)) + 7./3.*pow2(gam1_test(R,cref(P_l))) + 2./3.*gam2_test(R,epsrel,cref(P_l))) * pow3(sigsqr);
    case 3:
    return 0.75 * pow6(D_spt/(M_PI*dnorm_spt)) * Integrate<3>(bind(s4_num_integranda2, cref(P_l), R, a, _1, _2, _3), c,d, epsrel);
  }
}