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


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "time_frequency_inverse_transform.hpp"

#include <libefl/vector_functions.hpp>

#include <libpml/time_frequency_parameter.hpp>
#include <libpml/time_frequency_parameter_config.hpp>

#include <librbbl/circular_buffer.hpp>
#include <librbbl/fft_wrapper_base.hpp>
#include <librbbl/fft_wrapper_factory.hpp>

#include <boost/math/constants/constants.hpp>

#include <algorithm>
#include <ciso646>
#include <cmath>

namespace visr
{
namespace rcl
{

TimeFrequencyInverseTransform::TimeFrequencyInverseTransform( SignalFlowContext const & context,
                                                              char const * name,
                                                              CompositeComponent * parent,
                                                              std::size_t numberOfChannels,
                                                              std::size_t dftLength,
                                                              std::size_t hopSize,
                                                              char const * fftImplementation /*= "default"*/ )
 : AtomicComponent( context, name, parent )
 , mAlignment( cVectorAlignmentSamples )
 , mNumberOfChannels( numberOfChannels )
 , mDftLength( dftLength )
 , mDftSamplesPerPeriod( period() / hopSize )
 , mHopSize( hopSize )
 , mAccumulationBuffer( mNumberOfChannels, mDftLength - mHopSize, mAlignment )
 , mFftWrapper( rbbl::FftWrapperFactory<SampleType>::create( fftImplementation, dftLength, mAlignment ) )
 , mCalcBuffer(mDftLength, mAlignment )
 , mInput( "in", *this, pml::TimeFrequencyParameterConfig( dftLength, hopSize, numberOfChannels, mDftSamplesPerPeriod ) )
 , mOutput( "out", *this, numberOfChannels )
{
  if( period() % hopSize != 0 )
  {
    throw std::invalid_argument( "TimeFrequencyInverseTransform: Invalid hop size (no integer number of hops per audio processing period)." );
  }
}

TimeFrequencyInverseTransform::~TimeFrequencyInverseTransform() = default;

void TimeFrequencyInverseTransform::process()
{
  pml::TimeFrequencyParameter<SampleType> const & inMtx = mInput.data();
  const std::size_t accuElementsToCopy = mDftLength - mHopSize;
  // operating channel by channel might save copying to and fro the accumulation buffer in case of multiple hops per period.
  for( std::size_t channelIndex( 0 ); channelIndex < mNumberOfChannels; ++channelIndex )
  {
    efl::ErrorCode res;
    for( std::size_t hopIndex( 0 ); hopIndex < mDftSamplesPerPeriod; ++hopIndex )
    {
      std::complex<SampleType> const * dftPtr = inMtx.dftSlice( channelIndex, hopIndex );
      res = mFftWrapper->inverseTransform( dftPtr, mCalcBuffer.data() );
      if( res != efl::noError )
      {
        throw std::runtime_error( "TimeFrequencyInverseTransform: Error during FFT operation." );
      }
      res = efl::vectorAddInplace( mAccumulationBuffer.row(channelIndex), mCalcBuffer.data(), accuElementsToCopy );
      if( res != efl::noError )
      {
        throw std::runtime_error( "TimeFrequencyInverseTransform: Updating of output accumulator failed." );
      }
      // Copy the output buffer back into storage (skip first block to implement the shift)
      if( (res = efl::vectorCopy( mCalcBuffer.data() + mHopSize, mAccumulationBuffer.row( channelIndex ), accuElementsToCopy )) != efl::noError )
      {
        throw std::runtime_error( "TimeFrequencyInverseTransform: Storing partial results failed." );
      }
    }
    // Copy first portion of accumulated result to output port.
    if( (res = efl::vectorCopy( mCalcBuffer.data(), mOutput[channelIndex], period(), mAlignment )) != efl::noError )
    {
      throw std::runtime_error( "TimeFrequencyInverseTransform: Error while copying output data." );
    }
  }
}

} // namespace rcl
} // namespace visr


//Link to Boost
#define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
//#include <boost/test/included/unit_test.hpp>
#include <boost/test/unit_test.hpp>

#include "test.h"
#include "../HomotopyContinuation.h"
#include "../BetheHomotopy.h"
#include "../BethePolynomialHomotopy.h"

// test suite
BOOST_FIXTURE_TEST_SUITE(Integrated_suite, SimpleTestFixture, * utf::label("IntegratedTests"))

BOOST_AUTO_TEST_CASE(SimpleHomotopy_test1)
{
    UnityEquation start(1, 2);
    std::vector<var_t> coefs({-20.0,1.0,1.0});
    PolynomialFunction target(coefs);
    SimpleHomotopy sh(&start, &target);
    sh.setSteps(100);
    SimpleHomotopyContinuation hc;

    // one root is 5 and the other one is -4
    bool found1 = false;
    bool found2 = false;

    for (int i = 0; i < start.numberOfRoots(); i++) {
        Vector st = start.getRoot(i);
        Solution sol(st);
        hc.solve(sh, sol);
        BOOST_TEST(sol.root().size() == 1);
        if (std::abs(sol.get(0) - (elem_t)4.0)) {
            found1 = true;
        }
        if (std::abs(sol.get(0) + (elem_t)5.0)) {
            found2 = true;
        }
    }
    BOOST_TEST(found1);
    BOOST_TEST(found2);
}

BOOST_AUTO_TEST_CASE(SimpleHomotopy_test2)
{
    UnityEquation start(1, 4);
    std::vector<var_t> coefs({-20.0,0,1.0,0,1.0});
    PolynomialFunction target(coefs);
    SimpleHomotopy sh(&start, &target);
    sh.setSteps(100);
    SimpleHomotopyContinuation hc;

    // there are found roots.
    bool found1 = false;
    bool found2 = false;
    bool found3 = false;
    bool found4 = false;

    for (int i = 0; i < start.numberOfRoots(); i++) {
        Vector startRoot = start.getRoot(i);
        Solution sol(startRoot);
        hc.solve(sh, sol);
        if (std::abs(sol.get(0) - (elem_t)2.0) < EPS) {
            found1 = true;
        }
        if (std::abs(sol.get(0) + (elem_t)2.0) < EPS) {
            found2 = true;
        }
        if (std::abs(sol.get(0) - var_t((elem_t).0, (elem_t)sqrt(5.0))) < 1e-6) {
            found3 = true;
        }
        if (std::abs(sol.get(0) + var_t((elem_t).0, (elem_t)sqrt(5.0))) < 1e-6) {
            found4 = true;
        }
    }
    BOOST_TEST(found1);
    BOOST_TEST(found2);
    BOOST_TEST(found3);
    BOOST_TEST(found4);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L6M2)
{
    std::cout << "Running integrated test for L=6, M=2 cases" << std::endl;
    int L = 6;
    int M = 2;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
        hc.setTraceFile(file);

        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        std::cout <<"i=" << i << ", errors=" << hc.errors() << ", gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L6M3)
{
    std::cout << "Running integrated test for L=6, M=2 cases" << std::endl;
    int L = 6;
    int M = 3;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        std::cout <<"i=" << i << ", errors=" << hc.errors() << ", gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L7M2)
{
    int L = 7;
    int M = 2;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        try {
            hc.solve(hom, sol);
        } catch (HomotopyContinuationException &ex) {
            std::cout << ex.what() << std::endl;
//            throw ex;
        }
        std::cout <<"i=" << i << ", errors=" << hc.errors() << ", gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L7M3)
{
    int L = 7;
    int M = 3;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);

        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        if (sol.indexOfSingularRoot1() >= 0) {
            std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        }
        try {
            hc.solve(hom, sol);
        } catch (HomotopyContinuationException &ex) {
            std::cout << ex.what() << std::endl;
            throw ex;
        }

        std::cout <<"i=" << i << std::endl << "errors=" << hc.errors() << std::endl << "gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L8M2)
{
    int L = 8;
    int M = 2;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        std::cout <<"i=" << i << ", errors=" << hc.errors() << ", gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L8M3)
{
    int L = 8;
    int M = 3;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        std::cout <<"i=" << i << ", errors=" << hc.errors() << ", gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L8M4)
{
    int L = 8;
    int M = 4;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = 2 * Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    hc.setMaxNumberOfNewtonRaphonEachStep(50);
    hc.setMaxDepth(14);

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);

        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        std::cout <<"i=" << i << ", errors=" << hc.errors() << std::endl << "gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);

        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L10M4)
{
    int L = 10;
    int M = 4;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    hc.setMaxNumberOfNewtonRaphonEachStep(50);

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        if (sol.indexOfSingularRoot1() >= 0) {
            std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        }

        hc.solve(hom, sol);
        std::cout <<"i=" << i << std::endl << "errors=" << hc.errors() << std::endl << "gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi << ",normalRoot=" << sol.root());
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(BethePolynomialHomotopy_L10M5)
{
    int L = 10;
    int M = 5;
    std::cout << "Running integrated test for L=" << L << ", M=" << M << " cases" << std::endl;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    BethePolynomialHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    hc.setMaxNumberOfNewtonRaphonEachStep(50);

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
//        if (i == 15) continue;
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);
        Vector startRoot = cache.getRoot(i);
        InvertableSolution sol(startRoot, L);
        if (sol.indexOfSingularRoot1() >= 0) {
            std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        }

        hc.solve(hom, sol);
        std::cout <<"i=" << i << std::endl << "errors=" << hc.errors() << std::endl << "gaps=" << hc.gaps() << std::endl;
        BOOST_TEST_INFO("i=" << ", errors=" << hc.errors() << ", gaps=" << hc.gaps());
        BOOST_TEST(hc.gaps().max() < .5L);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi << ",normalRoot=" << sol.root());
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(PoleFreeBetheHomotopy_L6M2)
{
    std::cout << "Running integrated test for L=6, M=2 cases" << std::endl;
    int L = 6;
    int M = 2;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    PoleFreeBetheHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;

    SET_LOG_LEVEL(Error);
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        LOG("processing root #" << i << ":\t" << cache.getRoot(i), Warning);
        Vector startRoot = cache.getRoot(i);
        BetheSolution sol(startRoot, L, 0.0, totalPhase / hom.getSteps());
        std::cout << "eps=" << sol.epsilon() << ", c=" << sol.c() << std::endl;
        hc.solve(hom, sol);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
    SET_LOG_LEVEL(Error);
}

BOOST_AUTO_TEST_CASE(PoleFreeBetheHomotopy_L6M3)
{
    std::cout << "Running integrated test for L=6, M=3 cases" << std::endl;

    int L = 6;
    int M = 3;
    elem_t totalPhase = Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    PoleFreeBetheHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    SET_LOG_LEVEL(Error);

    for (int i = 0; i < cache.numberOfRoots(); i++) {
        Vector startRoot = cache.getRoot(i);
        BetheSolution sol(startRoot, L, 0.0, totalPhase / hom.getSteps());
        hc.solve(hom, sol);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
}

BOOST_AUTO_TEST_CASE(PoleFreeBetheHomotopy_L7M2)
{
    std::cout << "Running integrated test for L=7, M=2 cases" << std::endl;
    int L = 7;
    int M = 2;
    elem_t totalPhase = random(1.0, 2.0) * Pi;
//    elem_t totalPhase = 1.27095 * Pi;
//    elem_t totalPhase = 1.81386 * Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    PoleFreeBetheHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    SET_LOG_LEVEL(Error);

    // i = 1: at two identical infinity roots
    // i = 3 and 8: two conjugate finite roots.
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        Vector startRoot = cache.getRoot(i);
        BetheSolution sol(startRoot, L, 0.0, totalPhase / hom.getSteps());
        hc.solve(hom, sol);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("totalPhass/Pi=" << totalPhase/Pi << ", i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
}

BOOST_AUTO_TEST_CASE(PoleFreeBetheHomotopy_L7M3)
{
    std::cout << "Running integrated test for L=7, M=3 cases" << std::endl;
    int L = 7;
    int M = 3;
    elem_t totalPhase = 1.3453 * Pi;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    PoleFreeBetheHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    SET_LOG_LEVEL(Error);

    // i = 1: at two identical infinity roots
    // i = 3 and 8: two conjugate finite roots.
    for (int i = 0; i < cache.numberOfRoots(); i++) {
        Vector startRoot = cache.getRoot(i);
        BetheSolution sol(startRoot, L, 0.0, totalPhase / hom.getSteps());
        hc.solve(hom, sol);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
}

BOOST_AUTO_TEST_CASE(PoleFreeBetheHomotopy_L8M3)
{
    std::cout << "Running integrated test for L=8, M=3 cases" << std::endl;
    int L = 8;
    int M = 3;
//    elem_t totalPhase = random(1.0, 2.0) * Pi;
    elem_t totalPhase = 3.66417L;
    elem_t expectedMomentumDiff = floatMod(2 * M * totalPhase, 2 * Pi);
    PoleFreeBetheHomotopy hom(L, M, totalPhase);
    BetheRootCache cache(L, M);
    BetheHomotopyContinuation hc;
    SET_LOG_LEVEL(Error);

    // i = 1: at two identical infinity roots
    // i = 3 and 8: two conjugate finite roots.
    for (int i = 0; i < cache.numberOfRoots(); i++) {
//        std::string file = "BS_L" + ToString(L)+ "M" + ToString(M)+ "r" + ToString(i) + ".txt";
//        hc.setTraceFile(file);
        std::cout << "i=" << i << std::endl;

        Vector startRoot = cache.getRoot(i);
        BetheSolution sol(startRoot, L, 0.0, totalPhase / hom.getSteps());
        hc.solve(hom, sol);
        elem_t m1 = momentum(startRoot);
        elem_t m2 = momentum(sol.normalRoot());
        BOOST_TEST_INFO("i=" << i << ", m1/Pi=" << m1/(elem_t)Pi << ", m2/Pi=" << m2/(elem_t)Pi << ", expectedDiff/Pi=" << expectedMomentumDiff/(elem_t)Pi);
        BOOST_TEST(std::abs(floatMod(m2 - m1 - expectedMomentumDiff, 2.0 * Pi)) < 1e-4);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#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


// 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.
//
// Algorithm inspired by Nick Vannieuwenhoven, written by Cornelius Steinhardt

#  define MTL_VPT_LEVEL 5

#include <iostream>
#include <limits>

#include <boost/timer.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/numeric/mtl/io/read_el_matrix.hpp>
#include <boost/numeric/itl/pc/matrix_algorithms.hpp>
#include <boost/numeric/itl/pc/imf_preconditioner.hpp>
#include <boost/numeric/itl/pc/imf_algorithms.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#ifdef MTL_HAS_VPT
  #include <vt_user.h>
#endif

template< class ElementStructure >
void setup(ElementStructure& A, int lofi)
{
    typedef double value_type;

    int size( A.get_total_vars() );

    mtl::dense_vector<value_type>              x(size, 1), b(size), ident(size);
     iota(ident);

     std::cout<< "read ok, start with precond\n";
    boost::timer factorization;
    itl::pc::imf_preconditioner<value_type> precond(A, lofi);
    double ftime= factorization.elapsed();
    std::cout<< "imf ready\n";

//     for (int i= 0; i < size; i++)
//       x[i]= std::numeric_limits<value_type>::quiet_NaN();

    std::cout<< "size(rhs2)=" << num_rows(b) << "\n";
    mtl::compressed2D<double> B;
    assemble_compressed(A, B, ident);
    std::cout << "NNZ == " << B.nnz() << "\n";

    b= B * x;

#if 0
	mtl::io::tout << "------------------------------- STATISTICS -------------------------------" << std::endl;
	int rows = num_rows(*master_mat);
	int cols = num_cols(*master_mat);
 	int nnz = (*master_mat).nnz();
	mtl::io::tout << "Dimensions: " << rows << " x " << cols << std::endl;
	mtl::io::tout << "Non-zeros: " << nnz << std::endl;
	mtl::io::tout << "Sparsity (%): " << ((double(nnz) / rows) / cols) << std::endl;
	mtl::io::tout << "Avg nnz/row: " << double(nnz) / rows << std::endl;
	mtl::io::tout << std::endl;
	mtl::io::tout << "Elements: " << es.get_total_elements() << std::endl;
	mtl::io::tout << "Variables: " << es.get_total_vars() << std::endl;
	mtl::io::tout << "--------------------------------------------------------------------------" << std::endl;
// calculate eigenvalues
	mtl::dense2D<value_type> E(size,size),A(*master_mat);
	for(int i=0; i<size;i++){
	  mtl::dense_vector<value_type> tmp(A[mtl::irange(0, mtl::imax)][i]);
	  E[mtl::irange(0, mtl::imax)][i] = precond.solve(tmp);
	}
	mtl::io::tout<< "E=\n"<<E <<"\n";
#endif

	std::cout<< "start solver\n";
    itl::cyclic_iteration<value_type>          iter(b, size, 1.e-8, 0.0, 5);
    x= 0;
    boost::timer solver;
     bicgstab(B, x, b, precond, iter);
     std::cout << "Factorization took " << ftime << "s, solution took " << solver.elapsed() << "s\n";
}

int main(int, char** argv)
{
    mtl::vampir_trace<9999> tracer;
    typedef double value_type;

    std::string program_dir= mtl::io::directory_name(argv[0]),
  	        matrix_file= mtl::io::join(program_dir, "../../mtl/test/matrix_market/square3.mtx");
//  	        matrix_file= mtl::io::join(program_dir, "../../../../../data/sysMat_elem.mtx");
// 		matrix_file= mtl::io::join(program_dir, "../../mtl/test/matrix_market/obstacle_small.mtx");
// 	        matrix_file= mtl::io::join(program_dir, "../../../../../data/matrix_market/obstacle_q1q1_e64/obstacle_q1q1_e64_r00800.mtx");

    mtl::mat::element_structure<value_type> A;
    read_el_matrix(matrix_file, A);

    setup(A, 5);
    return 0;
}


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

}


#include <scitbx/math/tetrahedron.h>

#include <boost/python/class.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_value_policy.hpp>

namespace scitbx { namespace math {

namespace {

template<typename T>
struct tetrahedron_wrapper
{
  typedef tetrahedron<T> wt;

  static void wrap() {
    using namespace boost::python;
    return_value_policy<copy_const_reference> ccr;
    class_<wt>("tetrahedron", no_init)
      .def(init<typename wt::vertices_t const&>((arg("vertices"))))
      .add_property("vertices", make_function(&wt::vertices, ccr))
      .def("volume", &wt::volume)
      .def("gradients", &wt::gradients)
      ;
  }
};

}

namespace boost_python {
  void wrap_tetrahedron() {
    tetrahedron_wrapper<double>::wrap();
  }
}

}}


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

}


// Filename: ranged_for_iteration.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace mtl;

template <typename Matrix>
void f(Matrix& A)
{
    A= 7.0;  // Set values in diagonal

#if defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
    // Define the property maps
    auto row=   row_map(A);
    auto col=   col_map(A);
    auto value= const_value_map(A);

    // Now iterate over the matrix
    for (auto c : major_of(A))      // rows or columns
	for (auto i : nz_of(c))     // non-zeros within
	    std::cout << "A[" << row(i) << ", " << col(i) << "] = " << value(i) << '\n';
#endif
}

int main(int, char**)
{
    // Define a row-major sparse and a column-major dense matrix
    compressed2D<double>                             A(3, 3);
    dense2D<double, mat::parameters<col_major> >  B(3, 3);

    f(A);
    f(B);

    return 0;
}


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


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_MNORM_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_MNORM_HPP_INCLUDED
#include <nt2/include/functor.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/mpl/int.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/tags.hpp>

namespace nt2 { namespace tag
  {
    /*!
     @brief globalnorm generic tag

      Represents the mnorm function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct mnorm_ :  ext::abstract_<mnorm_>
    {
      /// INTERNAL ONLY
      typedef ext::abstract_<mnorm_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_mnorm_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::mnorm_, Site> dispatching_mnorm_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::mnorm_, Site>();
    }
    template<class... Args>
    struct impl_mnorm_;
  }

  /*!
    @brief Matricial norm

    Computes the matricial norms of a matrix expression with static or dynamic choice from
    by the optional second parameter
    that can be 1, nt2::tag::one_  2, nt2::two_, , nt2::inf_ or , nt2::fro_
    or the template parameter
    that can be 1, nt2::tag::one_  2, nt2::tag::two_, , nt2::tag::inf_ or , nt2::tag::fro_

    Call protocols to mnorm are summarized in the following table. We advise to use static
    calls whenever possible as it prevents cascaded run-time if clauses and goes directly
    to the right call at execution.

    @code
    |-------------------|-------------------|------------------------------|-------------------|
    | mnorm(a0, p)                                                                             |
    |-------------------|-------------------|------------------------------|-------------------|
    |    static p       |  dynamic p        |     formula  (pseudo-code)   |  equivalent to    |
    |-------------------|-------------------|------------------------------|-------------------|
    | nt2::one_         | 1                 |       max(sum(abs(x)))       | mnorm1(x)         |
    | nt2::two_         | 2                 |         max(svd(x))          | mnorm2(x)         |
    | nt2::inf_         | nt2::Inf<T>()     |   max(sum(abs(ctrans(x))))   | mnorminf(x)       |
    | nt2::fro_         | -1                | sqrt(sum(diag(ctrans(x)*x))) | mnormfro(x)       |
    |-------------------|-------------------|------------------------------|-------------------|
    | mnorm<p>(a0)                                                                             |
    |-------------------|-------------------|------------------------------|-------------------|
    |    static p       |                   |     matrix                   |                   |
    |-------------------|-------------------|------------------------------|-------------------|
    | nt2::tag::one_    |        -          |       max(sum(abs(x)))       |  mnorm1(x)        |
    | nt2::tag::two_    |        -          |         max(svd(x))          |  mnorm2(x)        |
    | nt2::tag::inf_    |        -          |   max(sum(abs(ctrans(x))))   |  mnorminf(x)      |
    | nt2::tag::fro_    |        -          | sqrt(sum(diag(ctrans(x)*x))) |  mnormfro(x)      |
    |-------------------|-------------------|------------------------------|-------------------|
    @endcode

    @par Semantic:
    1, 2 and inf can be given dynamically or statically as template parameter ie:

    For any expression @c a0 of type @c A0, the following call:

    @code
    as_real<A0::value_type>::type x = mnorm(a0);
    @endcode

    is equivalent to:

    @code
    as_real<A0::value_type>::type x = svd(a0)(1);
    @endcode

    For any expression @c a0 of type @c A0 and any value x in {one_, two_, inf_, fro_}
    following call:

    @code
    as_real<A0::value_type>::type r = mnorm(a0,nt2::x);
    @endcode

    or

    @code
    as_real<A0::value_type>::type r = mnorm<nt2::tag::x>(a0);
    @endcode
    is equivalent to:

    @code
    as_real<A0::value_type>::type r = mnormx(a0);
    @endcode

    @param a0 Expression to compute the norm of
    @param a1 Type of norm to compute
  **/

  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnorm_, mnorm, 1)

  /// @overload
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnorm_, mnorm, 2)

  /// @overload
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::mnorm_, mnorm2, 1)

  /// @overload
  template < class T, class A>
  BOOST_FORCEINLINE
  typename meta::as_real<typename A::value_type>::type
  mnorm(const A& a)
  {
    return mnorm(a, nt2::meta::as_<T>());
  }
  /// @overload
  template < int Value, typename A>
  BOOST_FORCEINLINE
  typename meta::as_real<typename A::value_type>::type
  mnorm(const A& a)
  {
    return mnorm(a, boost::mpl::int_<Value>());
  }
}

#endif


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/integral_constant.hpp>
using namespace boost::hana;

int main() {

{

//! [minus]
BOOST_HANA_CONSTANT_CHECK(minus(int_<3>, int_<5>) == int_<-2>);
BOOST_HANA_CONSTEXPR_CHECK(minus(1, 2) == -1);
//! [minus]

}{

//! [negate]
BOOST_HANA_CONSTANT_CHECK(negate(int_<3>) == int_<-3>);
BOOST_HANA_CONSTEXPR_CHECK(negate(2) == -2);
//! [negate]

}

}


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

#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/def.hpp>

#include <scitbx/math/distributions.h>

#if defined(__GNUC__) && __GNUC__ == 3 && __GNUC_MINOR__ == 2
# define SCITBX_MATH_STUDENTS_T_DISABLED // to avoid compilation errors
#endif

namespace scitbx { namespace math {

/*! Wrappers for boost::math statistical distributions.
    See also:
    http://www.boost.org/libs/math/doc/sf_and_dist/html/math_toolkit/dist.html
 */
namespace {

  template <typename FloatType>
  struct normal_distribution_wrappers
  {
    typedef boost::math::normal_distribution<FloatType> wt;

    static void
    wrap()
    {
      using namespace boost::python;

      class_<wt>("normal_distribution", no_init)
        .def(init<FloatType, FloatType>((arg("mean")=0, arg("sd")=1)))
      ;
    }
  };

#ifndef SCITBX_MATH_STUDENTS_T_DISABLED
  template <typename FloatType>
  struct students_t_distribution_wrappers
  {
    typedef FloatType ft;
    typedef boost::math::students_t_distribution<FloatType> wt;

    // workaround for Intel C++ 12.0.3
    static FloatType
    find_degrees_of_freedom_wrapper(
      ft difference_from_mean, ft alpha, ft beta, ft sd, ft hint)
    {
      return wt::find_degrees_of_freedom(
        difference_from_mean, alpha, beta, sd, hint);
    }

    static void
    wrap()
    {
      using namespace boost::python;

      class_<wt>("students_t_distribution", no_init)
        .def(init<FloatType>(arg("v")))
        .def("degrees_of_freedom", &wt::degrees_of_freedom)
        .def("find_degrees_of_freedom", find_degrees_of_freedom_wrapper, (
          arg("difference_from_mean"),
          arg("alpha"),
          arg("beta"),
          arg("sd"),
          arg("hint")=100))
        .staticmethod("find_degrees_of_freedom")
      ;
    }
  };
#endif

  template <typename FloatType, class Distribution>
  struct non_member_function_wrappers
  {
    typedef Distribution wt;

    #define NEW_MEMBER(name)                \
    static FloatType name(wt const &self) { \
      return boost::math::name(self);                    \
    }

    NEW_MEMBER(mean);
    NEW_MEMBER(median);
    NEW_MEMBER(mode);
    NEW_MEMBER(variance);
    NEW_MEMBER(standard_deviation);
    NEW_MEMBER(skewness);
    NEW_MEMBER(kurtosis);

    #undef NEW_MEMBER

    #define NEW_MEMBER(name)                               \
    static FloatType name(wt const &self, FloatType arg) { \
      return boost::math::name(self, arg);                              \
    }

    NEW_MEMBER(pdf);
    NEW_MEMBER(cdf);
    NEW_MEMBER(quantile);

    #undef NEW_MEMBER

    static scitbx::af::shared<FloatType> quantiles(wt const &self, std::size_t n) {
      return scitbx::math::quantiles<FloatType>(self, n);
    }

    static void
    wrap()
    {
      using namespace boost::python;
      def("mean"              , mean);
      def("median"            , median);
      def("mode"              , mode);
      def("variance"          , variance);
      def("standard_deviation", standard_deviation);
      def("skewness"          , skewness);
      def("kurtosis"          , kurtosis);
      def("pdf"               , pdf);
      def("cdf"               , cdf);
      def("quantile"          , quantile);
      def("quantiles"         , quantiles);
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_distributions()
  {
    normal_distribution_wrappers<double>::wrap();
    non_member_function_wrappers<
      double, boost::math::normal_distribution<double> >::wrap();
#ifndef SCITBX_MATH_STUDENTS_T_DISABLED
    students_t_distribution_wrappers<double>::wrap();
    non_member_function_wrappers<
      double, boost::math::students_t_distribution<double> >::wrap();
#endif
  }

}}} // namespace scitbx::math::boost_python


#include <tdp/testing/testing.h>
#include <iostream>
#include <Eigen/Dense>
#include <tdp/manifold/SO3.h>
#include <tdp/manifold/rotation.h>
#include <tdp/manifold/SE3.h>
#include <tdp/eigen/dense.h>

using namespace tdp;

TEST(SE3, deriv) {
  float eps = 1e-3;

  for (size_t i=0; i<100; ++i) {
    Eigen::Vector3f p_c = Eigen::Vector3f::Random();
    tdp::SE3f T_wc = tdp::SE3f::Random();
    for (size_t j=0; j<6; ++j) {
      Eigen::Matrix<float,6,1> delta = Eigen::Matrix<float,6,1>::Zero();
      delta(j) = eps;
      tdp::SE3f T_wcDelta = T_wc.Exp(delta);
      Eigen::Vector3f diffGt = T_wcDelta*p_c - T_wc*p_c ;
      Eigen::Matrix<float,3,6> J;
      J << -T_wc.rotation().matrix() * tdp::SO3f::invVee(p_c), T_wc.rotation().matrix();
      Eigen::Vector3f diffJ = J*delta;
      std::cout << j << ": " << (diffGt-diffJ).norm() << std::endl;
//        << ";\t" << diffGt.transpose() << " " << diffJ.transpose() << std::endl;
    }
  }
}

TEST(SE3, derivofInverse) {
  float eps = 1e-3;

  for (size_t i=0; i<100; ++i) {
    Eigen::Vector3f p_w = Eigen::Vector3f::Random();
    tdp::SE3f T_wc = tdp::SE3f::Random();
    for (size_t j=0; j<6; ++j) {
      Eigen::Matrix<float,6,1> delta = Eigen::Matrix<float,6,1>::Zero();
      delta(j) = eps;
      tdp::SE3f T_wcDelta = T_wc.Exp(delta);
      Eigen::Vector3f diffGt = T_wcDelta.Inverse()*p_w- T_wc.Inverse()*p_w;
      Eigen::Matrix<float,3,6> J;
      J << tdp::SO3f::invVee(T_wc.rotation().Inverse()*(p_w-T_wc.translation())), - Eigen::Matrix3f::Identity();
      Eigen::Vector3f diffJ = J*delta;

      std::cout << j << ": " << (diffGt-diffJ).norm()
        << ";\t" << diffGt.transpose() << " " << diffJ.transpose() << std::endl;
    }
  }
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}