#include <Eigen/Dense>
#include <iostream>

typedef Eigen::Matrix<float, 3, 3> MyMatrix33f;
typedef Eigen::Matrix<float, 3, 1> MyVector3f;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MyMatrix;

int main() {
  {
    // declaration
    MyMatrix33f a;
    MyVector3f v;
    MyMatrix m(10, 15);

    // initialization
    a = MyMatrix33f::Zero();
    std::cout << "Zero matrix:\n" << a << std::endl;

    a = MyMatrix33f::Identity();
    std::cout << "Identity matrix:\n" << a << std::endl;

    v = MyVector3f::Random();
    std::cout << "Random vector:\n" << v << std::endl;

    a << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    std::cout << "Comma initilized matrix:\n" << a << std::endl;

    a(0, 0) = 3;
    std::cout << "Matrix with changed element[0][0]:\n" << a << std::endl;

    int data[] = {1, 2, 3, 4};
    Eigen::Map<Eigen::RowVectorXi> v_map(data, 4);
    std::cout << "Row vector mapped to array:\n" << v_map << std::endl;

    std::vector<float> vdata = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    Eigen::Map<MyMatrix33f> a_map(vdata.data());
    std::cout << "Matrix mapped to array:\n" << a_map << std::endl;
  }
  // arithmetic
  {
    Eigen::Matrix2d a;
    a << 1, 2, 3, 4;
    Eigen::Matrix2d b;
    b << 1, 2, 3, 4;

    // element wise operations
    Eigen::Matrix2d result = a.array() * b.array();
    std::cout << "element wise a * b :\n" << result << std::endl;

    result = a.array() / b.array();
    std::cout << "element wise a / b :\n" << result << std::endl;

    a = b.array() * 4;
    std::cout << "element wise a = b * 4 :\n" << a << std::endl;

    // matrix operations
    result = a + b;
    std::cout << "matrices a + b :\n" << result << std::endl;

    a += b;
    std::cout << "matrices a += b :\n" << result << std::endl;

    result = a * b;
    std::cout << "matrices a * b :\n" << result << std::endl;
  }

  // patial access
  {
    Eigen::MatrixXf m = Eigen::MatrixXf::Random(4, 4);
    std::cout << "Random 4x4 matrix :\n" << m << std::endl;

    Eigen::Matrix2f b =
        m.block(1, 1, 2, 2);  // coping the middle part of matrix
    std::cout << "Middle of 4x4 matrix :\n" << b << std::endl;

    m.block(1, 1, 2, 2) *= 0;  // change values in original matrix
    std::cout << "Modified middle of 4x4 matrix :\n" << m << std::endl;

    m.row(1).array() += 3;
    std::cout << "Modified row of 4x4 matrix :\n" << m << std::endl;

    m.col(2).array() /= 4;
    std::cout << "Modified col of 4x4 matrix :\n" << m << std::endl;
  }

  // broadcasting
  {
    Eigen::MatrixXf mat = Eigen::MatrixXf::Random(2, 4);
    std::cout << "Random 2x4 matrix :\n" << mat << std::endl;

    Eigen::VectorXf v(2);  // column vector
    v << 100, 100;
    mat.colwise() += v;
    std::cout << "Sum broadcasted over columns :\n" << mat << std::endl;
  }
  return 0;
};


/*
 * Instruction.cpp
 *
 *  Created on: 2015/01/18
 *      Author: kryozahiro
 */

#include "Instruction.h"

#include <cassert>
#include <cmath>
#include <boost/lexical_cast.hpp>
using namespace std;

void Instruction::set(Opcode op, int ret, int mem1, int mem2) {
	this->op = op;
	this->ret = ret;
	this->arg1 = mem1;
	this->arg2 = mem2;
}

void Instruction::operator()(unsigned int& pc, unsigned int end, bool& condition, std::vector<double>& memory) const {
	//条件が満たされていないときに分岐以外の命令に到達したらスキップする
	if (!condition and op != Opcode::IF and op != Opcode::IF_GT and op != Opcode::IF_LE) {
		condition = true;
		return;
	}

	switch (op) {
	case Opcode::AND:
		memory[ret] = static_cast<int>(memory[arg1]) & static_cast<int>(memory[arg2]);
		break;
	case Opcode::OR:
		memory[ret] = static_cast<int>(memory[arg1]) | static_cast<int>(memory[arg2]);
		break;
	case Opcode::NOT:
		memory[ret] = ~static_cast<int>(memory[arg1]);
		break;
	case Opcode::ADD:
		memory[ret] = memory[arg1] + memory[arg2];
		break;
	case Opcode::SUB:
		memory[ret] = memory[arg1] - memory[arg2];
		break;
	case Opcode::MUL:
		memory[ret] = memory[arg1] * memory[arg2];
		break;
	case Opcode::DIV:
		memory[ret] = (memory[arg2] != 0) ? memory[arg1] / memory[arg2] : 0;
		break;
	case Opcode::IF:
		condition &= static_cast<int>(memory[arg1]);
		break;
	case Opcode::IF_GT:
		condition &= (memory[arg1] > memory[arg2]);
		break;
	case Opcode::IF_LE:
		condition &= (memory[arg1] <= memory[arg2]);
		break;
	case Opcode::JMP:
		pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		break;
	case Opcode::JG:
		if (memory[arg1] > 0) {
			pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		}
		break;
	case Opcode::JLE:
		if (memory[arg1] <= 0) {
			pc += min(static_cast<unsigned int>(abs(memory[arg2])), end);
		}
		break;
	case Opcode::SIN:
		memory[ret] = sin(memory[arg1]);
		break;
	case Opcode::COS:
		memory[ret] = cos(memory[arg1]);
		break;
	case Opcode::SQRT:
		memory[ret] = sqrt(abs(memory[arg1]));
		break;
	case Opcode::EXP:
		memory[ret] = exp(memory[arg1]);
		break;
	case Opcode::LOG:
		memory[ret] = (memory[arg1] != 0) ? log(abs(memory[arg1])) : 0;
		break;
	case Opcode::IMM:
		memory[ret] = arg1 * memory.size() + arg2;
		break;
	case Opcode::NOP:
		//do nothing
		break;
	default:
		assert(false);
	}
}

string Instruction::toString() const {
	return "[" + to_string(ret) + "] " + boost::lexical_cast<string>(op) + " [" + to_string(arg1) + "] [" + to_string(arg2) + "] ";
}

Instruction::Opcode Instruction::getOpcode() const {
	return op;
}

int Instruction::getRet() const {
	return ret;
}

int Instruction::getArg1() const {
	return arg1;
}

int Instruction::getArg2() const {
	return arg2;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp

 [begin_description]
 Coefficients for the Adams Moulton method.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

template< class Value , size_t Steps >
class adams_moulton_coefficients ;

template< class Value >
class adams_moulton_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 1 >()
      {
        (*this)[0] = static_cast< Value >( 1 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 2 >()
      {
        (*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
        (*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 3 >()
      {
        (*this)[0] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
        (*this)[1] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
        (*this)[2] = -static_cast< Value >( 1 ) / static_cast< Value >( 12 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 4 >()
      {
        (*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 8 );
        (*this)[1] = static_cast< Value >( 19 ) / static_cast< Value >( 24 );
        (*this)[2] = -static_cast< Value >( 5 ) / static_cast< Value >( 24 );
        (*this)[3] = static_cast< Value >( 1 ) / static_cast< Value >( 24 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 5 >()
      {
        (*this)[0] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
        (*this)[1] = static_cast< Value >( 323 ) / static_cast< Value >( 360 );
        (*this)[2] = -static_cast< Value >( 11 ) / static_cast< Value >( 30 );
        (*this)[3] = static_cast< Value >( 53 ) / static_cast< Value >( 360 );
        (*this)[4] = -static_cast< Value >( 19 ) / static_cast< Value >( 720 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 6 >()
      {
        (*this)[0] = static_cast< Value >( 95 ) / static_cast< Value >( 288 );
        (*this)[1] = static_cast< Value >( 1427 ) / static_cast< Value >( 1440 );
        (*this)[2] = -static_cast< Value >( 133 ) / static_cast< Value >( 240 );
        (*this)[3] = static_cast< Value >( 241 ) / static_cast< Value >( 720 );
        (*this)[4] = -static_cast< Value >( 173 ) / static_cast< Value >( 1440 );
        (*this)[5] = static_cast< Value >( 3 ) / static_cast< Value >( 160 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 7 >()
      {
        (*this)[0] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
        (*this)[1] = static_cast< Value >( 2713 ) / static_cast< Value >( 2520 );
        (*this)[2] = -static_cast< Value >( 15487 ) / static_cast< Value >( 20160 );
        (*this)[3] = static_cast< Value >( 586 ) / static_cast< Value >( 945 );
        (*this)[4] = -static_cast< Value >( 6737 ) / static_cast< Value >( 20160 );
        (*this)[5] = static_cast< Value >( 263 ) / static_cast< Value >( 2520 );
        (*this)[6] = -static_cast< Value >( 863 ) / static_cast< Value >( 60480 );
      }
};

template< class Value >
class adams_moulton_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
    adams_moulton_coefficients( void )
    : boost::array< Value , 8 >()
      {
        (*this)[0] = static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
        (*this)[1] = static_cast< Value >( 139849 ) / static_cast< Value >( 120960 );
        (*this)[2] = -static_cast< Value >( 4511 ) / static_cast< Value >( 4480 );
        (*this)[3] = static_cast< Value >( 123133 ) / static_cast< Value >( 120960 );
        (*this)[4] = -static_cast< Value >( 88547 ) / static_cast< Value >( 120960 );
        (*this)[5] = static_cast< Value >( 1537 ) / static_cast< Value >( 4480 );
        (*this)[6] = -static_cast< Value >( 11351 ) / static_cast< Value >( 120960 );
        (*this)[7] = static_cast< Value >( 275 ) / static_cast< Value >( 24192 );
      }
};

} // detail
} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED


#include "CirclePatterns.h"
#include <Eigen/SparseQR>

CirclePatterns::CirclePatterns(shared_ptr<ManifoldSurfaceMesh> mesh0, Vertex infVertex,
    EdgeData<bool> eMask, EdgeData<bool> eBdry,FaceData<bool> fMask, int optScheme0,
    vector<double>& solve, Eigen::VectorXd thetas):
mesh(mesh0),
infVertex(infVertex),
eMask(eMask),
eBdry(eBdry),
fMask(fMask),
angles(mesh->nHalfedges()),
thetas(thetas),
radii(mesh->nFaces()),
eIntIndices(mesh->nEdges()),
imaginaryHe(0),
OptScheme(optScheme0)
{
    // I added a plus 1 here and at radii; should figure why I need to
    solver.n = mesh->nFaces();
    sol = solve;
    uv = VertexData<Eigen::Vector2d> (*mesh);
    eInd = mesh->getEdgeIndices();
    vInd = mesh->getVertexIndices();
    fInd = mesh->getFaceIndices();
}

inline double Cl2(double x) {
    if (x == 0.0) return 0.0;
    x = std::remainder(x, 2*M_PI);
    if (x == 0.0) return 0.0;

    if (fabs(x) <= 2.0944) {
        double xx = x * x;
        return ((((((((((((2.3257441143020875e-22 * xx
                           + 1.0887357368300848e-20) * xx
                           + 5.178258806090624e-19) * xx
                           + 2.5105444608999545e-17) * xx
                           + 1.2462059912950672e-15) * xx
                           + 6.372636443183181e-14) * xx
                           + 3.387301370953521e-12) * xx
                           + 1.8978869988971e-10) * xx
                           + 1.1482216343327455e-8) * xx
                           + 7.873519778281683e-7) * xx
                           + 0.00006944444444444444) * xx
                           + 0.013888888888888888) * xx
                           - log(fabs(x)) + 1.0) * x;
    }

    x += ((x > 0.0) ? - M_PI : M_PI);
    double xx = x * x;
    return ((((((((((((3.901950904063069e-15 * xx
                       + 4.566487567193635e-14) * xx
                       + 5.429792727596476e-13) * xx
                       + 6.5812165661369675e-12) * xx
                       + 8.167010963952222e-11) * xx
                       + 1.0440290284867003e-9) * xx
                       + 1.3870999114054669e-8) * xx
                       + 1.941538399871733e-7) * xx
                       + 2.927965167548501e-6) * xx
                       + 0.0000496031746031746) * xx
                       + 0.0010416666666666667) * xx
                       + 0.041666666666666664) * xx
                       + log(0.5)) * x;
}

double ImLi2Sum(double dp, double theta) {
    double tStar = M_PI - theta;
    double x = 2*atan(tanh(0.5*dp) * tan(0.5*tStar));

    return x*dp + Cl2(x + tStar) + Cl2(-x + tStar) - Cl2(2.0*tStar);
}

double fe(double dp, double theta) {
    return atan2(sin(theta), exp(dp) - cos(theta));
}

void CirclePatterns::computeEnergy(double& energy, const Eigen::VectorXd& rho)
{
    energy = 0.0;

    // sum over edges
    for (Edge e : mesh->edges()) {
        if (eMask[e]) {
            int fk = e.halfedge().face().getIndex();

            if (eBdry[e]) {
                energy -= 2 * (M_PI - thetas[e.getIndex()]) * rho[fk];

            } else {
                int fl = e.halfedge().twin().face().getIndex();
                energy += ImLi2Sum(rho[fk] - rho[fl], thetas[e.getIndex()]) -
                          (M_PI - thetas[e.getIndex()]) * (rho[fk] + rho[fl]);
            }
        }
    }

    // sum over faces
    for (Face f: mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) energy += 2*M_PI*rho[fInd[f]];
    }
}

void CirclePatterns::computeGradient(Eigen::VectorXd& gradient, const Eigen::VectorXd& rho)
{
    // loop over faces
    for (Face f : mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) {
            int fk = fInd[f];
            gradient[fk] = 2*M_PI;

            // sum of adjacent edges
            Halfedge he = f.halfedge();
            do {
                Edge e = he.edge();
                if (eBdry[e]) {
                    gradient[fk] -= 2*(M_PI - thetas[eInd[e]]);

                } else {
                    Halfedge h = e.halfedge();
                    int fl = fk == (int)fInd[h.face()] ? fInd[h.twin().face()] : fInd[h.face()];
                    gradient[fk] -= 2*fe(rho[fk] - rho[fl], thetas[e.getIndex()]);
                }

                he = he.next();
            } while (he != f.halfedge());
        }
    }
}

void CirclePatterns::computeHessian(Eigen::SparseMatrix<double>& hessian, const Eigen::VectorXd& rho)
{
    std::vector<Eigen::Triplet<double>> HTriplets;

    for (Edge e : mesh->edges()) {
        if (!eBdry[e] && eMask[e]) {
            int fk = fInd[e.halfedge().face()];
            int fl = fInd[e.halfedge().twin().face()];

            double hessval = sin(thetas[eInd[e]]) / (cosh(rho(fk) - rho(fl)) - cos(thetas[eInd[e]]));
            HTriplets.push_back(Eigen::Triplet<double>(fk, fk, hessval + 1e-8));
            HTriplets.push_back(Eigen::Triplet<double>(fl, fl, hessval + 1e-8));
            HTriplets.push_back(Eigen::Triplet<double>(fk, fl, -hessval));
            HTriplets.push_back(Eigen::Triplet<double>(fl, fk, -hessval));
        }
    }

    hessian.setFromTriplets(HTriplets.begin(), HTriplets.end());
}

void CirclePatterns::setRadii()
{
    for (Face f : mesh->faces()) {
        if (!f.isBoundaryLoop() && fMask[f]) radii[fInd[f]] = exp(solver.x[fInd[f]]);
    }
}

bool CirclePatterns::computeRadii()
{
    MeshHandle handle;
    handle.computeEnergy = std::bind(&CirclePatterns::computeEnergy, this, _1, _2);
    handle.computeGradient = std::bind(&CirclePatterns::computeGradient, this, _1, _2);
    handle.computeHessian = std::bind(&CirclePatterns::computeHessian, this, _1, _2);

    solver.handle = &handle;
    if (OptScheme == GRAD_DESCENT) solver.gradientDescent();
    else if (OptScheme == NEWTON) solver.newton();
    else if (OptScheme == TRUST_REGION) solver.trustRegion();
    else solver.lbfgs();

    // set radii
    setRadii();

    return true;
}

void CirclePatterns::computeAnglesAndEdgeLengths(Eigen::VectorXd& lengths)
{
    for (Edge e : mesh->edges()) {
        if(eMask[e]) {
            Halfedge h1 = e.halfedge();

            if (eBdry[e]) {
                angles[h1.getIndex()] = M_PI - thetas[eInd[e]];

            } else {
                Halfedge h2 = h1.twin();
                double dp = log(radii[h1.face().getIndex()]) - log(radii[h2.face().getIndex()]);
                angles[h1.getIndex()] = fe(dp, thetas[e.getIndex()]);
                angles[h2.getIndex()] = fe(-dp, thetas[e.getIndex()]);
            }

            lengths[eInd[e]] = 2.0*radii[h1.face().getIndex()]*sin(angles[h1.getIndex()]);
        }
    }
}

void CirclePatterns::performFaceLayout(Halfedge he, const Eigen::Vector2d& dir,
                                       Eigen::VectorXd& lengths, std::unordered_map<int, bool>& visited,
                                       std::stack<Edge>& stack)
{
    if (he.isInterior() && fMask[he.face()]) {
        int fIdx = he.face().getIndex();
        if (visited.find(fIdx) == visited.end()) {
            Halfedge next = he.next();
            Halfedge prev = he.next().next();

            // compute new uv position
            double angle = angles[next.getIndex()];
            Eigen::Vector2d newDir = {cos(angle)*dir[0] - sin(angle)*dir[1],
                                      sin(angle)*dir[0] + cos(angle)*dir[1]};

            uv[prev.vertex()] = uv[he.vertex()] + newDir*lengths[eInd[prev.edge()]];

            // mark face as visited
            visited[fIdx] = true;

            // push edges onto stack
            if(eMask[next.edge()]) stack.push(next.edge());
            if(eMask[prev.edge()]) stack.push(prev.edge());
        }
    }
}

void CirclePatterns::setUVs()
{
    // compute edge lengths
    Eigen::VectorXd lengths(mesh->nEdges());
    computeAnglesAndEdgeLengths(lengths);

    // push any edge
    std::stack<Edge> stack;
    Edge e0;
    for (Edge e :mesh->edges()) {
        if(eMask[e]) {
            e0 = e;
            break;
        }
    };
    stack.push(e0);

    uv[e0.halfedge().vertex()] = Eigen::Vector2d::Zero();
    uv[e0.halfedge().next().vertex()] = Eigen::Vector2d(lengths[eInd[e0]], 0);

    // perform layout
    std::unordered_map<int, bool> visited;
    while (!stack.empty()) {
        Edge e = stack.top();
        stack.pop();

        Halfedge h1 = e.halfedge();
        Halfedge h2 = h1.twin();

        // compute edge vector

        Eigen::Vector2d dir = uv[h2.vertex()] - uv[h1.vertex()];

        dir.normalize();
        // boundary edges
        performFaceLayout(h1, dir, lengths, visited, stack);
        performFaceLayout(h2, -dir, lengths, visited, stack);
    }

    normalize();
}

VertexData<Eigen::Vector2d> CirclePatterns::parameterize() {
    // set interior edge indices
    int eIdx = 0;
    for (Edge e : mesh->edges()) {
        if (eMask[e]) {
            if (!eBdry[e])
                eIntIndices[eInd[e]] = eIdx++;
            else {
                eIntIndices[eInd[e]] = -1;
                imaginaryHe++;
            }
        }
    }

    // compute radii
    if (!computeRadii()) {
        std::cout << "Unable to compute radii" << std::endl;
        return VertexData<Eigen::Vector2d>();
    }

    // set uvs
    setUVs();
    return uv;
}
double CirclePatterns::uvArea(Face f) {
    if (f.isBoundaryLoop() || !fMask[f]) {
        return 0;
    }

    const Eigen::Vector2d& a(uv[f.halfedge().vertex()]);
    const Eigen::Vector2d& b(uv[f.halfedge().next().vertex()]);
    const Eigen::Vector2d& c(uv[f.halfedge().next().next().vertex()]);

    const Eigen::Vector2d u = b - a;
    const Eigen::Vector2d v = c - a;

    return 0.5 * (u.x()*v.y() - v.x()*u.y());
}

Eigen::Vector2d CirclePatterns::uvBarycenter(Face f) {
    if (f.isBoundaryLoop() || !fMask[f]) {
        return Eigen::Vector2d::Zero();
    }

    const Eigen::Vector2d& a(uv[f.halfedge().vertex()]);
    const Eigen::Vector2d& b(uv[f.halfedge().next().vertex()]);
    const Eigen::Vector2d& c(uv[f.halfedge().next().next().vertex()]);

    return (a + b + c) / 3.0;
}
void CirclePatterns::normalize() {
    // compute center
    double totalArea = 0;
    Eigen::Vector2d center = Eigen::Vector2d::Zero();
    uv[infVertex.getIndex()] = Eigen::Vector2d::Zero();
    /*
    uv[infVertex.getIndex()].x() = -8;
    uv[infVertex.getIndex()].y() = 5;
    */
    for (Face f : mesh->faces()) {
        if (fMask[f]){
            double area = uvArea(f);
            center += area * uvBarycenter(f);
            totalArea += area;
        }
    }
    center /= totalArea;

    // shift
    double r = 0.0;
    for (Vertex v : mesh->vertices()) {
        if (v != infVertex) {
            uv[v] -= center;
            r = std::max(r, uv[v].squaredNorm());
        }
    }

    // scale
    r = sqrt(r);
    for (Vertex v : mesh->vertices()) {
        uv[v] /= r;
    }
}

inline double shift(double c) {
    return (c + 2.) * 500;
}


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <Eigen/LU>

#include "tsef.h"
#include "../parameters.h"

CHARGEFW2_METHOD(TSEF)

double K(int i);

double K(int i) {
    double vals[] = {0.556, 0.778, 1.000, 1.053, 1.087, 1.091};
    if (i > 6)
        return vals[5];
    else
        return vals[i - 1];
}

std::vector<double> TSEF::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(n + 1, n + 1);
    Eigen::VectorXd b = Eigen::VectorXd::Zero(n + 1);

    const double alpha = 14.4;

    for (size_t i = 0; i < n; i++) {
        const auto &atom_i = molecule.atoms()[i];
        A(i, i) = parameters_->atom()->parameter(atom::hardness)(atom_i);
        b(i) = - parameters_->atom()->parameter(atom::electronegativity)(atom_i);
        for (size_t j = i + 1; j < n; j++) {
            const auto &atom_j = molecule.atoms()[j];
            int bd = molecule.bond_distance(atom_i, atom_j);
            auto x = alpha * K(bd) / (0.84 * bd + 0.46);
            A(i, j) = x;
            A(j, i) = x;
        }
    }

    A.row(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A.col(n) = Eigen::VectorXd::Constant(n + 1, 1);
    A(n, n) = 0;
    b(n) = molecule.total_charge();

    Eigen::VectorXd q = A.partialPivLu().solve(b).head(n);
    return std::vector<double>(q.data(), q.data() + q.size());
}


#ifdef USE_EIGEN
#define EIGEN_USE_MKL_ALL
#endif
// do this before include Eigen.

#include "optim.hpp"

#include "alap.h"
#include "matrix.h" // ""

#include "timer.h"

#ifdef USE_EIGEN
#include <Eigen/Core>
#endif

#include <iostream>

#ifdef USE_LBFGSPP
#include <LBFGS.h>
#endif

#include <iostream>

#include "helper.h"
#include "mesh.h"

static void show_usage()
{
    std::cerr << "Usage: "
              << "Options:\n"
              << "\t-h,--help\t\tShow this help message\n"
              << "\t-d,--destination DESTINATION\tSpecify the destination path"
              << std::endl;
}

/*
Differentiable Projection of Weights
*/

arma::mat W2F_n(const arma::mat& W)
{
    using namespace arma;

    arma::mat SW = sum(W, 1); // dim=1 so it sums all cols for each row.

    arma::mat FW = W.each_col() / SW;

    return FW;
}

const double DEFAULT_W_EPSILON = 1e-2;

arma::mat W2F_m1(const arma::mat& W, const double epsilon=DEFAULT_W_EPSILON)
{
    // In matlab grammar: W2F_m = @(W) (W>epsilon) .* W + (W<=epsilon) .* (- W.^3 / epsilon^2  + 2 * W.^2 / epsilon );

    using namespace arma;

    arma::mat FW = (W > epsilon) % W + (W <= epsilon) % (- W % W % W / (epsilon*epsilon) + 2 * W % W / epsilon);

    return FW;
}

arma::mat dFW2dW_m1(const arma::mat& D, const arma::mat& W, const double epsilon=DEFAULT_W_EPSILON)
{
    // In matlab grammar: dFW2dW_m = @(D,W) ( (W>epsilon) + (W<=epsilon) .* (- 3 * W.^2 / epsilon^2  + 4 * W / epsilon ) ) .* D;

    using namespace arma;

    arma::mat FD = ( (W > epsilon) + (W <= epsilon) % (-3 * W % W / (epsilon*epsilon) + 4 * W / epsilon) ) % D;

    return FD;
}

const double DEFAULT_W_EPSILON1 = 0e-2;
const double DEFAULT_W_EPSILON2 = 1e-4;

arma::mat W2F_m2(const arma::mat& W, const double e1 = DEFAULT_W_EPSILON1, const double e2 = DEFAULT_W_EPSILON2)
{

    using namespace arma;

    double c1 = (e1 + e2) / ((e2 - e1) * (e2 - e1) * (e2 - e1));
    double c2 = (e1 + 2 * e2) / ((e2 - e1) * (e2 - e1));

    arma::mat FW = (W <= -1000) % W -(W > e1) % (W < e2) % pow(W - e1, 3) * c1 + (W > e1) % (W < e2) % pow(W - e1, 2) * c2 + (W >= e2) % W;

    return FW;
}

arma::mat dFW2dW_m2(const arma::mat& D, const arma::mat& W, const double e1 = DEFAULT_W_EPSILON1, const double e2 = DEFAULT_W_EPSILON2)
{

    using namespace arma;

    double c1 = (e1 + e2) / ((e2 - e1) * (e2 - e1) * (e2 - e1));
    double c2 = (e1 + 2 * e2) / ((e2 - e1) * (e2 - e1));

    arma::mat FD = ((W <= -1000) - (W > e1) % (W < e2) % pow(W - e1, 2) * 3 * c1 + (W > e1) % (W < e2) % pow(W - e1, 2) * c2 + (W >= e2) ) % D;

    return FD;
}

arma::mat dFW2dW_n(const arma::mat& D, const arma::mat& W)
{
    // In matlab grammar: dFW2dW_n = @(D,W) bsxfun(@rdivide, D, sum(W,2)) - bsxfun(@times, bsxfun(@rdivide, W, sum(W,2).^2), sum(D,2));

    using namespace arma;

    arma::mat FD;

    arma::mat SW = sum(W, 1);
    arma::mat SD = sum(D, 1);

    arma::mat MM = W.each_col() / (SW % SW);

    FD = D.each_col() / SW - MM.each_col() % SD; // % is the element-wise multiplication, NOT '*'!!!

    return FD;
}

Dense W2F_n(const Dense& W)
{
    // const arma::mat W_arma(W.head(), W.nrow(), W.ncol());
    const arma::mat W_arma = dense_array_to_arma_mat(W);

    arma::mat FW_arma = W2F_n(W_arma);

    Dense FW = arma_mat_to_dense_array(FW_arma);

    return FW;
}

Dense dFW2dW_n(const Dense& D, const Dense& W)
{

    const arma::mat W_arma = dense_array_to_arma_mat(W);
    const arma::mat D_arma = dense_array_to_arma_mat(D);

    arma::mat FD = dFW2dW_n(D_arma, W_arma);

    return arma_mat_to_dense_array(FD);
}

Dense W2F_m(const Dense& W)
{
    const arma::mat W_arma = dense_array_to_arma_mat(W);

    arma::mat FW_arma = W2F_m2(W_arma);

    Dense FW = arma_mat_to_dense_array(FW_arma);

    return FW;
}

Dense dFW2dW_m(const Dense& D, const Dense& W)
{

    const arma::mat W_arma = dense_array_to_arma_mat(W);
    const arma::mat D_arma = dense_array_to_arma_mat(D);

    arma::mat FD = dFW2dW_m2(D_arma, W_arma);

    return arma_mat_to_dense_array(FD);
}

int main(int argc, char** argv)
{

    const std::string SNAPSHOT_NONE = std::string("none");

    std::vector <std::string> sources;
    std::string EXAMPLE = std::string("/qhw/qhw/data/tibiman-H");
    std::string SOLVER = std::string("adamd");
    std::string SNAPSHOT = SNAPSHOT_NONE;
    std::string OUTPUT = std::string("");
    int NUM_ITER = 50;
    double STEP_SIZE = 0.1;
    bool PROJECT_SIMPLEX = false;
    bool TIMING = false;
    int REPEAT = 5;
    bool LOG_HISTORY = false;
    int LBFGS_M = 10;
    double COND_BOUND = 0.2;
    double DELTA = 0;
    int verbose = 3;

    for (int i = 1; i < argc; i++) {
        std::string arg = argv[i];
        if ((arg == "-h") || (arg == "--help")) {
            show_usage();
            return 0;
        }
        else if ((arg == "--project")) {
            PROJECT_SIMPLEX = true;
        }
        else if ((arg == "--timing")) {
            TIMING = true;
        }
        else if ((arg == "-e") || (arg == "--example")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                EXAMPLE = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--example option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--snapshot")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                SNAPSHOT = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--snapshot option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--output")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                OUTPUT = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--output option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--solver")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                SOLVER = std::string(argv[++i]); // Increment 'i' so we don't get the argument as the next argv[i].
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--solver option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--lbfgs_m")) {
            if (i + 1 < argc) { // Make sure we aren't at the end of argv!
                LBFGS_M = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--lbfgs_m option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "-n") || (arg == "--num_iter")) {
            if (i + 1 < argc) {
                NUM_ITER = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--num_iter option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--repeat")) {
            if (i + 1 < argc) {
                REPEAT = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--repeat option requires one argument." << std::endl;
                return 1;
            }
        }
        else if (arg == "--verbose") {
            if (i + 1 < argc) {
                verbose = atoi(argv[++i]);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--verbose option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--log_history")) {
            LOG_HISTORY = true;
        }
        else if ( (arg == "--step_size")) {
            if (i + 1 < argc) {
                STEP_SIZE = atof(argv[++i]);
                printf("step_size=%f\n", STEP_SIZE);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--step_size option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--cond_bound")) {
            if (i + 1 < argc) {
                COND_BOUND = atof(argv[++i]);
                printf("cond_bound=%f\n", COND_BOUND);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--cond_bound option requires one argument." << std::endl;
                return 1;
            }
        }
        else if ((arg == "--delta")) {
            if (i + 1 < argc) {
                DELTA = atof(argv[++i]);
                printf("delta=%f\n", DELTA);
            }
            else { // Uh-oh, there was no argument to the destination option.
                std::cerr << "--delta option requires one argument." << std::endl;
                return 1;
            }
        }
        else {
            sources.push_back(argv[i]);
        }
    }

    double t = 0;

    std::string folder;
    int dim = 2;

    folder = EXAMPLE + std::string("/");

    printf("Loading files from %s.\n", folder.c_str());

    if (OUTPUT.length() > 0) {
        OUTPUT = OUTPUT;
    }

    int length_his = NUM_ITER * (REPEAT + 1) * 2 + 1; // This *2 should not be need, just for redundancy.
    double* energy_his = new double[length_his];
    double* time_his = new double[length_his];

    cholmod_common* cm = Begin();

    Dense V;
    DenseInt F;

    Dense FA;

//#ifdef USE_LARGER_G
    Sparse Gk, Gu, G;
//#endif

    Sparse Gx, Gy, Gz;

    Sparse Gxk, Gxu, Gyk, Gyu;
    Sparse Gzk, Gzu; // for 3D

    DenseInt known;
    DenseInt unknown;
    int nk, nu;

    Sparse L; // Note L is not p.d., just p.s.d.
    Sparse Mass;
    Sparse invMass;

    Dense mass_vertex;
    mass_vertex.read(folder + "mv.mtx");

    int f, mcdim;

    Dense BC(0, 1);
    BC.read(folder + "BC.mtx");
    printf("BC size (%d,%d)\n", BC.nrow(), BC.ncol());

    int nr;
    known.read(folder + "B.mtx", true); // setting 'true' will delete 1 from the matrix for 0-index.

    nk = known.nrow();
    assert(known.ncol() == 1);

    {

        V.read(folder + "V.mtx");
        F.read(folder + "F.mtx", true); // from 1-index matrix.

        printf("Files loaded successfully!\n");

        f = F.nrow();
        dim = F.ncol() - 1;

        complementary_list(known, V.nrow(), unknown);

        FA = (dim == 3) ? volume(V, F) : area(V, F);

        sparse_grads(V, F,
            known,
            Gx, Gy, Gz,
            Gxk, Gxu, Gyk, Gyu,
            Gzk, Gzu);

        Mass = Sparse::Diag(mass_vertex);
        invMass = Sparse::Diag(Dense::div(1.0, mass_vertex));

    }

    mcdim = (dim == 2) ? 3 : 6; // so au is of dim (f*mcdim)

    Sparse GxkT = Gxk.transposed();
    Sparse GxuT = Gxu.transposed();
    Sparse GykT = Gyk.transposed();
    Sparse GyuT = Gyu.transposed();
    Sparse GzkT;
    Sparse GzuT;
    if (dim == 3) {
        GzkT = Gzk.transposed();
        GzuT = Gzu.transposed();
    }

    Sparse GxuT_A00 = GxuT;
    Sparse GxuT_A01 = GxuT;

    Sparse GyuT_A01 = GyuT;
    Sparse GyuT_A11 = GyuT;

    // Used for 3D only:
    Sparse GxuT_A02 = GxuT;
    Sparse GzuT_A02 = GzuT;
    Sparse GyuT_A12 = GyuT;
    Sparse GzuT_A12 = GzuT;
    Sparse GzuT_A22 = GzuT;

    Sparse Mf = Sparse::Diag(FA);

    Dense Zf = Dense::Zeros(f, 1);

    Dense RFA = Dense::concatenate(FA,
        Dense::concatenate(Zf, FA));

    int stype = 0; // 0; // 0; unsymmetric // 1: symmetric and use triu

    stype = 1; // 1: symmetric and use triu
    Sparse A = GxuT_A00.mul(Gxu, stype)
        + GyuT_A01.mul(Gxu, stype)
        + GxuT_A01.mul(Gyu, stype)
        + GyuT_A11.mul(Gyu, stype);
    if (dim == 3) {
        A = A
            + GzuT_A22.mul(Gzu, stype)
            + GxuT_A02.mul(Gzu, stype)
            + GzuT_A02.mul(Gxu, stype)
            + GyuT_A12.mul(Gzu, stype)
            + GzuT_A12.mul(Gyu, stype);
    }

    L = (Mf * Gx).transposed().mul(Gx, stype)
        + (Mf * Gy).transposed().mul(Gy, stype);
    if (dim == 3) {
        L = L + (Mf * Gz).transposed().mul(Gz, stype);
    }

    int* I;
    int* J;
    symmetric_tensor_assemble_indices(I, J, f, dim);

    // Setup the objective function:
    Sparse Q;
    stype = 1; // 1: symmetric and use triu
    (L * invMass).mul(L, Q, stype);
    // So Q is a symmetric matrix such that
    // Q==L*M^-1*L.

    Sparse diagm = Sparse::Diag(
        Dense::concatenate(FA, dim==2? FA : Dense::concatenate(FA, FA))
    );

    Sparse Quu, Quk;
    Sparse Qku, Qkk;

    Sparse Lua = (GxuT * Mf * Gx + GyuT * Mf * Gy);
    Sparse Lka = (GxkT * Mf * Gx + GykT * Mf * Gy);
    if (dim == 3) {
        Lua = Lua + GzuT * Mf * Gz;
        Lka = Lka + GzkT * Mf * Gz;
    }
    Sparse Lau = Lua.transposed();
    Sparse Lak = Lka.transposed();
    (Lua* invMass).mul(Lau, Quu, 0);
    (Lka* invMass).mul(Lak, Qkk, 0);
    (Lua* invMass).mul(Lak, Quk, 0);

    Qku = Quk.transposed();

    Sparse SA = GxuT * Mf * Gxk + GyuT * Mf * Gyk;
    if (dim == 3)
        SA = SA + GzuT * Mf * Gzk;
    Dense B = (SA * BC) * (-1);

    Dense X(B.nrow(), B.ncol());

    if (verbose>2)
    {
        t = GetTime();
        for (int i = 0; i < 1; i++)
        {
            X = A.mul(B);
        }
        printf("Time for linear matrix-vector mul :%f.\n", GetTime() - t);

        Dense X3(B.nrow(), B.ncol());
        t = GetTime();
        for (int i = 0; i < 1000; i++)
        {
            Dense::saxy(X, X3, -0.4);
        }
        printf("Time for saxy x1000:%f.\n", GetTime() - t);

        t = GetTime();
        configure_solve(A.cm);
        A.solve(B, X); // X = A \ B;
        printf("Time for one linear solve :%f.\n", GetTime() - t);
    }

    Dense Y = A.mul(X);

    t = GetTime();
    A.symbolic_factor();
    printf("symbolic_factor: %f seconds.\n", GetTime() - t);

    t = GetTime();
    A.numerical_factor();
    printf("numerical_factor: %f seconds.\n", GetTime() - t);

    Dense W_u;
    Dense GW;
    Dense Res_u;
    Dense BR;
    Dense PS;

    Dense st0 = Dense::Ones(f, 1);
    // Dense st0 = FA;

    Dense at0 = Dense::concatenate(st0,
        Dense::concatenate(st0 * 1e-6, st0));

    if (dim == 3)
    {
        at0 = Dense::concatenate(at0,
            Dense::concatenate(Dense::concatenate(st0,st0) * 1e-6, st0));
    }

    if (SNAPSHOT != SNAPSHOT_NONE)
    {
        Dense at0_snapshot;
        at0_snapshot.read(SNAPSHOT);
        if (at0.nrow() == at0_snapshot.nrow() && at0.ncol() == at0_snapshot.ncol() ) {
            printf("Snapshot file %s is loaded.\n", SNAPSHOT.c_str());
            at0 = at0_snapshot;
        }
        else {
            printf("Snapshot file is of wrong size! File ignored. \n");
        }
    }

    Dense au = at0;

    Sparse diagAU = diagm;

    Dense W(L.nrow(), BC.ncol());
    W.slice_assign_value(known, BC, 0); // W(known, :) = BC;

    Dense FW(L.nrow(), BC.ncol());
    FW.slice_assign_value(known, BC, 0);

    Dense FW_u, FW_k;

    Dense gau = Dense::Zeros(f * mcdim, 1);
    Dense gau_j = Dense::Zeros(f * mcdim, 1);

    t = GetTime();

    std::function<Dense(const Dense&)> W2F_pn = [&](const Dense& W)
    {
        return W2F_n(W2F_m(W));
        //return W2F_n(W);
    };

    std::function<Dense(const Dense&, const Dense&)> dFW2dW_pn = [&](const Dense& D, const Dense& W)
    {
        return dFW2dW_n(dFW2dW_m(D, W), W2F_m(W));
        //return dFW2dW_n(D, W);
    };

    std::function<Dense(const Dense&)> W2F;
    std::function<Dense(const Dense&, const Dense&)> dFW2dW;

    if (PROJECT_SIMPLEX)
    {
        W2F = W2F_pn;
        dFW2dW = dFW2dW_pn;
    }
    else
    {
        W2F = [&](const Dense& W)
        {
            return W;
        };
        dFW2dW = [&](const Dense& D, const Dense& W)
        {
            return D;
        };
    }

    Grad grad = compute_grads_pre(V, F);

    // GRADtf64 grad_tf = GRADtf64(V, F, W.ncol());
    GRADtf grad_tf = GRADtf(V, F, W.ncol());

    int i = 0;
    double start_time = GetTime();
    // Dense gat = Dense::Zeros(mcdim * f, 1);
    // Dense at = Dense::concatenate(Dense::Ones(f, 1),
    //     Dense::concatenate(Dense::Ones(f, 1) * 1e-6, Dense::Ones(f, 1))); // do not use Dense::Zeros(f, 1)

    const Para para;

    auto fun = [&](const arma::vec& at, arma::vec& gat)
    {

        t = GetTime();

        if (TIMING)
        {
            printf("Entered loop for timing.\n");
        }

        /* update au from at */

        // arma::vec at_tmp = at;
        // s_at2au_fast(at_tmp, au, FA, dim, para);
        s_at2au(at, au, FA, dim, para);

        if (TIMING)
        {
            printf("Check point 1: %f.\t Apply Parameterization.\n", GetTime() - t); t = GetTime();
        }

        int stype = 1; // 1: symmetric and use triu

        Sparse A2 = Sparse::assemble_lap(GxuT, GyuT, GzuT, au, dim);
        Sparse::assign_value_same_pattern(A2, A);
        // A.symbolic_factor(); // no need since sparsity pattern remain unchanged.

        B = Sparse::assemble_lap_off_diag(GxuT, GyuT, GzuT, GxkT, GykT, GzkT, au, dim) * (BC * (-1));

        if (TIMING)
        {
            printf("Check point 2: %f.\t Assemble Lap.\n", GetTime() - t); t = GetTime();
        }

        configure_solve(A.cm);

        A.numerical_factor();

        if (TIMING)
        {
            printf("Check point 3: %f.\t Numerical factor.\n", GetTime() - t); t = GetTime();
        }

        W_u = A.solve_with_factor(B); // W_u = A\B; but more efficiently.

        if (TIMING)
        {
            printf("Check point 4: %f.\t Back Sub 1.\n", GetTime() - t); t = GetTime();
        }

        // project the weights to the probability simplex.
        FW_u = W2F(W_u);
        FW_k = BC;

        W.slice_assign_value(unknown, W_u, 0); // W(unknown, :) = W_u;

        double e = 1 / (i + 1);
        // many optimizers do not rely on the energy value e, only its gradient.
        // in this case e can be an arbitary value to save time.

        bool last_iter = i == (NUM_ITER * (REPEAT + 1)); // it's not (i+1) here.

        if (verbose > 2 || last_iter)
        {

            Dense H_u = Quu.mul(FW_u) + Quk.mul(FW_k);
            Dense H_k = Qku.mul(FW_u) + Qkk.mul(FW_k);
            e = ((Dense::times(FW_u, H_u).reduce_sum(0) + Dense::times(FW_k, H_k).reduce_sum(0)).reduce_sum(1))(0, 0);

            printf("\nIter %04d: energy=%f \t", i, e);

            if (verbose > 3)
            {
                printf("W_u.min()=");
                W_u.reduce_min().print();
                printf("FW_u.min()=");
                FW_u.reduce_min().print();
            }

            if (i < length_his)
            {
                energy_his[i] = e;
                time_his[i] = GetTime() - start_time;
            }
            if (LOG_HISTORY)
            {
                FW.slice_assign_value(unknown, FW_u, 0);
                char fname[64];
                sprintf(fname, "W%04d.mtx", i);
                FW.write(OUTPUT + fname); // projected weights
                sprintf(fname, "UW%04d.mtx", i);
                W.write(OUTPUT + fname); // unprojected weights
            }
        }
        else
        {
            printf("Iter %04d...\t", i);
        }

        if (verbose>4)
        {
            Dense NW = W2F_n(W);
            double pou = (W - NW).norm() / W.norm();
            printf("Checking partition of unity: %f, expecting ~0.\n", pou);
        }

        if (TIMING)
        {
            printf("Check point 5: %f.\n", GetTime() - t); t = GetTime();
        }

        // Calculate gradients.

        switch (4) { // 4 is fastest

        case 0:

            GW = G * W; // old code

            break;

        case 1:

            // [Gxu, Gxk]
            // [Gyu, Gyk] [W_u] =
            // [Gzu, Gzk] [W_k]
            GW = Dense::concatenate(Gxu * W_u + Gxk * BC, Gyu * W_u + Gyk * BC);
            if (dim == 3)
                GW = Dense::concatenate(GW, Gzu * W_u + Gzk * BC);

            break;

        case 2:

            compute_grads(V, F, grad, W, GW); // somehow slower than G * W...

            break;

        case 3:

            tf_compute_grads(V, F, W, GW);

            break;

        case 4:

            grad_tf.run(W, GW);

            break;
        default:
            ;
        }

        if (TIMING)
        {
            printf("Check point 6: %f. \t Gradient computation. \n", GetTime() - t); t = GetTime();
        }

        Res_u = (Quu.mul(FW_u) + Quk.mul(FW_k))* 2.0;

        Dense dW = dFW2dW(Res_u, W_u); // since dFW2dW is a row-wise operation

        if (TIMING)
        {
            printf("Check point 7: %f.\n", GetTime() - t); t = GetTime();
        }

        BR = A.solve_with_factor(dW);

        if (TIMING)
        {
            printf("Check point 8: %f. \t Back Sub 2\n", GetTime() - t); t = GetTime();
        }

        // Calculate gradients for the second time

        switch (2) { // 2 is fastest

        case 0:

            PS = Gu.mul(BR); // old code

            break;

        case 1:

            PS = Dense::concatenate(Gxu.mul(BR), Gyu.mul(BR));
            if (dim == 3)
                PS = Dense::concatenate(PS, Gzu.mul(BR));

            break;

        case 2:

            static Dense BRa;
            if (i==0)
                BRa = Dense::Zeros(V.nrow(), BR.ncol());

            BRa.slice_assign_value(unknown, BR, 0);

            grad_tf.run(BRa, PS);

            break;

        default:
            ;
        }

        if (TIMING)
        {
            printf("Check point 9: %f. \t Mat Multiplication 2.\n", GetTime() - t); t = GetTime();
        }

        gau = Dense::Zeros(f * mcdim, 1);

        for (int j = 0; j < W.ncol(); j++)
        {
            symmetric_tensor_span_dot(&GW(0, j), &PS(0, j), gau_j.head(), f, dim);
            // gau = gau - gau_j;
            Dense::saxy(gau_j, gau, -1.0);
        }

        if (TIMING)
        {
            printf("Check point 10: %f.\n", GetTime() - t); t = GetTime();
        }

        /* gat <- gau: back-prop grad */

        // s_pdapdt_lmul_fast(at_tmp, gau, gat, FA, dim, para);
        s_pdapdt_lmul(at, gau, gat, FA, dim, para);

        if (TIMING)
        {
            printf("Check point 11: %f.\n", GetTime() - t); t = GetTime();
        }

        i++;
        return e;
        // e may not be the function value, depends on parameters.
    };

    Dense at_out;
    {
        using arma::vec;
        using arma::mat;
        vec at = dense_array_to_arma_vec(at0);

        printf("Solver: %s\n", SOLVER.c_str());

        if (SOLVER==std::string("adamd"))
        {
            /* Adam - based optim */

            optim::algo_settings_t settings;
            settings.gd_method = 6;
            settings.gd_settings.step_size = STEP_SIZE;
            settings.iter_max = NUM_ITER;

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            for (int j = 0; j < REPEAT; j++)
            {
                optim::gd2(at, fn, NULL, settings); // gd2 gets rid of an extra call of fn at the end.

                settings.gd_settings.step_size /= 2.0;
                printf("\nlr=%g \n", settings.gd_settings.step_size);
            }

            optim::gd(at, fn, NULL, settings);
            // NUM_ITER * REPEAT + NUM_ITER: in total

        }
        else if (SOLVER == std::string("adam"))
        {

            optim::algo_settings_t settings;
            settings.gd_method = 6;
            settings.gd_settings.step_size = STEP_SIZE;
            settings.iter_max = NUM_ITER * (REPEAT+1);

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            optim::gd(at, fn, NULL, settings);

        }
        else if (SOLVER == std::string("lbfgs"))
        {

            optim::algo_settings_t settings;
            settings.iter_max = NUM_ITER * (REPEAT + 1);
            settings.lbfgs_par_M = LBFGS_M;

            std::function<double(const arma::vec&, arma::vec*, void*)> fn
                = [&](const arma::vec& at, arma::vec* pgat, void* opt_data) {
                double v = fun(at, *pgat);
                return v;
            };

            optim::lbfgs(at, fn, NULL, settings);

        }
        else
        {
            std::cerr << "--solver not supported." << std::endl;
        }

        at_out = arma_vec_to_dense_array(at);
    }

    printf("numerical_factor + solving: %f seconds.\n", GetTime() - start_time);

    if (LOG_HISTORY)
    {
        printf("Timing is not meaningful due to logging cost. \n");
    }

    FW.slice_assign_value(unknown, FW_u, 0); // FW(unknown, :) = FW_u;
    FW.write(OUTPUT+"W.mtx"); // projected weights
    W.write(OUTPUT+"UW.mtx"); // unprojected weights
    at_out.write(OUTPUT+"at.mtx");
    au.write(OUTPUT + "au.mtx");

    End(cm);

    free(I);
    free(J);

    FILE* filename = fopen((OUTPUT+"log.txt").c_str(), "wb");
    if (filename != NULL)
    {
        for (int i = 0; i < length_his; i++)
        {
            fprintf(filename, "%04d\t%g\t%g\n", i, energy_his[i], time_his[i]);
        }
        fclose(filename);
    }

    free(energy_his);

    return 0;
}

// #include "iter_lap_timing.cpp"


	#include "savageFunctions.h"
#include <iostream>
#include <cmath>
#include <cstdlib>
#include <string>
#include <Eigen/Dense>

// a little documentation up front:
// an eigen vector is what you'd expect--a vector that only scales in a linear transformation
// an "Eigen" vector is a vector using the Eigen api

// also, it's pretty convenient that eigen objects work as regular objects for pass by value and return by value
// so that's nice to keep in mind

Eigen::Matrix3d savageFunctions::skew(Eigen::Vector3d v)
{//pass in an "Eigen" vector

	//initialize a "skewwed" matrix
	Eigen::Matrix3d skewwed;

	//populate the "skewwed" matrix
	skewwed <<    0, -v(2),  v(1),
			   v(2),     0, -v(0),
			  -v(1),  v(0),     0;

  //return the skewwed matrix
  return skewwed;
}

Eigen::VectorXd savageFunctions::stateIntegrate(Eigen::Vector3d omega, Eigen::Vector3d accel,
												  Eigen::VectorXd prevState, Eigen::Matrix3d prevDCM, double d_t)
{
	//state of form,
	// 0 x
	// 1 y
	// 2 z
	// 3 v_x
	// 4 v_y
	// 5 v_z
	// 6 a_x
	// 7 a_y
	// 8 a_z

	//and of course the previous dcm is needed

	//EVERY PARAMETER must be of the "Eigen" type

	//Okay getting started here
	//First things first,
	//use the previous time step DCM and the current angular rates to
	//find the current derivative of the current DCM
	//the d_ will be used to denore a time rate of change for something that doesn't have a
	//standard usage (eg. theta->omega)
	//taken from 3-53 of savage (eq. 3.3.2-6)
	Eigen::Matrix3d d_DCM = prevDCM * skew(omega);

	//calculate the current DCM
	Eigen::Matrix3d DCM = d_DCM * d_t + prevDCM;

	// use the current DCM to convert acceleration to the inertial frame
	Eigen::Vector3d accelInertial = DCM * accel;

	//okay now working down through the state vector and integrating as I go

	Eigen::VectorXd state(9);  // x               v*t                 .5at^2
	               state << prevState[0] + prevState[3] * d_t + accelInertial[0] * pow(d_t,2) * .5, //x
							prevState[1] + prevState[4] * d_t + accelInertial[1] * pow(d_t,2) * .5, //y
							prevState[2] + prevState[5] * d_t + accelInertial[2] * pow(d_t,2) * .5, //z
							 // v                 a*t
							prevState[3] + accelInertial[0] * d_t, //v_x
							prevState[4] + accelInertial[1] * d_t, //v_y
							prevState[5] + accelInertial[2] * d_t, //v_z
							 // a
							accelInertial[0], //a_x
							accelInertial[1], //a_y
							accelInertial[2]; //a_z

	std::cout << DCM << std::endl;

	return state;
} s


#include <string>
#include <vector>
#include <map>
#include <set>
#include <cmath>
#include <utility>
#include <algorithm>

#include "main.h"
#include "option.h"
#include "likelihood.h"
#include <boost/math/distributions/chi_squared.hpp>

// needed by alglib
#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "optimization.h"
#include "ap.h"

struct fn_data {
    std::string         base;
    std::vector<double> errRateV;
};

// composite log likelihood: l_c(theta)
// mat theta is a column vector which has 4 elements for A, C, G, T, respectively.
//
double composite_LogLikelihood (
        const string         &base,
        const vector<double> &errRateV,
        const alglib::real_1d_array  &theta )
{
    double l_c(0.0);

    for (size_t i(0); i != base.size(); i++ ) {
        const double &e = errRateV[i]/3;
        switch( base[i] ) {
            case 'A': l_c += log( (1-4*e) * theta[0] + e ); break;
            case 'C': l_c += log( (1-4*e) * theta[1] + e ); break;
            case 'G': l_c += log( (1-4*e) * theta[2] + e ); break;
            case 'T': l_c += log( (1-4*e) * theta[3] + e ); break;
            default: cerr << "unknown base in " << base << endl,exit(1);
        }
    }

    return l_c;
}

// -composite score function: -U_c(theta)
// return a column vector
//
alglib::real_1d_array composite_score (
        const string         &base,
        const vector<double> &errRateV,
        const alglib::real_1d_array  &theta )
{
    alglib::real_1d_array U_c = "[0,0,0,0]";

    for (size_t i(0); i != base.size(); i++ ) {
        const double &e = errRateV[i]/3;

        switch( base[i] ) {
            case 'A': U_c[0] -= (1-4*e) / ( (1-4*e)*theta[0] + e );  break;
            case 'C': U_c[1] -= (1-4*e) / ( (1-4*e)*theta[1] + e );  break;
            case 'G': U_c[2] -= (1-4*e) / ( (1-4*e)*theta[2] + e );  break;
            case 'T': U_c[3] -= (1-4*e) / ( (1-4*e)*theta[3] + e );  break;
            default: cerr << "unknown base in " << base << endl, exit(1);
        }
    }

    return U_c;
}

// gradient optimization
void function1_grad (
        const alglib::real_1d_array  &x,
        double                       &func,
        alglib::real_1d_array        &grad,
        void                         *opt_data )
{
    fn_data* objfn_data = reinterpret_cast<fn_data*>(opt_data);
    const std::string &base = objfn_data->base;
    const std::vector<double> &errRateV = objfn_data->errRateV;

    func = -composite_LogLikelihood( base, errRateV, x);
    grad = composite_score( base, errRateV, x );
}

string initAlleleFreq (
        mCharUlong       &fr,
        const double     &depth,
        const char       &except_b )
{
    ostringstream s;
    s << '[';

    if ( depth == fr[except_b] ) {
        for ( auto b : "ACGT" ) {
            b == except_b ? s << 0.0 : s << 0.333333333;
            b == 'T' ? s << ']' : s << ',';
            if ( b == 'T' ) break;
        }
    }
    else {
        for ( auto b : "ACGT" ) {
            b == except_b ? s << 0.0 : s << fr[b] / (depth - fr[except_b]);
            b == 'T' ? s << ']' : s << ',';
            if ( b== 'T' ) break;
        }
    }
    return s.str();
}

string initAlleleFreq (
        mCharUlong       &fr,
        double           depth,
        const set<char>  &except_bs )
{
    ostringstream s;

    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        if ( it != except_bs.end() ) {
            fr[b] = 0;
            depth -= fr[b];
        }

        if ( b== 'T' ) break;
    }

    if ( depth == 0 ) {
        s  << "[0.25,0.25,0.25,0.25]";
        return s.str();
    }

    s << '[';
    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        it != except_bs.end() ? s << 0.0 : s << fr[b] / depth;  // depth here don't contain num of except_bs
        b == 'T' ? s << ']' : s << ',';
        if ( b== 'T' ) break;
    }

    return s.str();
}

string _upBoundary(const char except_b)
{
    ostringstream s;
    s << '[';

    for ( auto b : "ACGT" ) {
        b == except_b ? s << 0 : s << 1;
        b == 'T' ? s << ']' : s << ',';
        if ( b == 'T' ) break;
    }
    return s.str();
}

string _upBoundary(const set<char> except_bs)
{
    ostringstream s;
    s << '[';

    for ( auto b : "ACGT" ) {
        set<char>::const_iterator it = except_bs.find(b);
        it != except_bs.end() ? s << 0 : s << 1;
        b == 'T' ? s << ']' : s << ',';
        if ( b == 'T' ) break;
    }
    return s.str();
}

map<char, vector<double> > llh_genotype(const string &s, const string &q, const Option &opt)
{
//    mCharDouble ntP; // nt => pvalue
    map<char, vector<double> > ntPF; // nt => pvalue, fraction

    boost::math::chi_squared X2_dist(1);

    mCharUlong fr;
    for ( auto b : "ACGTN" ) {
        fr[b] = 0;
        if ( b == 'N' ) break;
    }

    string new_s(""), new_q("");
    for ( size_t i(0); i != s.size(); i++ ) {
        if ( lowQuality(q[i], opt) || s[i] == 'N' || s[i] == '*' )  continue; // fr['N'] == 0
        fr[ s[i] ]++;
        new_s += s[i];
        new_q += q[i];
    }

    double depth(new_s.size());
    vector<double> errV = quaToErrorRate(new_q, opt);

    if ( depth == 0 ) return ntPF;

    fn_data data;
    data.base = new_s;
    data.errRateV = errV;

    // var for alglib
    alglib::minbleicstate state;
    alglib::minbleicreport rep;
    double epsg(0.000001);
    double epsf(0.0);
    double epsx(0.0);
    alglib::ae_int_t maxits(0);

    // constraint: sum of frequency of 4 alleles == 1
    alglib::real_2d_array c = "[[1,1,1,1,1]]";  // sum of four allele == 1
    alglib::integer_1d_array ct = "[0]";    // equal
    alglib::real_1d_array bndl = "[0,0,0,0]";  // lower boundary

    // four allele maximize
    double cl_4(0.0);
    try {
        string AFstr = initAlleleFreq(fr, depth, 'N');
        alglib::real_1d_array alg_x = AFstr.c_str();
        alglib::real_1d_array bndu = "[1,1,1,1]";

        alglib::minbleiccreate(alg_x, state);
        alglib::minbleicsetlc(state, c, ct);
        alglib::minbleicsetbc(state, bndl, bndu);
        alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
        alglib::minbleicoptimize(state, function1_grad, NULL, &data );
        alglib::minbleicresults(state, alg_x, rep);

        if ( opt.debug ) {
            printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
            printf("%s\n", alg_x.tostring(20).c_str());
        }

        cl_4 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
        if ( opt.debug ) cout << "cl_4: " << setprecision(20) << cl_4 << endl;
    }
    catch ( alglib::ap_error &e ) {
        cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q << "]" << endl;
    }

    map<char, string> init_V;
    map<char, string> bndu_V;

    for ( auto b : "ACGT" ) {
        init_V[b] = initAlleleFreq(fr, depth, b);
        bndu_V[b] = _upBoundary(b);
        if ( b == 'T' ) break;
    }

    for ( mCharUlong::const_iterator it = fr.begin(); it != fr.end(); it++ )
    {
        if ( it->second < opt.minSupOnEachStrand || it->second/depth < opt.minFractionInFam ) continue;

        double cl_3(0.0);
        try {
            alglib::real_1d_array alg_x = init_V[ it->first ].c_str();
            alglib::real_1d_array bndu = bndu_V[ it->first ].c_str();

            alglib::minbleiccreate(alg_x, state);
            alglib::minbleicsetlc(state, c, ct);
            alglib::minbleicsetbc(state, bndl, bndu);
            alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
            alglib::minbleicoptimize(state, function1_grad, NULL, &data );
            alglib::minbleicresults(state, alg_x, rep);

            if ( opt.debug ) {
                printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
                printf("%s\n", alg_x.tostring(20).c_str());
            }

            cl_3 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
            if ( opt.debug ) cout << "cl_3: " << cl_3 << endl;
        }
        catch ( alglib::ap_error &e ) {
            cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q
                << "] for base[" << it->first << "]" << endl;
        }

        if ( cl_4 - cl_3 > opt.lhrGapCutoff ) {
//            ntP[ it->first ] = 1 - boost::math::cdf(X2_dist, 2*(cl_4 - cl_3) );
            ntPF[ it->first ].push_back( 1 - boost::math::cdf(X2_dist, 2*(cl_4 - cl_3)) );
        }
    }

    if ( ntPF.size() == 1 ) {
        ntPF[ ntPF.begin()->first ].push_back(1.0);
        return ntPF;
    }
    else if ( ntPF.size() > 1 ) {
        set<char> except_bs;
        for ( auto b : "ACGT" ) {
            map<char, vector<double> >::const_iterator it = ntPF.find(b);
            if ( it == ntPF.end() ) {  // not in ntPF
                except_bs.insert(b);
            }

            if ( b == 'T' ) break;
        }

        string AFstr = initAlleleFreq(fr, depth, except_bs);
        alglib::real_1d_array alg_x = AFstr.c_str();

        try {
            string upBnd = _upBoundary(except_bs);
            alglib::real_1d_array bndu = upBnd.c_str();

            alglib::minbleiccreate(alg_x, state);
            alglib::minbleicsetlc(state, c, ct);
            alglib::minbleicsetbc(state, bndl, bndu);
            alglib::minbleicsetcond(state, epsg, epsf, epsx, maxits);
            alglib::minbleicoptimize(state, function1_grad, NULL, &data );
            alglib::minbleicresults(state, alg_x, rep);

            if ( opt.debug ) {
                printf("%d\n", int(rep.terminationtype)); // EXPECTED: 4
                printf("%s\n", alg_x.tostring(20).c_str());
            }

            cl_4 = composite_LogLikelihood( data.base, data.errRateV, alg_x );
            if ( opt.debug ) cout << "cl_4: " << setprecision(20) << cl_4 << endl;
        }
        catch ( alglib::ap_error &e ) {
            cerr << "catch error: " << e.msg << " at seq[" << new_s << "] qua[" << new_q << "]" << endl;
        }

        string st = "ACGT";
        map<char, double> mBaseFrac;
        for ( int i(0); i != 4; i++ ) {
            mBaseFrac[ st[i] ] = alg_x[i];
        }

        for ( auto &p : ntPF ) {
            ntPF[ p.first ].push_back( mBaseFrac[p.first] );
        }

        return ntPF;
    }
    else if ( ntPF.size() > 4 ) {
        cerr << "ntPF contain unknown base" << endl, exit(1);
    }
    else {
        return ntPF;
    }
}


#ifndef FEA_MATERIAL
#define FEA_MATERIAL

#include <Eigen/Dense>
#include "../euclid/Geometry"

namespace FEA {
class Material {
	double _young;
	double _poisson;
	public:
	Material(double E=1.,double nu=0.3) : _young(E), _poisson(nu) {};
	const Eigen::Matrix<double,6,6> ConstitutiveLinearIsotropicElastic ();
};

inline const Eigen::Matrix<double,6,6>
Material::ConstitutiveLinearIsotropicElastic () {
	double l = ( _young * _poisson ) / ((1. + _poisson ) * (1. - 2. * _poisson ));
	double g =   _young / (2. * (1. + _poisson ));
	double G = 2. * g;
	Eigen::Matrix<double,6,6> C;
	C << 	l+G, 	l, 		l, 		0., 	0., 	0.,
			l, 		l+G, 	l, 		0., 	0., 	0.,
			l, 		l, 		l+G, 	0., 	0., 	0.,
			0., 	0., 	0., 	g, 		0., 	0.,
			0., 	0., 	0., 	0., 	g, 		0.,
			0., 	0., 	0., 	0., 	0., 	g;
	return C;
};

}

#endif


/* Boost libs/numeric/odeint/examples/multiprecision/cmp_precision.cpp

 Copyright 2009-2013 Karsten Ahnert
 Copyright 2009-2013 Mario Mulansky

 example comparing double to multiprecision using Boost.Multiprecision

 Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <boost/numeric/odeint.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace boost::numeric::odeint;

typedef boost::multiprecision::cpp_dec_float_50 mp_50;

/* we solve the simple ODE x' = 3/(2t^2) + x/(2t)
 * with initial condition x(1) = 0.
 * Analytic solution is x(t) = sqrt(t) - 1/t
 */

void rhs_m( const mp_50 x , mp_50 &dxdt , const mp_50 t )
{   // version for multiprecision
    dxdt = mp_50(3)/(mp_50(2)*t*t) + x/(mp_50(2)*t);
}

void rhs_d( const double x , double &dxdt , const double t )
{   // version for double precision
    dxdt = 3.0/(2.0*t*t) + x/(2.0*t);
}

// state_type = mp_50 = deriv_type = time_type = mp_50
typedef runge_kutta4< mp_50 , mp_50 , mp_50 , mp_50 , vector_space_algebra , default_operations , never_resizer > stepper_type_m;

typedef runge_kutta4< double , double , double , double , vector_space_algebra , default_operations , never_resizer > stepper_type_d;

int main()
{

    stepper_type_m stepper_m;
    stepper_type_d stepper_d;

    mp_50 dt_m( 0.5 );
    double dt_d( 0.5 );

    cout << "dt" << '\t' << "mp" << '\t' << "double" << endl;

    while( dt_m > 1E-20 )
    {

        mp_50 x_m = 0; //initial value x(1) = 0
        stepper_m.do_step( rhs_m , x_m , mp_50( 1 ) , dt_m );
        double x_d = 0;
        stepper_d.do_step( rhs_d , x_d , 1.0 , dt_d );

        cout << dt_m << '\t';
        cout << abs((x_m - (sqrt(1+dt_m)-mp_50(1)/(1+dt_m)))/x_m) << '\t' ;
        cout << abs((x_d - (sqrt(1+dt_d)-mp_50(1)/(1+dt_d)))/x_d) << endl ;
        dt_m /= 2;
        dt_d /= 2;
    }
}


#include "multi_cg/multi_cg.hpp"

#include <Eigen/Core>

#include <iostream>

using namespace Eigen;

template<typename T>
struct BlockVector {
    Matrix<T, Dynamic, Dynamic> vec;

    typedef T value_type;

    typedef Matrix<T, Dynamic, 1> VectorT;

    BlockVector(Matrix<T, Dynamic, Dynamic> && X) : vec(std::move(X)) {}

    // Make it easy to switch between f32 and f64.
    template <typename U>
    BlockVector(BlockVector<U> const &X) : vec(X.vec.template cast<T>()) {}

    template <typename U>
    void block_add(BlockVector<U> const &X, size_t num) {
        vec.leftCols(num) += X.vec.leftCols(num).template cast<T>();
    }

    void block_axpy(std::vector<T> alphas, BlockVector const &X, size_t num) {
        DiagonalMatrix<T,Dynamic,Dynamic> D = Map<VectorT>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) += X.vec.leftCols(num) * D;
    }

    void block_axpy_scatter(std::vector<T> alphas, BlockVector const &X, std::vector<size_t> ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            vec.col(ids[i]) += alphas[i] * X.vec.col(i);
        }
    }

    // rhos[i] = dot(X[i], Y[i])
    void block_dot(BlockVector const &Y, std::vector<T> &rhos, size_t num) {
        VectorT result = (vec.leftCols(num).transpose() * Y.vec.leftCols(num)).diagonal();
        VectorT::Map(rhos.data(), result.size()) = result;
    }

    // X[:, i] = Z[:, i] + alpha[i] * X[:, i] for i < num_unconverged
    void block_xpby(BlockVector const &Z, std::vector<T> alphas, size_t num) {
        DiagonalMatrix<T,Dynamic,Dynamic> D = Map<VectorT>(alphas.data(), num).asDiagonal();
        vec.leftCols(num) = Z.vec.leftCols(num) + vec.leftCols(num) * D;
    }

    void copy(BlockVector const &X, size_t num) {
        vec.leftCols(num) = X.vec.leftCols(num);
    }

    void fill(T val) {
        vec.fill(val);
    }

    auto cols() {
        return vec.cols();
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];
            if (j != i) {
                vec.col(i) = vec.col(j);
            }
        }
    }
};

// This is a linear but special operator A(X)
// producing AX + XD where D_ii = shifts[i] is a diagonal matrix.
// So column-wise it performs (A + shift[i])X[:, i]
// the multiply function basically does a gemv on every column with a different shift
// so alpha * A(X) + beta * Y.
template <typename T>
struct PosDefMatrixShifted {
    DiagonalMatrix<T, Dynamic, Dynamic> A;
    Matrix<T, Dynamic, 1> shifts;

    PosDefMatrixShifted(DiagonalMatrix<T, Dynamic, Dynamic> && A, Matrix<T, Dynamic, 1> && shifts)
        : A(std::move(A)),
          shifts(std::move(shifts))
    {}

    // Make it easy to switch between f32 and f64.
    template <typename U>
    PosDefMatrixShifted(PosDefMatrixShifted<U> const &mat)
        : A(mat.A.diagonal().template cast<T>().asDiagonal()),
          shifts(mat.shifts.template cast<T>())
    {}

    void multiply(T alpha, BlockVector<T> const &u, T beta, BlockVector<T> &v, size_t num) {
        v.vec.leftCols(num) = alpha * A * u.vec.leftCols(num)
                              + alpha * u.vec.leftCols(num) * shifts.head(num).asDiagonal()
                              + beta * v.vec.leftCols(num);
    }

    void repack(std::vector<size_t> const &ids) {
        for (size_t i = 0; i < ids.size(); ++i) {
            auto j = ids[i];

            if (j != i)
                shifts[i] = shifts[j];
        }
    }
};

struct IdentityPreconditioner {
    template<typename T>
    void apply(BlockVector<T> &C, BlockVector<T> const &B) {
        C = B;
    }
    void repack(std::vector<size_t> const &ids) {
        // nothing to do;
    }
};

int main(int argc, char ** argv) {
    // The general idea is to solve Ax = b in mixed precision
    // Define the residual r_k = b - Ax_k
    // and the error e_k := x - x_k
    // which satisfy Ae_k = Ax - Ax_k = b - Ax_k = r_k.
    // So we are gonna solve Ae_k = r_k approximately for e_k, and by definition
    // x = e_k + x_k.
    // So we compute r_k = b - Ax_k in f64, and then solve Ae_k = r_k
    // approximately in f32.
    auto m = argc >= 2 ? std::stoul(argv[1]) : 1000;
    auto n = argc >= 3 ? std::stoul(argv[2]) : 20;
    auto outer_iter = argc >= 4 ? std::stoul(argv[3]) : 100;
    auto inner_iter = argc >= 5 ? std::stoul(argv[4]) : 40;

    if (n > m)
        throw std::runtime_error("matrix order should be >= block size");

    // Let's stick to this no-op preconditioner.
    auto P = IdentityPreconditioner{};

    // Setup f64 matrices and vecs
    auto A_hi = PosDefMatrixShifted<double>{
        VectorXd::LinSpaced(m, 1, m).asDiagonal(),
        VectorXd::LinSpaced(n, 1, n)
    };
    auto X_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto B_hi = BlockVector<double>{MatrixXd::Random(m, n)};
    auto R_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto U_hi = BlockVector<double>{MatrixXd::Zero(m, n)};
    auto C_hi = BlockVector<double>{MatrixXd::Zero(m, n)};

    // Setup f32 stuff.
    auto U_lo = BlockVector<float>{MatrixXf::Zero(m, n)};
    auto C_lo = BlockVector<float>{MatrixXf::Zero(m, n)};
    auto E_lo = BlockVector<float>(MatrixXf::Zero(m, n));
    auto R_lo = BlockVector<float>(MatrixXf::Zero(m, n));

    auto tol = 1e-10;

    std::vector<std::vector<float>> all_resnorms(n);

    for (size_t outer = 0; outer < outer_iter; ++outer) {
        // A_lo is mutated during multi_cg, so let's just reinitialize.
        auto A_lo = PosDefMatrixShifted<float>(A_hi);
        R_hi = B_hi;
        A_hi.multiply(-1.0, X_hi, 1.0, R_hi, n);

        E_lo.fill(0);
        R_lo = R_hi;
        auto iter_resnorms = sirius::cg::multi_cg(
            A_lo, P,
            E_lo, R_lo, U_lo, C_lo,
            inner_iter, tol, true
        );
        X_hi.block_add(E_lo, n);

        // Save all the resnorms
        bool done = true;
        for (size_t i = 0; i < n; ++i) {
            done &= iter_resnorms[i].back() <= tol;
            all_resnorms[i].insert(all_resnorms[i].end(), iter_resnorms[i].begin(), iter_resnorms[i].end());
        }
        if (done) break;
    }

    for (auto r : all_resnorms[0])
        std::cout << r << '\n';
    std::cout << '\n';

    // Compare to a f64-only run.
    X_hi.fill(0);
    R_hi = B_hi;
    auto resnorms_64 = sirius::cg::multi_cg(
        A_hi, P,
        X_hi, R_hi, U_hi, C_hi,
        m * n, tol, true
    );

    for (auto r : resnorms_64[0])
        std::cout << r << '\n';
    std::cout << '\n';
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
#define CROCODDYL_CORE_SOLVERS_DDP_HPP_

#include <Eigen/Cholesky>
#include <vector>

#include "crocoddyl/core/solver-base.hpp"

namespace crocoddyl {

/**
 * @brief Differential Dynamic Programming (DDP) solver
 *
 * The DDP solver computes an optimal trajectory and control commands by iterates running `backwardPass()` and
 * `forwardPass()`. The backward-pass updates locally the quadratic approximation of the problem and computes descent
 * direction. If the warm-start is feasible, then it computes the gaps \f$\mathbf{\bar{f}}_s\f$ and run a modified
 * Riccati sweep:
 * \f{eqnarray*}
 *   \mathbf{Q}_{\mathbf{x}_k} &=& \mathbf{l}_{\mathbf{x}_k} + \mathbf{f}^\top_{\mathbf{x}_k} (V_{\mathbf{x}_{k+1}} +
 * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
 *   \mathbf{Q}_{\mathbf{u}_k} &=& \mathbf{l}_{\mathbf{u}_k} + \mathbf{f}^\top_{\mathbf{u}_k} (V_{\mathbf{x}_{k+1}} +
 * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
 *   \mathbf{Q}_{\mathbf{xx}_k} &=& \mathbf{l}_{\mathbf{xx}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{x}_k},\\
 *   \mathbf{Q}_{\mathbf{xu}_k} &=& \mathbf{l}_{\mathbf{xu}_k} + \mathbf{f}^\top_{\mathbf{x}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{u}_k},\\
 *   \mathbf{Q}_{\mathbf{uu}_k} &=& \mathbf{l}_{\mathbf{uu}_k} + \mathbf{f}^\top_{\mathbf{u}_k} V_{\mathbf{xx}_{k+1}}
 * \mathbf{f}_{\mathbf{u}_k}.
 * \f}
 * Then, the forward-pass rollouts this new policy by integrating the system dynamics along a tuple of optimized
 * control commands \f$\mathbf{u}^*_s\f$, i.e.
 * \f{eqnarray}
 *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0,\\
 *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
 *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k).
 * \f}
 *
 * \sa `backwardPass()` and `forwardPass()`
 */
class SolverDDP : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /**
   * @brief Initialize the DDP solver
   *
   * @param[in] problem  Shooting problem
   */
  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverDDP();

  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
                     const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
                     const bool& is_feasible = false, const double& regInit = 1e-9);
  virtual void computeDirection(const bool& recalc = true);
  virtual double tryStep(const double& steplength = 1);
  virtual double stoppingCriteria();
  virtual const Eigen::Vector2d& expectedImprovement();

  /**
   * @brief Update the Jacobian and Hessian of the optimal control problem
   *
   * These derivatives are computed around the guess state and control trajectory. These trajectory can be set by using
   * `setCandidate()`.
   *
   * @return  The total cost around the guess trajectory
   */
  virtual double calcDiff();

  /**
   * @brief Run the backward pass (Riccati sweep)
   *
   * It assumes that the Jacobian and Hessians of the optimal control problem have been compute (i.e. `calcDiff()`).
   * The backward pass handles infeasible guess through a modified Riccati sweep:
   * \f{eqnarray*}
   *   \mathbf{Q}_{\mathbf{x}_k} &=& \mathbf{l}_{\mathbf{x}_k} + \mathbf{f}^\top_{\mathbf{x}_k} (V_{\mathbf{x}_{k+1}}
   * +
   * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
   *   \mathbf{Q}_{\mathbf{u}_k} &=& \mathbf{l}_{\mathbf{u}_k} + \mathbf{f}^\top_{\mathbf{u}_k} (V_{\mathbf{x}_{k+1}}
   * +
   * V_{\mathbf{xx}_{k+1}}\mathbf{\bar{f}}_{k+1}),\\
   *   \mathbf{Q}_{\mathbf{xx}_k} &=& \mathbf{l}_{\mathbf{xx}_k} + \mathbf{f}^\top_{\mathbf{x}_k}
   * V_{\mathbf{xx}_{k+1}}
   * \mathbf{f}_{\mathbf{x}_k},\\
   *   \mathbf{Q}_{\mathbf{xu}_k} &=& \mathbf{l}_{\mathbf{xu}_k} + \mathbf{f}^\top_{\mathbf{x}_k}
   * V_{\mathbf{xx}_{k+1}}
   * \mathbf{f}_{\mathbf{u}_k},\\
   *   \mathbf{Q}_{\mathbf{uu}_k} &=& \mathbf{l}_{\mathbf{uu}_k} + \mathbf{f}^\top_{\mathbf{u}_k}
   * V_{\mathbf{xx}_{k+1}} \mathbf{f}_{\mathbf{u}_k}, \f} where
   * \f$\mathbf{l}_{\mathbf{x}_k}\f$,\f$\mathbf{l}_{\mathbf{u}_k}\f$,\f$\mathbf{f}_{\mathbf{x}_k}\f$ and
   * \f$\mathbf{f}_{\mathbf{u}_k}\f$ are the Jacobians of the cost function and dynamics,
   * \f$\mathbf{l}_{\mathbf{xx}_k}\f$,\f$\mathbf{l}_{\mathbf{xu}_k}\f$ and \f$\mathbf{l}_{\mathbf{uu}_k}\f$ are the
   * Hessians of the cost function, \f$V_{\mathbf{x}_{k+1}}\f$ and \f$V_{\mathbf{xx}_{k+1}}\f$ defines the
   * linear-quadratic approximation of the Value function, and \f$\mathbf{\bar{f}}_{k+1}\f$ describes the gaps of the
   * dynamics.
   */
  virtual void backwardPass();

  /**
   * @brief Run the forward pass or rollout
   *
   * It rollouts the action model given the computed policy (feedforward terns and feedback gains) by the
   * `backwardPass()`:
   * \f{eqnarray}
   *   \mathbf{\hat{x}}_0 &=& \mathbf{\tilde{x}}_0,\\
   *   \mathbf{\hat{u}}_k &=& \mathbf{u}_k + \alpha\mathbf{k}_k + \mathbf{K}_k(\mathbf{\hat{x}}_k-\mathbf{x}_k),\\
   *   \mathbf{\hat{x}}_{k+1} &=& \mathbf{f}_k(\mathbf{\hat{x}}_k,\mathbf{\hat{u}}_k).
   * \f}
   * We can define different step lengths \f$\alpha\f$.
   *
   * @param  stepLength  applied step length (\f$0\leq\alpha\leq1\f$)
   */
  virtual void forwardPass(const double& stepLength);

  /**
   * @brief Compute the feedforward and feedback terms using a Cholesky decomposition
   *
   * To compute the feedforward \f$\mathbf{k}_k\f$ and feedback \f$\mathbf{K}_k\f$ terms, we use a Cholesky
   * decomposition to solve \f$\mathbf{Q}_{\mathbf{uu}_k}^{-1}\f$ term:
   * \f{eqnarray}
   * \mathbf{k}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{u}},\\
   * \mathbf{K}_k &=& \mathbf{Q}_{\mathbf{uu}_k}^{-1}\mathbf{Q}_{\mathbf{ux}}.
   * \f}
   *
   * Note that if the Cholesky decomposition fails, then we re-start the backward pass and increase the
   * state and control regularization values.
   */
  virtual void computeGains(const std::size_t& t);

  /**
   * @brief Increase the state and control regularization values by a `regfactor_` factor
   */
  void increaseRegularization();

  /**
   * @brief Decrease the state and control regularization values by a `regfactor_` factor
   */
  void decreaseRegularization();

  /**
   * @brief Allocate all the internal data needed for the solver
   */
  virtual void allocateData();

  /**
   * @brief Return the regularization factor used to decrease / increase it
   */
  const double& get_regfactor() const;

  /**
   * @brief Return the minimum regularization value
   */
  const double& get_regmin() const;

  /**
   * @brief Return the maximum regularization value
   */
  const double& get_regmax() const;

  /**
   * @brief Return the set of step lengths using by the line-search procedure
   */
  const std::vector<double>& get_alphas() const;

  /**
   * @brief Return the step-length threshold used to decrease regularization
   */
  const double& get_th_stepdec() const;

  /**
   * @brief Return the step-length threshold used to increase regularization
   */
  const double& get_th_stepinc() const;

  /**
   * @brief Return the tolerance of the expected gradient used for testing the step
   */
  const double& get_th_grad() const;

  /**
   * @brief Return the threshold for accepting a gap as non-zero
   */
  const double& get_th_gaptol() const;

  /**
   * @brief Return the Hessian of the Value function \f$V_{\mathbf{xx}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Vxx() const;

  /**
   * @brief Return the Hessian of the Value function \f$V_{\mathbf{x}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Vx() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{xx}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Qxx() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{xu}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Qxu() const;

  /**
   * @brief Return the Hessian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{uu}_s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_Quu() const;

  /**
   * @brief Return the Jacobian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{x}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Qx() const;

  /**
   * @brief Return the Jacobian of the Hamiltonian function \f$\mathbf{Q}_{\mathbf{u}_s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_Qu() const;

  /**
   * @brief Return the feedback gains \f$\mathbf{K}_{s}\f$
   */
  const std::vector<Eigen::MatrixXd>& get_K() const;

  /**
   * @brief Return the feedforward gains \f$\mathbf{k}_{s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_k() const;

  /**
   * @brief Return the gaps \f$\mathbf{\bar{f}}_{s}\f$
   */
  const std::vector<Eigen::VectorXd>& get_fs() const;

  /**
   * @brief Modify the regularization factor used to decrease / increase it
   */
  void set_regfactor(const double& reg_factor);

  /**
   * @brief Modify the minimum regularization value
   */
  void set_regmin(const double& regmin);

  /**
   * @brief Modify the maximum regularization value
   */
  void set_regmax(const double& regmax);

  /**
   * @brief Modify the set of step lengths using by the line-search procedure
   */
  void set_alphas(const std::vector<double>& alphas);

  /**
   * @brief Modify the step-length threshold used to decrease regularization
   */
  void set_th_stepdec(const double& th_step);

  /**
   * @brief Modify the step-length threshold used to increase regularization
   */
  void set_th_stepinc(const double& th_step);

  /**
   * @brief Modify the tolerance of the expected gradient used for testing the step
   */
  void set_th_grad(const double& th_grad);

  /**
   * @brief Modify the threshold for accepting a gap as non-zero
   */
  void set_th_gaptol(const double& th_gaptol);

 protected:
  double regfactor_;  //!< Regularization factor used to decrease / increase it
  double regmin_;     //!< Minimum allowed regularization value
  double regmax_;     //!< Maximum allowed regularization value

  double cost_try_;                      //!< Total cost computed by line-search procedure
  std::vector<Eigen::VectorXd> xs_try_;  //!< State trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> us_try_;  //!< Control trajectory computed by line-search procedure
  std::vector<Eigen::VectorXd> dx_;

  // allocate data
  std::vector<Eigen::MatrixXd> Vxx_;  //!< Hessian of the Value function
  std::vector<Eigen::VectorXd> Vx_;   //!< Gradient of the Value function
  std::vector<Eigen::MatrixXd> Qxx_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Qxu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::MatrixXd> Quu_;  //!< Hessian of the Hamiltonian
  std::vector<Eigen::VectorXd> Qx_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::VectorXd> Qu_;   //!< Gradient of the Hamiltonian
  std::vector<Eigen::MatrixXd> K_;    //!< Feedback gains
  std::vector<Eigen::VectorXd> k_;    //!< Feed-forward terms
  std::vector<Eigen::VectorXd> fs_;   //!< Gaps/defects between shooting nodes

  Eigen::VectorXd xnext_;                              //!< Next state
  Eigen::MatrixXd FxTVxx_p_;                           //!< fxTVxx_p_
  std::vector<Eigen::MatrixXd> FuTVxx_p_;              //!< fuTVxx_p_
  Eigen::VectorXd fTVxx_p_;                            //!< fTVxx_p term
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;  //!< Cholesky LLT solver
  std::vector<Eigen::VectorXd> Quuk_;                  //!< Quuk term
  std::vector<double> alphas_;                         //!< Set of step lengths using by the line-search procedure
  double th_grad_;     //!< Tolerance of the expected gradient used for testing the step
  double th_gaptol_;   //!< Threshold limit to check non-zero gaps
  double th_stepdec_;  //!< Step-length threshold used to decrease regularization
  double th_stepinc_;  //!< Step-length threshold used to increase regularization
  bool was_feasible_;  //!< Label that indicates in the previous iterate was feasible
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_SOLVERS_DDP_HPP_


//  (C) Copyright Anton Bikineev 2014
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_RECURRENCE_HPP_
#define BOOST_MATH_TOOLS_RECURRENCE_HPP_

#include <boost/math/tools/config.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/tools/tuple.hpp>
#include <boost/math/tools/fraction.hpp>
#include <boost/math/tools/cxx03_warn.hpp>
#include <boost/math/tools/assert.hpp>

namespace boost {
   namespace math {
      namespace tools {
         namespace detail{

            //
            // Function ratios directly from recurrence relations:
            // H. Shintan, Note on Miller's recurrence algorithm, J. Sci. Hiroshima Univ. Ser. A-I
            // Math., 29 (1965), pp. 121 - 133.
            // and:
            // COMPUTATIONAL ASPECTS OF THREE-TERM RECURRENCE RELATIONS
            // WALTER GAUTSCHI
            // SIAM REVIEW Vol. 9, No. 1, January, 1967
            //
            template <class Recurrence>
            struct function_ratio_from_backwards_recurrence_fraction
            {
               typedef typename boost::remove_reference<decltype(boost::math::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;
               typedef std::pair<value_type, value_type> result_type;
               function_ratio_from_backwards_recurrence_fraction(const Recurrence& r) : r(r), k(0) {}

               result_type operator()()
               {
                  value_type a, b, c;
                  boost::math::tie(a, b, c) = r(k);
                  ++k;
                  // an and bn defined as per Gauchi 1.16, not the same
                  // as the usual continued fraction a' and b's.
                  value_type bn = a / c;
                  value_type an = b / c;
                  return result_type(-bn, an);
               }

            private:
               function_ratio_from_backwards_recurrence_fraction operator=(const function_ratio_from_backwards_recurrence_fraction&);

               Recurrence r;
               int k;
            };

            template <class R, class T>
            struct recurrence_reverser
            {
               recurrence_reverser(const R& r) : r(r) {}
               boost::math::tuple<T, T, T> operator()(int i)
               {
                  using std::swap;
                  boost::math::tuple<T, T, T> t = r(-i);
                  swap(boost::math::get<0>(t), boost::math::get<2>(t));
                  return t;
               }
               R r;
            };

            template <class Recurrence>
            struct recurrence_offsetter
            {
               typedef decltype(std::declval<Recurrence&>()(0)) result_type;
               recurrence_offsetter(Recurrence const& rr, int offset) : r(rr), k(offset) {}
               result_type operator()(int i)
               {
                  return r(i + k);
               }
            private:
               Recurrence r;
               int k;
            };

         }  // namespace detail

         //
         // Given a stable backwards recurrence relation:
         // a f_n-1 + b f_n + c f_n+1 = 0
         // returns the ratio f_n / f_n-1
         //
         // Recurrence: a functor that returns a tuple of the factors (a,b,c).
         // factor:     Convergence criteria, should be no less than machine epsilon.
         // max_iter:   Maximum iterations to use solving the continued fraction.
         //
         template <class Recurrence, class T>
         T function_ratio_from_backwards_recurrence(const Recurrence& r, const T& factor, std::uintmax_t& max_iter)
         {
            detail::function_ratio_from_backwards_recurrence_fraction<Recurrence> f(r);
            return boost::math::tools::continued_fraction_a(f, factor, max_iter);
         }

         //
         // Given a stable forwards recurrence relation:
         // a f_n-1 + b f_n + c f_n+1 = 0
         // returns the ratio f_n / f_n+1
         //
         // Note that in most situations where this would be used, we're relying on
         // pseudo-convergence, as in most cases f_n will not be minimal as N -> -INF
         // as long as we reach convergence on the continued-fraction before f_n
         // switches behaviour, we should be fine.
         //
         // Recurrence: a functor that returns a tuple of the factors (a,b,c).
         // factor:     Convergence criteria, should be no less than machine epsilon.
         // max_iter:   Maximum iterations to use solving the continued fraction.
         //
         template <class Recurrence, class T>
         T function_ratio_from_forwards_recurrence(const Recurrence& r, const T& factor, std::uintmax_t& max_iter)
         {
            boost::math::tools::detail::function_ratio_from_backwards_recurrence_fraction<boost::math::tools::detail::recurrence_reverser<Recurrence, T> > f(r);
            return boost::math::tools::continued_fraction_a(f, factor, max_iter);
         }

         // solves usual recurrence relation for homogeneous
         // difference equation in stable forward direction
         // a(n)w(n-1) + b(n)w(n) + c(n)w(n+1) = 0
         //
         // Params:
         // get_coefs: functor returning a tuple, where
         //            get<0>() is a(n); get<1>() is b(n); get<2>() is c(n);
         // last_index: index N to be found;
         // first: w(-1);
         // second: w(0);
         //
         template <class NextCoefs, class T>
         inline T apply_recurrence_relation_forward(const NextCoefs& get_coefs, unsigned number_of_steps, T first, T second, long long* log_scaling = 0, T* previous = 0)
         {
            BOOST_MATH_STD_USING
            using boost::math::tuple;
            using boost::math::get;

            T third;
            T a, b, c;

            for (unsigned k = 0; k < number_of_steps; ++k)
            {
               tie(a, b, c) = get_coefs(k);

               if ((log_scaling) &&
                  ((fabs(tools::max_value<T>() * (c / (a * 2048))) < fabs(first))
                     || (fabs(tools::max_value<T>() * (c / (b * 2048))) < fabs(second))
                     || (fabs(tools::min_value<T>() * (c * 2048 / a)) > fabs(first))
                     || (fabs(tools::min_value<T>() * (c * 2048 / b)) > fabs(second))
                     ))

               {
                  // Rescale everything:
                  long long log_scale = lltrunc(log(fabs(second)));
                  T scale = exp(T(-log_scale));
                  second *= scale;
                  first *= scale;
                  *log_scaling += log_scale;
               }
               // scale each part separately to avoid spurious overflow:
               third = (a / -c) * first + (b / -c) * second;
               BOOST_MATH_ASSERT((boost::math::isfinite)(third));

               swap(first, second);
               swap(second, third);
            }

            if (previous)
               *previous = first;

            return second;
         }

         // solves usual recurrence relation for homogeneous
         // difference equation in stable backward direction
         // a(n)w(n-1) + b(n)w(n) + c(n)w(n+1) = 0
         //
         // Params:
         // get_coefs: functor returning a tuple, where
         //            get<0>() is a(n); get<1>() is b(n); get<2>() is c(n);
         // number_of_steps: index N to be found;
         // first: w(1);
         // second: w(0);
         //
         template <class T, class NextCoefs>
         inline T apply_recurrence_relation_backward(const NextCoefs& get_coefs, unsigned number_of_steps, T first, T second, long long* log_scaling = 0, T* previous = 0)
         {
            BOOST_MATH_STD_USING
            using boost::math::tuple;
            using boost::math::get;

            T next;
            T a, b, c;

            for (unsigned k = 0; k < number_of_steps; ++k)
            {
               tie(a, b, c) = get_coefs(-static_cast<int>(k));

               if ((log_scaling) &&
                  ( (fabs(tools::max_value<T>() * (a / b) / 2048) < fabs(second))
                     || (fabs(tools::max_value<T>() * (a / c) / 2048) < fabs(first))
                     || (fabs(tools::min_value<T>() * (a / b) * 2048) > fabs(second))
                     || (fabs(tools::min_value<T>() * (a / c) * 2048) > fabs(first))
                  ))
               {
                  // Rescale everything:
                  int log_scale = itrunc(log(fabs(second)));
                  T scale = exp(T(-log_scale));
                  second *= scale;
                  first *= scale;
                  *log_scaling += log_scale;
               }
               // scale each part separately to avoid spurious overflow:
               next = (b / -a) * second + (c / -a) * first;
               BOOST_MATH_ASSERT((boost::math::isfinite)(next));

               swap(first, second);
               swap(second, next);
            }

            if (previous)
               *previous = first;

            return second;
         }

         template <class Recurrence>
         struct forward_recurrence_iterator
         {
            typedef typename boost::remove_reference<decltype(std::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;

            forward_recurrence_iterator(const Recurrence& r, value_type f_n_minus_1, value_type f_n)
               : f_n_minus_1(f_n_minus_1), f_n(f_n), coef(r), k(0) {}

            forward_recurrence_iterator(const Recurrence& r, value_type f_n)
               : f_n(f_n), coef(r), k(0)
            {
               std::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<boost::math::policies::policy<> >();
               f_n_minus_1 = f_n * boost::math::tools::function_ratio_from_forwards_recurrence(detail::recurrence_offsetter<Recurrence>(r, -1), value_type(boost::math::tools::epsilon<value_type>() * 2), max_iter);
               boost::math::policies::check_series_iterations<value_type>("forward_recurrence_iterator<>::forward_recurrence_iterator", max_iter, boost::math::policies::policy<>());
            }

            forward_recurrence_iterator& operator++()
            {
               using std::swap;
               value_type a, b, c;
               boost::math::tie(a, b, c) = coef(k);
               value_type f_n_plus_1 = a * f_n_minus_1 / -c + b * f_n / -c;
               swap(f_n_minus_1, f_n);
               swap(f_n, f_n_plus_1);
               ++k;
               return *this;
            }

            forward_recurrence_iterator operator++(int)
            {
               forward_recurrence_iterator t(*this);
               ++(*this);
               return t;
            }

            value_type operator*() { return f_n; }

            value_type f_n_minus_1, f_n;
            Recurrence coef;
            int k;
         };

         template <class Recurrence>
         struct backward_recurrence_iterator
         {
            typedef typename boost::remove_reference<decltype(std::get<0>(std::declval<Recurrence&>()(0)))>::type value_type;

            backward_recurrence_iterator(const Recurrence& r, value_type f_n_plus_1, value_type f_n)
               : f_n_plus_1(f_n_plus_1), f_n(f_n), coef(r), k(0) {}

            backward_recurrence_iterator(const Recurrence& r, value_type f_n)
               : f_n(f_n), coef(r), k(0)
            {
               std::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<boost::math::policies::policy<> >();
               f_n_plus_1 = f_n * boost::math::tools::function_ratio_from_backwards_recurrence(detail::recurrence_offsetter<Recurrence>(r, 1), value_type(boost::math::tools::epsilon<value_type>() * 2), max_iter);
               boost::math::policies::check_series_iterations<value_type>("backward_recurrence_iterator<>::backward_recurrence_iterator", max_iter, boost::math::policies::policy<>());
            }

            backward_recurrence_iterator& operator++()
            {
               using std::swap;
               value_type a, b, c;
               boost::math::tie(a, b, c) = coef(k);
               value_type f_n_minus_1 = c * f_n_plus_1 / -a + b * f_n / -a;
               swap(f_n_plus_1, f_n);
               swap(f_n, f_n_minus_1);
               --k;
               return *this;
            }

            backward_recurrence_iterator operator++(int)
            {
               backward_recurrence_iterator t(*this);
               ++(*this);
               return t;
            }

            value_type operator*() { return f_n; }

            value_type f_n_plus_1, f_n;
            Recurrence coef;
            int k;
         };

      }
   }
} // namespaces

#endif // BOOST_MATH_TOOLS_RECURRENCE_HPP_