///////////////////////////////////////////////////////////////////////////////
//
// ParVoro++: A scalable parallel algorithm for constructing 3D Voronoi tessellations based on KD-tree decomposition
//
// Guoqing Wu
// High Performance Computing Center, Institute of Applied Physics and Computational Mathematics, Beijing
// wu_guoqing@iapcm.ac.cn
//
///////////////////////////////////////////////////////////////////////////////
#ifndef __VORONOI_KD_H
#define __VORONOI_KD_H

#include "StandardIncludes.h"
#include "data/particle.hpp"
#include "data/common.hpp"
#include <voro++/voro++.hh>
#include <itree/avtIntervalTree.h>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Extreme_points_traits_adapter_3.h>

#include <boost/iterator/counting_iterator.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel       K;

template<typename RealType, unsigned NDIM>
void
convex_hull(ParticleBlock<RealType, NDIM>* b_,
            const diy::Master::ProxyWithLink& cp,
	    std::vector<std::set<size_t> >& ch_indices)
{
    typedef  diy::ContinuousBounds            Bounds;
    typedef  ParticleBlock<RealType, NDIM>    Block;
    typedef  diy::RegularContinuousLink       RCLink;

    Block*  b  = static_cast<Block*>(b_);
    RCLink* l  = dynamic_cast<RCLink*>(cp.link());
    int index = b->lid;

    std::vector<K::Point_3> points;
    for(size_t i = 0; i <  b->num_orig_particles; i++)
    {
        K::Point_3 p( b->particles[i][0],  b->particles[i][1],  b->particles[i][2]);
        points.push_back(p);
    }

    //This will contain the extreme vertices
    std::vector<size_t> extreme_point_indices;
    //call the function with the traits adapter for vertices
    CGAL::extreme_points_3(CGAL::make_range(boost::counting_iterator<size_t>(0),
                                          boost::counting_iterator<size_t>(points.size())),
                           std::back_inserter(extreme_point_indices),
                           CGAL::make_extreme_points_traits_adapter(CGAL::make_property_map(points)));

    std::copy(extreme_point_indices.begin(), extreme_point_indices.end(), std::inserter(ch_indices[index], ch_indices[index].begin()));
    for(int i=0; i<l->size(); i++)
    {
        for(std::set<size_t>::iterator it=ch_indices[index].begin(); it!=ch_indices[index].end(); ++it)
        {
            // send convex hull vertex to nearest block
            RemoteParticle<RealType, NDIM> rp;
            rp.particle[0] = (RealType)(b->particles[*it][0]);
            rp.particle[1] = (RealType)(b->particles[*it][1]);
            rp.particle[2] = (RealType)(b->particles[*it][2]);
            rp.info.bid = b->bid;
            rp.info.index = *it;
            cp.enqueue(l->target(i), rp);
        }
    }

}

template<typename RealType, unsigned NDIM>
int
incomplete_cells(ParticleBlock<RealType, NDIM>* b,
                 const diy::Master::ProxyWithLink& cp,
	         bool wrap,
                 voro::container* con[],
                 voro::particle_order* po[],
	         avtIntervalTree* itree_spatial,
	         diy::StaticAssigner& assigner,
                 std::vector<std::set<size_t> >& ch_indices)
{
    typedef diy::RegularContinuousLink  RCLink;
    RCLink* l = dynamic_cast<RCLink*>(cp.link());

    std::vector<std::set<int> > to_send( b->num_orig_particles);

    std::set<int> neighbors;
    for(int i = 0; i < l->size(); i++)
        neighbors.insert(l->target(i).gid);

    int index = b->lid;
    int pid;
    voro::voronoicell c;
    double x, y, z, r;
    double center[3];
    RemoteParticle<float, NDIM> rp;
    voro::c_loop_order clo(*con[index],*po[index]);
    if(clo.start()) do if((con[index]->compute_cell(c, clo)))
    {
        clo.pos(pid, x, y, z, r);

        center[0] = x;
        center[1] = y;
        center[2] = z;

        std::vector<double> v;
        c.vertices(x,y,z,v);
        for(int vi = 0; vi < c.p; vi++)
        {
            double verts[3];
            verts[0] = v[3*vi];
            verts[1] = v[3*vi+1];
            verts[2] = v[3*vi+2];
            double rad = distance(center, verts) + EPSILON;

            // check if the circumsphere is too deep inside the block to be able to stick out
	    int j;
            for(j = 0; j < 3; ++j)
            {
                if(verts[j] - b->bounds.min[j] <= rad) break;
                if(b->bounds.max[j] - verts[j] <= rad) break;
            }
            if(j == 3)	// the circumsphere is too deep inside the block
                continue;

	    // computing bounding box of the circumsphere
    	    double min_vec[3], max_vec[3];
            for(int i = 0; i < 3; i++)
            {
	        min_vec[i] = verts[i] - rad;
	        max_vec[i] = verts[i] + rad;
            }

	    // no wrap, NB: wrap case includes no wrap
	    {
                std::vector<int> list;
                itree_spatial->GetElementsListFromRange(min_vec, max_vec, list); //基于区间树查找相交box
                for(int i = 0; i < list.size(); ++i)
                {
		    if(list[i] != b->bid.gid) // do not send to itself
		        to_send[pid].insert(list[i]);
	        }
	    }

    	    if(wrap)
	    {
                std::vector<int> wrap_dir(3,0);
		int flag[3] = {0, 0, 0};
	  	if(min_vec[0] <= b->global_bounds.min[0]) // -X
		{
		    wrap_dir[0] = 1;
		    flag[0]++;
		}
	  	if(min_vec[1] <= b->global_bounds.min[1]) // -Y
		{
		    wrap_dir[1] = 1;
		    flag[1]++;
		}
	  	if(min_vec[2] <= b->global_bounds.min[2]) // -Z
		{
		    wrap_dir[2] = 1;
		    flag[2]++;
		}
	  	if(max_vec[0] >= b->global_bounds.max[0]) // +X
		{
		    wrap_dir[0] = -1;
		    flag[0]++;
		}
	  	if(max_vec[1] >= b->global_bounds.max[1]) // +Y
		{
		    wrap_dir[1] = -1;
		    flag[1]++;
		}
	  	if(max_vec[2] >= b->global_bounds.max[2]) // +Z
		{
		    wrap_dir[2] = -1;
		    flag[2]++;
		}

                // handle the weird case, where the box intersects both sides
	        wrap_dir[0] = (flag[0]>1) ? 0 : wrap_dir[0];
		wrap_dir[1] = (flag[1]>1) ? 0 : wrap_dir[1];
		wrap_dir[2] = (flag[2]>1) ? 0 : wrap_dir[2];

		for(int x = 0; x < 2; x++)
		    for(int y = 0; y < 2; y++)
		        for(int z = 0; z < 2; z++)
			{
			    if(x==0 && y==0 && z==0) // no wrap case
				continue;

			    std::vector<int> dir(3,0);
			    dir[0] = x * wrap_dir[0];
			    dir[1] = y * wrap_dir[1];
			    dir[2] = z * wrap_dir[2];

    	    		    double min_[3], max_[3];
            		    for(int i = 0; i < 3; i++)
            		    {
	                        min_[i] = min_vec[i];
	                        max_[i] = max_vec[i];
                            }
			    for(int i = 0; i < 3; ++i)
                            {
                                min_[i] += dir[i] * (b->global_bounds.max[i] - b->global_bounds.min[i]);
                                max_[i] += dir[i] * (b->global_bounds.max[i] - b->global_bounds.min[i]);
                            }
                	    std::vector<int> list;
                            itree_spatial->GetElementsListFromRange(min_, max_, list); //基于区间树查找相交box
                            for(int i = 0; i < list.size(); ++i)
                            {
		                if(list[i] != b->bid.gid) // do not send to itself
		                to_send[pid].insert(list[i]);
	                    }
			}
	    }//if(wrap)
        }//for vi
    }while(clo.inc());

    // enqueue the particles
    size_t enqueued = 0;
    for(int p = 0; p <  b->num_orig_particles; p++)
    {
        for(std::set<int>::iterator it  = to_send[p].begin(); it != to_send[p].end(); it++)
        {
	    if(ch_indices[b->lid].count(p) && neighbors.count(*it)) // already send as convex hull particle
	        continue;

            rp.particle =  b->particles[p];
            rp.info.bid = b->bid;
            rp.info.index = p;
            diy::BlockID bid(*it, assigner.rank(*it));
            cp.enqueue(bid, rp);
            ++enqueued;
        }
    }

    return enqueued;
}

template<typename RealType, unsigned NDIM>
void
local_cells(ParticleBlock<RealType, NDIM>* b,
            const diy::Master::ProxyWithLink& cp,
            bool wrap,
            voro::container* con[],
            voro::particle_order* po[])
{
    int index = b->lid;
    int nx,ny,nz;
    voro::pre_container pcon(b->global_bounds.min[0], b->global_bounds.max[0], b->global_bounds.min[1], b->global_bounds.max[1], b->global_bounds.min[2], b->global_bounds.max[2], wrap, wrap, wrap);
    for(int j = 0; j < b->num_orig_particles; j++)
        pcon.put(j,  b->particles[j][0],  b->particles[j][1],  b->particles[j][2]);
    pcon.guess_optimal(nx,ny,nz);
    con[index] = new voro::container(b->global_bounds.min[0], b->global_bounds.max[0], b->global_bounds.min[1], b->global_bounds.max[1], b->global_bounds.min[2], b->global_bounds.max[2], 2*nx, 2*ny, 2*nz, wrap, wrap, wrap, 8);
    po[index] = new voro::particle_order();
    pcon.setup(*po[index], *con[index]);

    for(int j = b->num_orig_particles; j < b->num_particles; j++)
        con[index]->put(j, b->particles[j][0], b->particles[j][1], b->particles[j][2]);
}

template<typename RealType, unsigned NDIM>
void
update_cells(ParticleBlock<RealType, NDIM>* b,
             const diy::Master::ProxyWithLink& cp,
             bool wrap,
             voro::container* con[],
             voro::particle_order* po[],
             bool test)
{
    int index = b->lid;
    for(int j = con[index]->total_particles(); j < b->num_particles; j++)
        con[index]->put(j, b->particles[j][0], b->particles[j][1], b->particles[j][2]);

    // Add a wall to the container
    //voro::wall_cylinder wall(15,15,15,0,0,1,10);
    //voro::wall_sphere wall(15, 15, 15, 5);
    //con[index]->add_wall(wall);

    if(test)
        con[index]->compute_all_cells();
}

template<typename RealType, unsigned NDIM>
void
Voronoi_kd(diy::Master& master,
           bool wrap,
           voro::container* con[],
           voro::particle_order* po[],
	   avtIntervalTree* itree_spatial,
	   diy::StaticAssigner& assigner,
           bool test)
{
    std::vector<std::set<size_t> > ch_indices(master.size()); // NB: multi-blocks
    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {convex_hull<RealType, NDIM>(b, cp, ch_indices);});
    master.exchange();
    master.foreach(&recv_particles<RealType, NDIM>);

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {local_cells<RealType, NDIM>(b, cp, wrap, con, po);});

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {incomplete_cells<RealType, NDIM>(b, cp, wrap, con, po, itree_spatial, assigner, ch_indices);});
    bool remote = true;  // NB: (remote) exchange some data outside the links
    master.exchange(remote);
    master.foreach(&recv_particles<RealType, NDIM>);

    master.foreach([&](ParticleBlock<RealType, NDIM>* b, const diy::Master::ProxyWithLink & cp)
                  {update_cells<RealType, NDIM>(b, cp, wrap, con, po, test);});
}

#endif


#include <iostream>
#include <stdlib.h>
#include <cmath>

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

#include <boost/timer/timer.hpp>

namespace ublas  = boost::numeric::ublas;

template<class mat, class vec>
double diff(const mat& A, const vec& x, const vec& b) {
  vec temp(prod(A, x) - b);
  double result = 0;
  for (typename vec::size_type i=0; i<temp.size(); ++i) {
    result += temp(i)*temp(i);
  }
  return std::sqrt(result);
}

template<class mat, class vec>
double diff(const vec& x, const mat& A, const vec& b) {
  return diff(trans(A), x, b);
}

namespace ublas  = boost::numeric::ublas;

int main() {
  const int n=7000;
#if 1
  ublas::compressed_matrix<double, ublas::row_major>     mat_row_upp(n, n);
  ublas::compressed_matrix<double, ublas::column_major>  mat_col_upp(n, n);
  ublas::compressed_matrix<double, ublas::row_major>     mat_row_low(n, n);
  ublas::compressed_matrix<double, ublas::column_major>  mat_col_low(n, n);
#else
  ublas::matrix<double, ublas::row_major>     mat_row_upp(n, n, 0);
  ublas::matrix<double, ublas::column_major>  mat_col_upp(n, n, 0);
  ublas::matrix<double, ublas::row_major>     mat_row_low(n, n, 0);
  ublas::matrix<double, ublas::column_major>  mat_col_low(n, n, 0);
#endif
  ublas::vector<double>  b(n, 1);

  std::cerr << "Constructing..." << std::endl;
  for (int i=0; i<n; ++i) {
    b(i) = std::rand() % 10;
    double main = -10 + std::rand() % 20 ;
    if (main == 0) main+=1;
    double side = -10 + std::rand() % 20 ;
    if (i-1>=0) {
      mat_row_low(i, i-1) = side;
    }
    mat_row_low(i, i) = main;

    mat_col_low(i, i) = main;
    if (i+1<n) {
      mat_col_low(i+1, i) = side;
    }

    mat_row_upp(i, i) = main;
    if (i+1<n) {
      mat_row_upp(i, i+1) = side;
    }

    if (i-1>=0) {
      mat_col_upp(i-1, i) = side;
    }
    mat_col_upp(i, i) = main;
  }

  std::cerr << "Starting..." << std::endl;
  {
    boost::timer::auto_cpu_timer t(std::cerr, "col_low x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_col_low,  x, ublas::lower_tag());
    std::cerr << "delta: " << diff(mat_col_low, x, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "row_low x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_row_low, x, ublas::lower_tag());
    std::cerr << "delta: " << diff(mat_row_low, x, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "col_upp x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_col_upp,  x, ublas::upper_tag());
    std::cerr << "delta: " << diff(mat_col_upp, x, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "row_upp x: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(mat_row_upp, x, ublas::upper_tag());
    std::cerr << "delta: " << diff(mat_row_upp, x, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "x col_low: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_col_low, ublas::lower_tag());
    std::cerr << "delta: " << diff(x, mat_col_low, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "x row_low: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_row_low, ublas::lower_tag());
    std::cerr << "delta: " << diff(x, mat_row_low, b) << "\n";
  }

  {
    boost::timer::auto_cpu_timer t(std::cerr, "x col_upp: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_col_upp, ublas::upper_tag());
    std::cerr << "delta: " << diff(x, mat_col_upp, b) << "\n";
  }
  {
    boost::timer::auto_cpu_timer t(std::cerr, "x row_upp: %t sec CPU, %w sec real\n");
    ublas::vector<double>  x(b);
    ublas::inplace_solve(x, mat_row_upp, ublas::upper_tag());
    std::cerr << "delta: " << diff(x, mat_row_upp, b) << "\n";
  }

}


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// Geometric.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file Geometric.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Recursive geometric product for project_namespace.

#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

#include <Eigen/Core>

#include "project_namespace/Mvec.hpp"
#include "project_namespace/Constants.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace {

    template<typename T> class Mvec;

    /// \brief Recursively compute the geometric product between two multivectors mv1 and mv2, the result is put into the multivector mv3
    /// \tparam the type of value that we manipulate, either float or double or something.
    /// \param mv1 - the first multivector
    /// \param mv2 - the second multivector
    /// \param mv3 - the multivector that will content the result of the operation mv3 = mv1 * mv2
    /// \param grade_mv1 - the grade of the first multivector
    /// \param grade_mv2 - the grade of the second multivector
    /// \param grade_mv3 - could be the grade of the result, however here it is useless due to the fact that the resulting multivector may not be homogeneous
    /// \param currentGradeMv1 - the current grade of the traversed tree of mv1
    /// \param currentGradeMv2 - the current grade of the traversed tree of mv2
    /// \param currentGradeMv3 - the current grade of the traversed tree of mv3
    /// \param sign - compute the sign of the geometric product between two blades
    /// \param complement - activate the flip of sign
    /// \param indexLastVector_mv1 - last vector traversed in the multivector mv1
    /// \param indexLastVector_mv2 - last vector traversed in the multivector mv2
    /// \param indexLastVector_mv3 - last vector traversed in the multivector mv3
    /// \param currentMetricCoefficient - coefficient that is used for handling the metric in the recursive formula
    /// \param depth - depth in the resulting multivector tree
    template<typename T>
    void geoProduct(const Eigen::Matrix<T, Eigen::Dynamic, 1> &mv1,
                    const Eigen::Matrix<T, Eigen::Dynamic, 1> &mv2,
                    Mvec<T> &mv3,          // multivectors to be processed
                    const unsigned int grade_mv1,
                    const unsigned int grade_mv2,
                    const unsigned int grade_mv3,   // grade of the k-vectors
                    unsigned int currentXorIdx1=0, unsigned int currentXorIdx2=0, unsigned int currentXorIdx3=0,    // position in the prefix tree
                    unsigned int currentGradeMv1=0, unsigned int currentGradeMv2=0, unsigned int currentGradeMv3=0, // grade relative to the position in the prefix tree
                    int sign=1, int complement=1,
                    unsigned int indexLastVector_mv1=0, unsigned int indexLastVector_mv2=0, unsigned int indexLastVector_mv3=0,
                    double currentMetricCoefficient=1.0, int depth=0) {

        // sign updating
        int tmpSign = sign;
        if(complement == -1) {
            tmpSign = -tmpSign;
        }

        // when we reach the grade of mv1 and mv2
        if( (currentGradeMv1==grade_mv1) && (currentGradeMv2==grade_mv2) ){
            // do the required computation
            mv3.at(currentGradeMv3, xorIndexToHomogeneousIndex[currentXorIdx3]) += sign * currentMetricCoefficient * mv1(xorIndexToHomogeneousIndex[currentXorIdx1]) * mv2(xorIndexToHomogeneousIndex[currentXorIdx2]);
        }else {
            // if position in the tree for mv3 is not yet of grade of mv3, just call the recursive calls, without computation
            // for each possible children of the current node whose index is given by depth
            for (unsigned int i = (unsigned int) (1 << depth);
                 i < (1 << algebraDimension); i *= 2) {

                unsigned int xorIndexMv1Child = currentXorIdx1 + i; // xor index of the child of the first tree multivector
                unsigned int xorIndexMv2Child = currentXorIdx2 + i; // xor index of the child of the second tree multivector
                unsigned int xorIndexMv3Child = currentXorIdx3 + i; // xor index of the child of the third tree multivector

                // if we reach neither the grade of mv1 nor the grade of mv2 AND if the child of the node of mv1 lead to at least one node whose grade is grade_mv1
                // AND if the child of the node of mv2 lead to at least one node whose grade is grade_mv2
                if ((currentGradeMv1 < grade_mv1) &&
                    ((i << (grade_mv1 - (currentGradeMv1 + 1))) < (1 << algebraDimension)) &&
                    (currentGradeMv2 < grade_mv2) &&
                    ((i << (grade_mv2 - (currentGradeMv2 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  xorIndexMv1Child, xorIndexMv2Child, currentXorIdx3,
                                  currentGradeMv1 + 1, currentGradeMv2 + 1, currentGradeMv3,
                                  tmpSign, -complement,
                                  i, i << 1, indexLastVector_mv3,
                                  diagonalMetric<T>(depth) * currentMetricCoefficient,
                                  depth + 1); // scalar product part of the geometric product
                }

                // if we do not reach the grade of mv1 AND if the child of the node of mv1 lead to at least one node whose grade is grade_mv1
                if ((currentGradeMv1 < grade_mv1) &&
                    ((i << (grade_mv1 - (currentGradeMv1 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  xorIndexMv1Child, currentXorIdx2, xorIndexMv3Child,
                                  currentGradeMv1 + 1, currentGradeMv2, currentGradeMv3 + 1,
                                  tmpSign, complement,
                                  i, i << 1, indexLastVector_mv3,
                                  currentMetricCoefficient, depth + 1); // outer part of the geometric product to CHECK
                }

                // if we do not reach the grade of mv2 AND if the child of the node of mv2 lead to at least one node whose grade is grade_mv2
                if ((currentGradeMv2 < grade_mv2) &&
                    ((i << (grade_mv2 - (currentGradeMv2 + 1))) < (1 << algebraDimension))) {
                    geoProduct<T>(mv1, mv2, mv3,
                                  grade_mv1, grade_mv2, grade_mv3,
                                  currentXorIdx1, xorIndexMv2Child, xorIndexMv3Child,
                                  currentGradeMv1, currentGradeMv2 + 1, currentGradeMv3 + 1,
                                  sign, -complement,
                                  indexLastVector_mv1, i << 1, i,
                                  currentMetricCoefficient, depth + 1); // outer part of the geometric product
                }
                depth++;
            }
        }
    }

}/// End of Namespace

#endif // project_inclusion_guard


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

#include <iostream>
#include <utility>
#include <cmath>
// #include <boost/test/minimal.hpp>

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

using namespace std;

template <typename Vector>
struct f_test
{
    f_test() : s(3)
    {
	mtl::vec::inserter<Vector> ins(s);
	ins[0] << 1; ins[1] << 2; ins[2] << 2;
    }

    typename mtl::Collection<Vector>::value_type
    operator() (const Vector& x) const
    {
	return dot(s, Vector(ele_prod(x, x)));
    }
    Vector s;
};

template <typename Vector>
struct grad_f_test
{
    grad_f_test() : s(3)
    {
	mtl::vec::inserter<Vector> ins(s);
	ins[0] << 2; ins[1] << 4; ins[2] << 4;
    }

    Vector operator() (const Vector& x) const {  return Vector(ele_prod(s, x)); }
    Vector s;
};

int main(int, char**)
{
    using namespace mtl;
    using mtl::io::tout;

    mtl::dense_vector<double>       x(3, 8);
    tout << "x= " << x << "\n";

    grad_f_test<mtl::dense_vector<double> > grad_f;
    f_test<mtl::dense_vector<double> >      f;

    itl::cyclic_iteration<double>  iter(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter1(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter2(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter3(grad_f(x), 1000, 0, 1e-4, 100);
    itl::cyclic_iteration<double> iter4(grad_f(x), 1000, 0, 1e-4, 100);

    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::bfgs(), iter);
    iter.error_code();

   // tout << "x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter.atol()) throw "x should be 0.";
    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::dfp(), iter1);
    iter1.error_code();

   // tout << "dfp x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter1.atol()) throw "x should be 0.";

    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::broyden(), iter2);
    iter2.error_code();

    tout << "broyden x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter2.atol()) throw "x should be 0.";

 #if 0  //bad condition on some compiler
    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::sr1(), iter3);
    iter3.error_code();

    tout << "sr1 x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n";
    if (two_norm(x) > 10 * iter3.atol())
	throw "x should be 0.";
#endif

    x= 8;
    quasi_newton(x, f, grad_f, itl::wolf<>(), itl::psb(), iter4);
    iter4.error_code();

    tout << "psb x= " << x << "\n";
    tout << "grad_f(x)= " << grad_f(x) << "\n\n";
    if (two_norm(x) > 10 * iter4.atol()) throw "x should be 0.";

    return 0;
}


#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <vector>

template <typename State, typename Input, typename ProcessNoiseVec>
class ManifoldCDKF
{
 public:
  using Scalar = typename State::Scalar;

  template <int N>
  using Vec = Eigen::Matrix<Scalar, N, 1>;

  template <int N, int M>
  using Mat = Eigen::Matrix<Scalar, N, M>;

  using StateCov = Mat<State::tangent_dim_, State::tangent_dim_>;

  using ProcessModelFunc = std::function<State(
      State const &state, Input const &u, ProcessNoiseVec const &w, Scalar dt)>;

  ManifoldCDKF(State const &state, StateCov const &state_covariance,
               ProcessModelFunc const &process_model)
      : state_{state},
        state_cov_{state_covariance},
        process_model_{process_model}
  {
    if(!process_model_)
    {
      throw std::invalid_argument("Invalid process_model");
    }
  }

  /// Get current state
  State getState() const { return state_; }
  /// Set current state
  void setState(State const &state) { state_ = state; }

  /// Get current state covariance
  StateCov getStateCovariance() const { return state_cov_; }
  /// Set current state covariance
  void setStateCovariance(StateCov const &cov) { state_cov_ = cov; }

  /**
   * Set the CDKF sigma point spread parameter. The sigma points are generated
   * as \f$ x \pm h \sqrt{P} \f$. The recommended value for gaussian noise is
   * \f$ \sqrt{3} \f$.
   *
   * @param[in] h Sigma point spread parameter
   */
  void setParameter(Scalar h) { h_ = h; }

  void setSigmaPointMeanParameters(Scalar threshold,
                                   unsigned int max_iterations)
  {
    sigma_points_mean_threshold_ = threshold;
    sigma_points_mean_max_iterations_ = max_iterations;
  }

  /**
   * Run the process update using the provided process model, input and input
   * covariance.
   *
   * @param[in] dt Time duration since last processUpdate
   * @param[in] u Input
   * @param[in] Q Input covariance
   * @param[in] debug Flag indicating whether to print internal debug messages
   *
   * @return True if the process update was successful else false
   */
  template <typename InputCov>
  bool processUpdate(Scalar const dt, Input const &u, InputCov const &Q,
                     bool debug = false);

  /**
   * Run the measurement update using the provided measurement model , current
   * measurement and measurement covariance.
   *
   * @param[in] measurement_func takes in the state and returns the expected
   * measurement with type MeasurementType
   * @param[in] z Current measurement
   * @param[in] R Measurement covariance
   * @param[in] debug Flag indicating whether to print internal debug messages
   *
   * @return True if the measurement update was successful else false
   */
  template <typename MeasurementFunc, typename MeasurementType,
            typename MeasCovType>
  bool measurementUpdate(MeasurementFunc const &measurement_func,
                         MeasurementType const &z, MeasCovType const &R,
                         bool debug = false);

  // TODO(Kartik): Add a MeasurementUpdateLinear function

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 private:
  /**
   * Generate weights for the CDKF equations
   *
   * @param[in] L Number of sigma points that would be used
   *
   * @return Array containing the 4 weights, [wm0, wm1, wc1, wc2]
   */
  std::array<Scalar, 4> generateWeights(unsigned int L) const
  {
    Scalar const h_sq = h_ * h_;
    Scalar const wm0 = (h_sq - L) / h_sq;
    Scalar const wm1 = 1 / (2 * h_sq);
    Scalar const wc1 = 1 / (4 * h_sq);
    Scalar const wc2 = (h_sq - 1) / (4 * h_sq * h_sq);
    return {wm0, wm1, wc1, wc2};
  }

  /**
   * Compute the mean of the sigma points
   *
   * @param[in] sigma_points Input sigma points
   * @param[in] wm0 Weight wm0 for CDKF
   * @param[in] wm1 Weight wm1 for CDKF
   *
   * @return Mean of the sigma points
   */
  template <typename T>
  T meanOfSigmaPoints(
      std::vector<T, Eigen::aligned_allocator<T>> const &sigma_points,
      Scalar wm0, Scalar wm1) const
  {
    T mean = sigma_points[0];
    unsigned int iterations = 0;
    typename T::TangentVec dx;
    do
    {
      dx = wm0 * (sigma_points[0] - mean);
      for(unsigned int i = 1; i < sigma_points.size(); ++i)
      {
        dx += wm1 * (sigma_points[i] - mean);
      }
      mean = mean + dx;
      iterations += 1;
      // std::cout << "meanOfSigmaPoints: dx.norm(): " << dx.norm()
      //           << ", iterations: " << iterations << "\n";
    } while((dx.squaredNorm() >
             sigma_points_mean_threshold_ * sigma_points_mean_threshold_) &&
            (iterations < sigma_points_mean_max_iterations_));

    return mean;
  }

  /**
   * Calculate the matrix square root of a positive (semi-)definite matrix.
   *
   * @param[in] mat The positive (semi-)definite matrix
   *
   * @return The square root of the input matrix which satisfies \f$ret *
   * ret^{T} = mat\f$
   */
  template <typename Derived>
  static typename Derived::PlainObject matrixSquareRoot(
      Eigen::MatrixBase<Derived> const &mat)
  {
    // Try LLT first
    {
      Eigen::LLT<typename Derived::PlainObject> const cov_chol{mat};
      if(cov_chol.info() == Eigen::Success)
      {
        return cov_chol.matrixL();
      }
    }
    // If not successful, try LDLT
    {
      Eigen::LDLT<typename Derived::PlainObject> const cov_chol{mat};
      if(cov_chol.info() == Eigen::Success)
      {
        typename Derived::PlainObject const L = cov_chol.matrixL();
        auto const &P = cov_chol.transpositionsP();
        auto const D = cov_chol.vectorD().array();
        if((D >= 0).all())
        {
          auto const D2 = D.sqrt().matrix().asDiagonal();
          return P.transpose() * L * D2;
        }
      }
    }
    // If not successful, try eigen-decomposition with slightly inflated
    // eigenvalues
    {
      Eigen::SelfAdjointEigenSolver<typename Derived::PlainObject> const cov_es{
          mat};
      auto eigvals = cov_es.eigenvalues().array().eval();
      auto const lowest_eval = eigvals(0);
      if(lowest_eval < 0)
      {
        eigvals -= lowest_eval;
      }
      return cov_es.eigenvectors() * eigvals.sqrt().matrix().asDiagonal() *
             cov_es.eigenvectors().transpose();
    }
  }

  /// State
  State state_;

  /// State Covariance
  StateCov state_cov_;

  /// Process model
  ProcessModelFunc const process_model_;

  /// CDKF Parameter
  Scalar h_ = std::sqrt(Scalar(3));

  /// Threshold for change in state during sigma point mean computation
  Scalar sigma_points_mean_threshold_ =
      10 * std::numeric_limits<Scalar>::epsilon();

  /// Maximum number of iterations for sigma point mean computation
  unsigned int sigma_points_mean_max_iterations_ = 10;
};

template <typename State, typename Input, typename ProcNoiseVec>
template <typename InputCov>
bool ManifoldCDKF<State, Input, ProcNoiseVec>::processUpdate(Scalar const dt,
                                                             Input const &u,
                                                             InputCov const &Q,
                                                             bool debug)
{
  if(dt < 0)
  {
    return false;
  }

  if(debug) // For debugging
  {
    std::cout << std::string(32, '=') << " Process Update "
              << std::string(32, '=') << "\n";
    std::cout << "State:\n" << state_ << std::endl;
    std::cout << "state_cov:\n" << state_cov_ << std::endl;
  }

  auto const proc_noise_count = static_cast<unsigned int>(Q.rows());
  auto const L = State::tangent_dim_ + proc_noise_count;

  // Generate sigma points
  auto const X = (h_ * matrixSquareRoot(state_cov_)).eval();
  auto const W = (h_ * matrixSquareRoot(Q)).eval();
  auto const weights = generateWeights(L);
  auto const wm0 = weights[0], wm1 = weights[1], wc1 = weights[2],
             wc2 = weights[3];
  // auto const [wm0, wm1, wc1, wc2] = generateWeights(L); // In C++17

  std::vector<State, Eigen::aligned_allocator<State>> Xa(2 * L + 1);

  // Apply process model
  Xa[0] = process_model_(state_, u, ProcNoiseVec::Zero(), dt);
  for(unsigned int k = 1; k <= State::tangent_dim_; ++k)
  {
    Xa[k] = process_model_(state_ + X.col(k - 1), u, ProcNoiseVec::Zero(), dt);
    Xa[L + k] =
        process_model_(state_ + -X.col(k - 1), u, ProcNoiseVec::Zero(), dt);
  }
  for(unsigned int k = 1; k <= proc_noise_count; ++k)
  {
    Xa[State::tangent_dim_ + k] = process_model_(state_, u, W.col(k - 1), dt);
    Xa[L + State::tangent_dim_ + k] =
        process_model_(state_, u, -W.col(k - 1), dt);
  }

  state_ = meanOfSigmaPoints(Xa, wm0, wm1);

  // Covariance
  state_cov_.setZero();
  for(unsigned int k = 1; k <= L; ++k)
  {
    auto const x1 = Xa[k] - Xa[0];
    auto const x2 = Xa[L + k] - Xa[0];
    auto const d1 = (x1 - x2).eval();
    auto const d2 = (x1 + x2).eval();
    state_cov_ += wc1 * d1 * d1.transpose() + wc2 * d2 * d2.transpose();
  }

  if(debug)
  {
    std::cout << std::string(80, '=') << "\n";
  }

  return true;
}

template <typename State, typename Input, typename ProcessNoiseVec>
template <typename MeasurementFunc, typename MeasurementType,
          typename MeasCovType>
bool ManifoldCDKF<State, Input, ProcessNoiseVec>::measurementUpdate(
    MeasurementFunc const &measurement_func, MeasurementType const &z,
    MeasCovType const &R, bool debug)
{
  if(debug)
  {
    std::cout << std::string(30, '=') << " Measurement Update "
              << std::string(30, '=') << "\n";
  }

  constexpr unsigned int L = State::tangent_dim_;

  // Generate sigma points
  auto const X = (h_ * matrixSquareRoot(state_cov_)).eval();
  auto const weights = generateWeights(L);
  auto const wm0 = weights[0], wm1 = weights[1], wc1 = weights[2],
             wc2 = weights[3];
  // auto const [wm0, wm1, wc1, wc2] = generateWeights(L); // In C++17

  if(debug)
  {
    std::cout << "Pa:\n" << state_cov_ << std::endl;
    std::cout << "X:\n" << X << std::endl;
  }

  // Apply measurement model
  std::vector<MeasurementType, Eigen::aligned_allocator<MeasurementType>> Zaa(
      2 * L + 1);

  Zaa[0] = measurement_func(state_);
  for(unsigned int k = 1; k <= L; k++)
  {
    Zaa[k] = measurement_func(state_ + X.col(k - 1));
    Zaa[L + k] = measurement_func(state_ + -X.col(k - 1));
  }
  if(debug)
  {
    for(unsigned int k = 0; k < 2 * L + 1; ++k)
    {
      std::cout << "Z[" << k << "]:\n" << Zaa[k] << std::endl;
    }
  }

  auto const z_pred = meanOfSigmaPoints(Zaa, wm0, wm1);

  // Covariance
  auto Pzz = MeasCovType::Zero(R.rows(), R.cols()).eval();
  auto Pxz =
      Mat<State::tangent_dim_, MeasurementType::tangent_dim_>::Zero().eval();
  for(unsigned int k = 1; k <= L; k++)
  {
    auto const z1 = Zaa[k] - Zaa[0];
    auto const z2 = Zaa[L + k] - Zaa[0];
    auto const dz1 = (z1 - z2).eval();
    auto const dz2 = (z1 + z2).eval();
    Pzz += wc1 * dz1 * dz1.transpose() + wc2 * dz2 * dz2.transpose();
    Pxz += wm1 * X.col(k - 1) * dz1.transpose();
  }
  Pzz += R;

  // Innovation
  auto const inno = decltype(z_pred){z} - z_pred;
  if(debug)
  {
    std::cout << "z:\n" << z << "\n";
    std::cout << "z_pred:\n" << z_pred << "\n";
    std::cout << "Pxz:\n" << Pxz << "\n";
    std::cout << "Pzz:\n" << Pzz << "\n";
    // Kalman Gain;
    auto const K = Pxz * Pzz.inverse();
    std::cout << "K:\n" << K << "\n";
    std::cout << "inno: " << inno.transpose() << "\n";
  }

  auto dx = State::TangentVec::Zero().eval();

  auto const Pzz_llt = Pzz.llt();
  if(Pzz_llt.info() == Eigen::Success) // Pzz is positive-definite
  {
    dx = Pxz * Pzz_llt.solve(inno);
    // Covariance around the old mean
    state_cov_ -= Pxz * Pzz_llt.solve(Pxz.transpose());
  }
  else
  {
    // Pzz is not positive definite, try LDLT
    auto const Pzz_ldlt = Pzz.ldlt();
    if(Pzz_ldlt.info() == Eigen::Success) // Pzz is positive semi-definite
    {
      dx = Pxz * Pzz_ldlt.solve(inno);
      // Covariance around the old mean
      state_cov_ -= Pxz * Pzz_ldlt.solve(Pxz.transpose());
    }
  }

  // Make sure that diagonal elements of state_cov_ are non-negative
  for(unsigned int i = 0; i < State::tangent_dim_; ++i)
  {
    if(state_cov_(i, i) < 0)
    {
      for(unsigned int j = 0; j < State::tangent_dim_; ++j)
      {
        state_cov_(i, j) = 0;
        state_cov_(j, i) = 0;
      }
    }
  }

  if(debug)
  {
    std::cout << "dx: " << dx.transpose() << "\n";
    std::cout << "state_cov:\n" << state_cov_ << "\n";
  }

  // Compute the mean and move the covariance to be around the new mean
  auto const X_new = (h_ * matrixSquareRoot(state_cov_)).eval();
  if(debug)
  {
    std::cout << "X_new:\n" << X_new << "\n";
  }

  std::vector<State, Eigen::aligned_allocator<State>> Xa(2 * L + 1);
  Xa[0] = state_ + dx;
  for(unsigned int k = 1; k <= L; k++)
  {
    Xa[k] = state_ + (dx + X_new.col(k - 1));
    Xa[L + k] = state_ + (dx - X_new.col(k - 1));
  }

  // Get mean
  state_ = meanOfSigmaPoints(Xa, wm0, wm1);

  // Calculate covariance around the new mean
  state_cov_.setZero();
  for(unsigned int k = 1; k <= L; ++k)
  {
    auto const x1 = Xa[k] - Xa[0];
    auto const x2 = Xa[L + k] - Xa[0];
    auto const d1 = x1 - x2;
    auto const d2 = x1 + x2;
    state_cov_ += wc1 * d1 * d1.transpose() + wc2 * d2 * d2.transpose();
  }

  if(debug)
  {
    std::cout << std::string(80, '=') << "\n";
  }

  return true;
}


#include "fa_hierarchy.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <cassert>
#include <iostream>
#include <stan/math/prim/prob.hpp>
#include <vector>

#include "algorithm_state.pb.h"
#include "hierarchy_prior.pb.h"
#include "ls_state.pb.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

double FAHierarchy::like_lpdf(const Eigen::RowVectorXd& datum) const {
  using stan::math::NEG_LOG_SQRT_TWO_PI;
  double base = (Eigen::MatrixXd(state.cov_chol.matrixL()))
                    .diagonal()
                    .array()
                    .log()
                    .sum() -
                NEG_LOG_SQRT_TWO_PI * dim;
  double exp =
      0.5 * ((datum.transpose() - state.mu)
                 .dot(state.cov_chol.solve((datum.transpose() - state.mu))));
  return -(base + exp);
}

FA::State FAHierarchy::draw(const FA::Hyperparams& params) {
  auto& rng = bayesmix::Rng::Instance().get();
  FA::State out;
  out.mu = params.mutilde;
  out.psi = params.beta / (params.alpha0 + 1.);
  out.eta = Eigen::MatrixXd::Zero(card, params.q);
  out.lambda = Eigen::MatrixXd::Zero(dim, params.q);

  for (size_t j = 0; j < dim; j++) {
    out.mu[j] =
        stan::math::normal_rng(params.mutilde[j], sqrt(params.phi), rng);

    out.psi[j] = stan::math::inv_gamma_rng(params.alpha0, params.beta[j], rng);

    for (size_t i = 0; i < params.q; i++) {
      out.lambda(j, i) = stan::math::normal_rng(0, 1, rng);
    }
  }

  for (size_t i = 0; i < card; i++) {
    for (size_t j = 0; j < params.q; j++) {
      out.eta(i, j) = stan::math::normal_rng(0, 1, rng);
    }
  }

  out.psi_inverse = out.psi.cwiseInverse().asDiagonal();
  out.cov_chol = (out.lambda * out.lambda.transpose() +
                  Eigen::MatrixXd(out.psi.asDiagonal()))
                     .llt();

  return out;
}

void FAHierarchy::initialize_state() {
  state.mu = hypers->mutilde;
  state.psi = hypers->beta / (hypers->alpha0 + 1.);
  state.eta = Eigen::MatrixXd::Zero(card, hypers->q);
  state.lambda = Eigen::MatrixXd::Zero(dim, hypers->q);
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
}

void FAHierarchy::initialize_hypers() {
  if (prior->has_fixed_values()) {
    // Set values
    hypers->mutilde = bayesmix::to_eigen(prior->fixed_values().mutilde());
    dim = hypers->mutilde.size();
    hypers->beta = bayesmix::to_eigen(prior->fixed_values().beta());
    hypers->phi = prior->fixed_values().phi();
    hypers->alpha0 = prior->fixed_values().alpha0();
    hypers->q = prior->fixed_values().q();

    // Automatic initialization
    if (dim == 0) {
      hypers->mutilde = dataset_ptr->colwise().mean();
      dim = hypers->mutilde.size();
    }
    if (hypers->beta.size() == 0) {
      Eigen::MatrixXd centered =
          dataset_ptr->rowwise() - dataset_ptr->colwise().mean();
      auto cov_llt = ((centered.transpose() * centered) /
                      double(dataset_ptr->rows() - 1.))
                         .llt();
      Eigen::MatrixXd precision_matrix(
          cov_llt.solve(Eigen::MatrixXd::Identity(dim, dim)));
      hypers->beta =
          (hypers->alpha0 - 1) * precision_matrix.diagonal().cwiseInverse();
      if (hypers->alpha0 == 1) {
        throw std::invalid_argument(
            "Scale parameter must be different than 1 when automatic "
            "initialization is used");
      }
    }
    // Check validity
    if (dim != hypers->beta.rows()) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    for (size_t j = 0; j < dim; j++) {
      if (hypers->beta[j] <= 0) {
        throw std::invalid_argument("Shape parameter must be > 0");
      }
    }
    if (hypers->alpha0 <= 0) {
      throw std::invalid_argument("Scale parameter must be > 0");
    }
    if (hypers->phi <= 0) {
      throw std::invalid_argument("Diffusion parameter must be > 0");
    }
    if (hypers->q <= 0) {
      throw std::invalid_argument("Number of factors must be > 0");
    }
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void FAHierarchy::update_hypers(
    const std::vector<bayesmix::AlgorithmState::ClusterState>& states) {
  auto& rng = bayesmix::Rng::Instance().get();
  if (prior->has_fixed_values()) {
    return;
  }

  else {
    throw std::invalid_argument("Unrecognized hierarchy prior");
  }
}

void FAHierarchy::update_summary_statistics(const Eigen::RowVectorXd& datum,
                                            bool add) {
  if (add) {
    data_sum += datum;
  } else {
    data_sum -= datum;
  }
}

void FAHierarchy::clear_summary_statistics() {
  data_sum = Eigen::VectorXd::Zero(dim);
}

void FAHierarchy::set_state_from_proto(
    const google::protobuf::Message& state_) {
  auto& statecast = downcast_state(state_);
  state.mu = bayesmix::to_eigen(statecast.fa_state().mu());
  state.psi = bayesmix::to_eigen(statecast.fa_state().psi());
  state.eta = bayesmix::to_eigen(statecast.fa_state().eta());
  state.lambda = bayesmix::to_eigen(statecast.fa_state().lambda());
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
  set_card(statecast.cardinality());
}

std::shared_ptr<bayesmix::AlgorithmState::ClusterState>
FAHierarchy::get_state_proto() const {
  bayesmix::FAState state_;
  bayesmix::to_proto(state.mu, state_.mutable_mu());
  bayesmix::to_proto(state.psi, state_.mutable_psi());
  bayesmix::to_proto(state.eta, state_.mutable_eta());
  bayesmix::to_proto(state.lambda, state_.mutable_lambda());

  auto out = std::make_shared<bayesmix::AlgorithmState::ClusterState>();
  out->mutable_fa_state()->CopyFrom(state_);
  return out;
}

void FAHierarchy::set_hypers_from_proto(
    const google::protobuf::Message& hypers_) {
  auto& hyperscast = downcast_hypers(hypers_).fa_state();
  hypers->mutilde = bayesmix::to_eigen(hyperscast.mutilde());
  hypers->alpha0 = hyperscast.alpha0();
  hypers->beta = bayesmix::to_eigen(hyperscast.beta());
  hypers->phi = hyperscast.phi();
  hypers->q = hyperscast.q();
}

std::shared_ptr<bayesmix::AlgorithmState::HierarchyHypers>
FAHierarchy::get_hypers_proto() const {
  bayesmix::FAPriorDistribution hypers_;
  bayesmix::to_proto(hypers->mutilde, hypers_.mutable_mutilde());
  bayesmix::to_proto(hypers->beta, hypers_.mutable_beta());
  hypers_.set_alpha0(hypers->alpha0);
  hypers_.set_phi(hypers->phi);
  hypers_.set_q(hypers->q);

  auto out = std::make_shared<bayesmix::AlgorithmState::HierarchyHypers>();
  out->mutable_fa_state()->CopyFrom(hypers_);
  return out;
}

void FAHierarchy::sample_full_cond(bool update_params) {
  if (this->card == 0) {
    // No posterior update possible
    sample_prior();
  } else {
    sample_eta();
    sample_mu();
    sample_psi();
    sample_lambda();
  }
}

void FAHierarchy::sample_eta() {
  auto& rng = bayesmix::Rng::Instance().get();
  auto sigma_eta_inv_llt =
      (Eigen::MatrixXd::Identity(hypers->q, hypers->q) +
       state.lambda.transpose() * state.psi_inverse * state.lambda)
          .llt();
  if (state.eta.rows() != card) {
    state.eta = Eigen::MatrixXd::Zero(card, state.eta.cols());
  }
  Eigen::MatrixXd temp_product(
      sigma_eta_inv_llt.solve(state.lambda.transpose() * state.psi_inverse));
  auto iterator = cluster_data_idx.begin();
  for (size_t i = 0; i < card; i++, iterator++) {
    Eigen::VectorXd tempvector(dataset_ptr->row(
        *iterator));  // TODO use slicing when Eigen is updated to v3.4
    state.eta.row(i) = (bayesmix::multi_normal_prec_chol_rng(
        temp_product * (tempvector - state.mu), sigma_eta_inv_llt, rng));
  }
}

void FAHierarchy::sample_mu() {
  auto& rng = bayesmix::Rng::Instance().get();
  Eigen::DiagonalMatrix<double, Eigen::Dynamic> sigma_mu;

  sigma_mu.diagonal() =
      (card * state.psi_inverse.diagonal().array() + hypers->phi)
          .cwiseInverse();

  Eigen::VectorXd sum = (state.eta.colwise().sum());

  Eigen::VectorXd mumean =
      sigma_mu * (hypers->phi * hypers->mutilde +
                  state.psi_inverse * (data_sum - state.lambda * sum));

  state.mu = bayesmix::multi_normal_diag_rng(mumean, sigma_mu, rng);
}

void FAHierarchy::sample_lambda() {
  auto& rng = bayesmix::Rng::Instance().get();

  Eigen::MatrixXd temp_etateta(state.eta.transpose() * state.eta);

  for (size_t j = 0; j < dim; j++) {
    auto sigma_lambda_inv_llt =
        (Eigen::MatrixXd::Identity(hypers->q, hypers->q) +
         temp_etateta / state.psi[j])
            .llt();
    Eigen::VectorXd tempsum(card);
    const Eigen::VectorXd& data_col = dataset_ptr->col(j);
    auto iterator = cluster_data_idx.begin();
    for (size_t i = 0; i < card; i++, iterator++) {
      tempsum[i] = data_col(
          *iterator);  // TODO use slicing when Eigen is updated to v3.4
    }
    tempsum = tempsum.array() - state.mu[j];
    tempsum = tempsum.array() / state.psi[j];

    state.lambda.row(j) = bayesmix::multi_normal_prec_chol_rng(
        sigma_lambda_inv_llt.solve(state.eta.transpose() * tempsum),
        sigma_lambda_inv_llt, rng);
  }
}

void FAHierarchy::sample_psi() {
  auto& rng = bayesmix::Rng::Instance().get();

  for (size_t j = 0; j < dim; j++) {
    double sum = 0;
    auto iterator = cluster_data_idx.begin();
    for (size_t i = 0; i < card; i++, iterator++) {
      sum += std::pow(
          ((*dataset_ptr)(*iterator, j) -
           state.mu[j] -  // TODO use slicing when Eigen is updated to v3.4
           state.lambda.row(j).dot(state.eta.row(i))),
          2);
    }
    state.psi[j] = stan::math::inv_gamma_rng(hypers->alpha0 + card / 2,
                                             hypers->beta[j] + sum / 2, rng);
  }
  state.psi_inverse = state.psi.cwiseInverse().asDiagonal();
  state.cov_chol = (state.lambda * state.lambda.transpose() +
                    Eigen::MatrixXd(state.psi.asDiagonal()))
                       .llt();
}


// Copyright © 2016-2019 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#include <cmath>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/expm1.hpp>
#include <boost/math/special_functions/log1p.hpp>
#include <boost/math/special_functions/fpclassify.hpp> // isnan
#include <vinecopulib/misc/tools_eigen.hpp>

namespace vinecopulib {
inline JoeBicop::JoeBicop()
{
    family_ = BicopFamily::joe;
    parameters_ = Eigen::VectorXd(1);
    parameters_lower_bounds_ = Eigen::VectorXd(1);
    parameters_upper_bounds_ = Eigen::VectorXd(1);
    parameters_ << 1;
    parameters_lower_bounds_ << 1;
    parameters_upper_bounds_ << 30;
}

inline double JoeBicop::generator(const double &u)
{
    return (-1) * boost::math::log1p(-std::pow(1 - u, parameters_(0)));
}

inline double JoeBicop::generator_inv(const double &u)
{
    return 1 - std::pow(-boost::math::expm1(-u), 1 / parameters_(0));
}

inline double JoeBicop::generator_derivative(const double &u)
{
    double theta = double(parameters_(0));
    return (-theta) * std::pow(1 - u, theta - 1) / (1 - std::pow(1 - u, theta));
}

//inline double JoeBicop::generator_derivative2(const double &u)
//{
//    double theta = double(parameters_(0));
//    double res = theta * (theta - 1 + std::pow(1 - u, theta));
//    return res * std::pow(1 - u, theta - 2) /
//           std::pow(-1 + std::pow(1 - u, theta), 2);
//}

inline Eigen::VectorXd JoeBicop::pdf_raw(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double theta = static_cast<double>(parameters_(0));
    auto f = [theta](const double &u1, const double &u2) {
        double t1 = std::pow(1-u1,theta);
        double t2 = std::pow(1-u2,theta);
        return std::pow(t1 + t2 - t1 * t2,1/theta-2)
               * std::pow(1-u1,theta-1)*std::pow(1-u2,theta-1)
               * (theta-1 + t1 + t2 - t1 * t2);
    };
    return tools_eigen::binaryExpr_or_nan(u, f);
}

// inverse h-function
inline Eigen::VectorXd JoeBicop::hinv1(
    const Eigen::Matrix<double, Eigen::Dynamic, 2> &u
)
{
    double theta = double(parameters_(0));
    double u1, u2;
    Eigen::VectorXd hinv = Eigen::VectorXd::Zero(u.rows());
    for (int j = 0; j < u.rows(); ++j) {
        u1 = u(j, 1);
        u2 = u(j, 0);
        if ((boost::math::isnan)(u1) | (boost::math::isnan)(u2)) {
            hinv(j) = std::numeric_limits<double>::quiet_NaN();
        } else {
            hinv(j) = qcondjoe(&u1, &u2, &theta);
        }
    }

    return hinv;
}

// link between Kendall's tau and the par_bicop parameter
inline Eigen::MatrixXd JoeBicop::tau_to_parameters(const double &tau)
{
    Eigen::VectorXd tau0 = Eigen::VectorXd::Constant(1, std::fabs(tau));
    auto f = [&](const Eigen::VectorXd &v) {
        return Eigen::VectorXd::Constant(1, std::fabs(parameters_to_tau(v)));
    };
    return tools_eigen::invert_f(tau0,
                                 f,
                                 parameters_lower_bounds_(0) + 1e-6,
                                 parameters_upper_bounds_(0) - 1e-6);
}

inline double JoeBicop::parameters_to_tau(const Eigen::MatrixXd &parameters)
{
    double par = parameters(0);
    double tau = 2 / par + 1;
    tau = boost::math::digamma(2.0) - boost::math::digamma(tau);
    return 1 + 2 * tau / (2 - par);
}

inline Eigen::VectorXd JoeBicop::get_start_parameters(const double tau)
{
    Eigen::VectorXd par = tau_to_parameters(tau);
    par = par.cwiseMax(parameters_lower_bounds_);
    par = par.cwiseMin(parameters_upper_bounds_);
    return par;
}
}

// This is copy&paste from the VineCopula package
inline double qcondjoe(double *q, double *u, double *de)
{
    double t1, t2, t3, t4, t5, t6, t7, t8, t9, t10, t11, t13, t15, t16, t19, t23, t28, t31;
    double c21, pdf;
    int iter;
    double diff, v, de1, dtem, de1inv, tem;

    t1 = 1.0 - *u;
    t2 = pow(t1, 1.0 * (*de));
    t7 = 1. / (*de);
    t10 = t2 * (*de);
    t11 = 1. / t1;
    t19 = (*de) * (*de);
    de1 = *de - 1;  // may need better modification for large delta
    dtem = -de1 / (1. + de1);
    de1inv = -1. / de1;

    // v = 0.5 * (q+u); // starting guess

    // Use a better starting point based on reflected B4 copula
    // A good starting point is crucial when delta is large because
    //    C_{2|1} will be steep
    // C_{R,2|1}(v|u)=1-C_{2|1}(1-v|1-u),
    // C_{R,2|1}^{-1}(q|u)=1-C_{2|1}^{-1}(1-q|1-u)
    tem = pow(1. - *q, dtem) - 1.;
    tem = tem * pow(1. - *u, -de1) + 1.;
    v = pow(tem, de1inv);
    v = 1. - v;
    diff = 1;
    iter = 0;
    while (fabs(diff) > 1.e-6 && iter < 20) {
        t3 = 1. - v;
        t4 = pow(t3, *de);
        t5 = t2 * t4;
        t6 = t2 + t4 - t5;
        t8 = pow(t6, t7);
        t9 = t7 * t8;
        t13 = t11 * t4;
        t15 = -t10 * t11 + t10 * t13;
        t16 = 1. / t6;
        t23 = 1. / t3;
        t28 = t6 * t6;
        t31 = (-t4 * (*de) * t23 + t5 * (*de) * t23) / t28 * t15;
        c21 = -t9 * t15 * t16;
        pdf = -t8 / t19 * t31 + t8 * (*de) * t2 * t13 * t23 * t16 + t9 * t31;
        iter++;
        if ((boost::math::isnan)(pdf) ||
            (boost::math::isnan)(c21)) {
            diff /= -2.;
        }  // added for de>=30
        else
            diff = (c21 - *q) / pdf;
        v -= diff;
        int iter2 = 0;
        while ((v <= 0 || v >= 1 || fabs(diff) > 0.25) & (iter2 < 20)) {
            ++iter2;
            diff /= 2.;
            v += diff;
        }
    }

    // make sure that boundaries are respected
    if (v <= 0) {
        v = 1e-10;
    } else if (v >= 1) {
        v = 1 - 1e-10;
    }

    return v;
}


// This file is part of KWIVER, and is distributed under the
// OSI-approved BSD 3-Clause License. See top-level LICENSE file or
// https://github.com/Kitware/kwiver/blob/master/LICENSE for details.

/**
 * \file
 * \brief core fundamental matrix template implementations
 */

#include "fundamental_matrix.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/SVD>

namespace kwiver {
namespace vital {

/// Construct from a provided matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( Eigen::Matrix<T,3,3> const &mat )
{
  Eigen::JacobiSVD<matrix_t> svd(mat, Eigen::ComputeFullU |
                                      Eigen::ComputeFullV);
  auto S = svd.singularValues();
  const matrix_t& U = svd.matrixU();
  const matrix_t& V = svd.matrixV();

  // clear the last singular value
  S[2] = T(0);
  S /= S.norm();
  mat_ = U*S.asDiagonal()*V.transpose();
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
fundamental_matrix_<float>
::fundamental_matrix_( fundamental_matrix_<float> const &other )
  : mat_( other.mat_ )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix_<double> const &other )
  : mat_( other.mat_ )
{
}

/// Construct from a generic fundamental_matrix
template <typename T>
fundamental_matrix_<T>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix().template cast<T>() )
{
}

/// Construct from a generic fundamental_matrix -- double specialization
template <>
fundamental_matrix_<double>
::fundamental_matrix_( fundamental_matrix const &base )
  : mat_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
fundamental_matrix_sptr
fundamental_matrix_<T>
::clone() const
{
  return fundamental_matrix_sptr( new fundamental_matrix_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix
template <typename T>
Eigen::Matrix<double,3,3>
fundamental_matrix_<T>
::matrix() const
{
  return this->mat_.template cast<double>();
}

/// Specialization for matrices with native double type
template <>
Eigen::Matrix<double,3,3>
fundamental_matrix_<double>
::matrix() const
{
  return this->mat_;
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// Output stream operator for \p fundamental_matrix instances
std::ostream&
operator<<( std::ostream &s, fundamental_matrix const &f )
{
  s << f.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_FUNDAMENTAL_MATRIX(T) \
  template class fundamental_matrix_<T>;

INSTANTIATE_FUNDAMENTAL_MATRIX(float);
INSTANTIATE_FUNDAMENTAL_MATRIX(double);
#undef INSTANTIATE_FUNDAMENTAL_MATRIX
/// \endcond

} } // end vital namespace


/**
 *           c++11-only implementation of the L-BFGS-B algorithm
 *
 * Copyright (c) 2014 Patrick Wieschollek
 *               https://github.com/PatWie/LBFGSB
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:

 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef LBFGSB_H_
#define LBFGSB_H_

#include "meta.h"

#include <list>
#include <stdio.h>
#include <iostream>
#include <functional>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

/* coded from scratch !!!
 * based on the paper
 * A LIMITED MEMORY ALGORITHM FOR BOUND CONSTRAINED OPTIMIZATION
 * (Byrd, Lu, Nocedal, Zhu)
 */

namespace PatWieLBFGSB {

class LBFGSB {

	// contains options for optimization process
	Options Options_;

	// oracles for function value and gradient
	FunctionOracleType FunctionObjectiveOracle_;
	GradientOracleType FunctionGradientOracle_;

	Matrix W, M;
	Vector lb, ub;
	double theta;
	int DIM;

	std::list<Vector> xHistory;

public:

	Vector XOpt;

	LBFGSB(const Vector &l, const Vector &u) :
			lb(l), ub(u), theta(1.0), DIM(l.rows()) {
		lb = l;
		ub = u;
		theta = 1.0;
		DIM = l.rows();
		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);
	}

	LBFGSB(Options &Options, const Vector &l, const Vector &u) {
		Options_ = Options;
		lb = l;
		ub = u;
		theta = 1.0;
		DIM = l.rows();
		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);

	}

	/// <summary>
	/// find cauchy point in x
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	void GetGeneralizedCauchyPoint(Vector &x, Vector &g, Vector &x_cauchy,
			Vector &c) {
		const int DIM = x.rows();
		// PAGE 8
		// Algorithm CP: Computation of the generalized Cauchy point
		// Given x,l,u,g, and B = \theta I-WMW

		// {all t_i} = { (idx,value), ... }
		// TODO: use "std::set" ?
		std::vector<std::pair<int, double> > SetOfT;
		// the feasible set is implicitly given by "SetOfT - {t_i==0}"
		Vector d = Vector::Zero(DIM, 1);

		// n operations
		for (int j = 0; j < DIM; j++) {
			if (g(j) == 0) {
				SetOfT.push_back(std::make_pair(j, INF));
			} else {
				double tmp = 0;
				if (g(j) < 0) {
					tmp = (x(j) - ub(j)) / g(j);
				} else {
					tmp = (x(j) - lb(j)) / g(j);
				}
				d(j) = -g(j);
				SetOfT.push_back(std::make_pair(j, tmp));
			}

		}Debug(d.transpose());

		// paper: using heapsort
		// sortedindices [1,0,2] means the minimal element is on the 1th entry
		std::vector<int> SortedIndices = sort_indexes(SetOfT);

		x_cauchy = x;
		// Initialize
		// p := 	W^T*p
		Vector p = (W.transpose() * d);						// (2mn operations)
		// c := 	0
		c = Eigen::MatrixXd::Zero(M.rows(), 1);
		// f' := 	g^T*d = -d^Td
		double f_prime = -d.dot(d);							// (n operations)
		// f'' :=	\theta*d^T*d-d^T*W*M*W^T*d = -\theta*f' - p^T*M*p
		double f_doubleprime = (double) (-1.0 * theta) * f_prime - p.dot(M * p);// (O(m^2) operations)
		// \delta t_min :=	-f'/f''
		double dt_min = -f_prime / f_doubleprime;
		// t_old := 	0
		double t_old = 0;
		// b := 	argmin {t_i , t_i >0}
		int i = 0;
		for (int j = 0; j < DIM; j++) {
			i = j;
			if (SetOfT[SortedIndices[j]].second != 0)
				break;
		}
		int b = SortedIndices[i];
		// see below
		// t        			:= 	min{t_i : i in F}
		double t = SetOfT[b].second;
		// \delta t 			:= 	t - 0
		double dt = t - t_old;

		// examination of subsequent segments
		while ((dt_min >= dt) && (i < DIM)) {
			if (d(b) > 0)
				x_cauchy(b) = ub(b);
			else if (d(b) < 0)
				x_cauchy(b) = lb(b);

			// z_b = x_p^{cp} - x_b
			double zb = x_cauchy(b) - x(b);
			// c   :=  c +\delta t*p
			c += dt * p;
			// cache
			Vector wbt = W.row(b);

			f_prime += dt * f_doubleprime + (double) g(b) * g(b)
					+ (double) theta * g(b) * zb
					- (double) g(b) * wbt.transpose() * (M * c);
			f_doubleprime += (double) -1.0 * theta * g(b) * g(b)
					- (double) 2.0 * (g(b) * (wbt.dot(M * p)))
					- (double) g(b) * g(b) * wbt.transpose() * (M * wbt);
			p += g(b) * wbt.transpose();
			d(b) = 0;
			dt_min = -f_prime / f_doubleprime;
			t_old = t;
			++i;
			if (i < DIM) {
				b = SortedIndices[i];
				t = SetOfT[b].second;
				dt = t - t_old;
			}

		}

		dt_min = fmax(dt_min, 0.0);
		t_old += dt_min;

		Debug(SortedIndices[0]<< " "<< SortedIndices[1]);

#pragma omp parallel for
		for (int ii = i; ii < x_cauchy.rows(); ii++) {
			x_cauchy(SortedIndices[ii]) = x(SortedIndices[ii])
					+ t_old * d(SortedIndices[ii]);
		}Debug(x_cauchy.transpose());

		c += dt_min * p;
		Debug(c.transpose());

	}

	/// <summary>
	/// find valid alpha for (8.5)
	/// </summary>
	/// <parameter name="x_cp">cauchy point</parameter>
	/// <parameter name="du">unconstrained solution of subspace minimization</parameter>
	/// <parameter name="FreeVariables">flag (1 if is free variable and 0 if is not free variable)</parameter>
	double FindAlpha(Vector &x_cp, Vector &du,
			std::vector<int> &FreeVariables) {
		/* this returns
		 * a* = max {a : a <= 1 and  l_i-xc_i <= a*d_i <= u_i-xc_i}
		 */
		double alphastar = 1;
		const unsigned int n = FreeVariables.size();
		for (unsigned int i = 0; i < n; i++) {
			if (du(i) > 0) {
				alphastar = fmin(alphastar,
						(ub(FreeVariables[i]) - x_cp(FreeVariables[i]))
								/ du(i));
			} else {
				alphastar = fmin(alphastar,
						(lb(FreeVariables[i]) - x_cp(FreeVariables[i]))
								/ du(i));
			}
		}
		return alphastar;
	}

	/// <summary>
	/// using linesearch to determine step width
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	/// <parameter name="dx">direction</parameter>
	/// <parameter name="f">current value of objective (will be changed)</parameter>
	/// <parameter name="g">current gradient of objective (will be changed)</parameter>
	/// <parameter name="t">step width (will be changed)</parameter>
	void LineSearch(Vector &x, Vector dx, double &f, Vector &g, double &t) {

		const double alpha = 0.2;
		const double beta = 0.8;

		const double f_in = f;
		const Vector g_in = g;
		const double Cache = alpha * g_in.dot(dx);

		t = 1.0;
		f = FunctionObjectiveOracle_(x + t * dx);
		while (f > f_in + t * Cache) {
			t *= beta;
			f = FunctionObjectiveOracle_(x + t * dx);
		}
		FunctionGradientOracle_(x + t * dx, g);
		x += t * dx;

	}

	/// <summary>
	/// direct primal approach
	/// </summary>
	/// <parameter name="x">start in x</parameter>
	void SubspaceMinimization(Vector &x_cauchy, Vector &x, Vector &c, Vector &g,
			Vector &SubspaceMin) {

		// cached value: ThetaInverse=1/theta;
		double theta_inverse = 1 / theta;

		// size of "t"
		std::vector<int> FreeVariablesIndex;
		Debug(x_cauchy.transpose());

		//std::cout << "free vars " << FreeVariables.rows() << std::endl;
		for (int i = 0; i < x_cauchy.rows(); i++) {
			Debug(x_cauchy(i) << " "<< ub(i) << " "<< lb(i));
			if ((x_cauchy(i) != ub(i)) && (x_cauchy(i) != lb(i))) {
				FreeVariablesIndex.push_back(i);
			}
		}
		const int FreeVarCount = FreeVariablesIndex.size();

		Matrix WZ = Matrix::Zero(W.cols(), FreeVarCount);

		for (int i = 0; i < FreeVarCount; i++)
			WZ.col(i) = W.row(FreeVariablesIndex[i]);

		Debug(WZ);

		// r=(g+theta*(x_cauchy-x)-W*(M*c));
		Debug(g);Debug(x_cauchy);Debug(x);
		Vector rr = (g + theta * (x_cauchy - x) - W * (M * c));
		// r=r(FreeVariables);
		Vector r = Matrix::Zero(FreeVarCount, 1);
		for (int i = 0; i < FreeVarCount; i++)
			r.row(i) = rr.row(FreeVariablesIndex[i]);

		Debug(r.transpose());

		// STEP 2: "v = w^T*Z*r" and STEP 3: "v = M*v"
		Vector v = M * (WZ * r);
		// STEP 4: N = 1/theta*W^T*Z*(W^T*Z)^T
		Matrix N = theta_inverse * WZ * WZ.transpose();
		// N = I - MN
		N = Matrix::Identity(N.rows(), N.rows()) - M * N;
		// STEP: 5
		// v = N^{-1}*v
		v = N.lu().solve(v);
		// STEP: 6
		// HERE IS A MISTAKE IN THE ORIGINAL PAPER!
		Vector du = -theta_inverse * r
				- theta_inverse * theta_inverse * WZ.transpose() * v;
		Debug(du.transpose());
		// STEP: 7
		double alpha_star = FindAlpha(x_cauchy, du, FreeVariablesIndex);

		// STEP: 8
		Vector dStar = alpha_star * du;

		SubspaceMin = x_cauchy;
		for (int i = 0; i < FreeVarCount; i++) {
			SubspaceMin(FreeVariablesIndex[i]) = SubspaceMin(
					FreeVariablesIndex[i]) + dStar(i);
		}
	}

	void Solve(Vector &x0, const FunctionOracleType& FunctionValue,
			const GradientOracleType& FunctionGradient) {
		FunctionObjectiveOracle_ = FunctionValue;
		FunctionGradientOracle_ = FunctionGradient;

		Assert(x0.rows() == lb.rows(), "lower bound size incorrect");
		Assert(x0.rows() == ub.rows(), "upper bound size incorrect");

		Debug(x0.transpose());Debug(lb.transpose());Debug(ub.transpose());

		Assert((x0.array() >= lb.array()).all(),
				"seed is not feasible (violates lower bound)");
		Assert((x0.array() <= ub.array()).all(),
				"seed is not feasible (violates upper bound)");

		const int DIM = x0.rows();

		xHistory.push_back(x0);

		Matrix yHistory = Matrix::Zero(DIM, 0);
		Matrix sHistory = Matrix::Zero(DIM, 0);

		Vector x = x0, g;
		int k = 0;

		double f = FunctionObjectiveOracle_(x);
		FunctionGradientOracle_(x, g);
		Debug(f);Debug(g.transpose());

		theta = 1.0;

		W = Matrix::Zero(DIM, 0);
		M = Matrix::Zero(0, 0);

		auto noConvergence =
				[&](Vector& x, Vector& g)->bool {
					return (((x - g).cwiseMax(lb).cwiseMin(ub) - x).lpNorm<Eigen::Infinity>()>= Options_.tol);
				};

		while (noConvergence(x, g) && (k < Options_.maxIter)) {
			Debug("iteration "<<k)
			double f_old = f;
			Vector x_old = x;
			Vector g_old = g;

			// STEP 2: compute the cauchy point by algorithm CP
			Vector CauchyPoint = Matrix::Zero(DIM, 1), c = Matrix::Zero(DIM, 1);
			GetGeneralizedCauchyPoint(x, g, CauchyPoint, c);
			// STEP 3: compute a search direction d_k by the primal method
			Vector SubspaceMin;
			SubspaceMinimization(CauchyPoint, x, c, g, SubspaceMin);

			Matrix H;
			double Length = 0;

			// STEP 4: perform linesearch and STEP 5: compute gradient
			LineSearch(x, SubspaceMin - x, f, g, Length);

			xHistory.push_back(x);

			// prepare for next iteration
			Vector newY = g - g_old;
			Vector newS = x - x_old;

			// STEP 6:
			double test = newS.dot(newY);
			test = (test < 0) ? -1.0 * test : test;

			if (test > EPS * newY.squaredNorm()) {
				if (k < Options_.m) {
					yHistory.conservativeResize(DIM, k + 1);
					sHistory.conservativeResize(DIM, k + 1);
				} else {

					yHistory.leftCols(Options_.m - 1) = yHistory.rightCols(
							Options_.m - 1).eval();
					sHistory.leftCols(Options_.m - 1) = sHistory.rightCols(
							Options_.m - 1).eval();
				}
				yHistory.rightCols(1) = newY;
				sHistory.rightCols(1) = newS;

				// STEP 7:
				theta = (double) (newY.transpose() * newY)
						/ (newY.transpose() * newS);

				W = Matrix::Zero(yHistory.rows(),
						yHistory.cols() + sHistory.cols());

				W << yHistory, (theta * sHistory);

				Matrix A = sHistory.transpose() * yHistory;
				Matrix L = A.triangularView<Eigen::StrictlyLower>();
				Matrix MM(A.rows() + L.rows(), A.rows() + L.cols());
				Matrix D = -1 * A.diagonal().asDiagonal();
				MM << D, L.transpose(), L, ((sHistory.transpose() * sHistory)
						* theta);

				M = MM.inverse();
			}

			Vector ttt = Matrix::Zero(1, 1);
			ttt(0) = f_old - f;
			Debug( "--> "<< ttt.norm());
			if (ttt.norm() < Options_.tol) {
				// successive function values too similar
				break;
			}
			k++;

		}

		XOpt = x;
		x0 = x;

	}
};

}

#endif /* LBFGSB_H_ */


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <fstream>
#include <iostream>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    no_property, property < edge_weight_t, int > > Graph;
  typedef graph_traits < Graph >::edge_descriptor Edge;
  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  typedef std::pair<int, int> E;

  const int num_nodes = 5;
  E edge_array[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
    E(3, 4), E(4, 0), E(4, 1)
  };
  int weights[] = { 1, 1, 2, 7, 3, 1, 1, 1 };
  int num_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  for (std::size_t j = 0; j < num_edges; ++j) {
    Edge e; bool inserted;
    tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edge_array, edge_array + num_edges, weights, num_nodes);
#endif
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  std::vector < Edge > spanning_tree;

  kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

  std::cout << "Print the edges in the MST:" << std::endl;
  for (std::vector < Edge >::iterator ei = spanning_tree.begin();
       ei != spanning_tree.end(); ++ei) {
    std::cout << source(*ei, g) << " <--> " << target(*ei, g)
      << " with weight of " << weight[*ei]
      << std::endl;
  }

  std::ofstream fout("figs/kruskal-eg.dot");
  fout << "graph A {\n"
    << " rankdir=LR\n"
    << " size=\"3,3\"\n"
    << " ratio=\"filled\"\n"
    << " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
  graph_traits<Graph>::edge_iterator eiter, eiter_end;
  for (tie(eiter, eiter_end) = edges(g); eiter != eiter_end; ++eiter) {
    fout << source(*eiter, g) << " -- " << target(*eiter, g);
    if (std::find(spanning_tree.begin(), spanning_tree.end(), *eiter)
        != spanning_tree.end())
      fout << "[color=\"black\"]\n";
    else
      fout << "[color=\"gray\"]\n";
  }
  fout << "}\n";
  return EXIT_SUCCESS;
}