#include <algorithm>
#include <iostream>
#include <sstream>
#include <fstream>
#include <math.h>
#include <vector>
#include "hybrid_astar.h"
#include <boost/heap/binomial_heap.hpp>

using namespace std;

/**
 * Initializes HAS
 */
HAS::HAS() {

}

HAS::~HAS() {}

//heap optimization method
struct Compare_cost {

  bool operator()(const HAS::Node3D & lhs, const HAS::Node3D & rhs) const {
    return lhs.f > rhs.f;
  }

};
typedef boost::heap::binomial_heap< HAS::Node3D,
                                    boost::heap::compare<Compare_cost>> SortedQueue;

double HAS::heuristic(double x, double y,
                      vector<int> goal,
                      string heuristic_method){

      double dx  = fabs(y - goal[0]);
      double dy  = fabs(x - goal[1]);
      double tie = (1.0 + 1.0/10);

      //http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#speed-or-accuracy
      if (heuristic_method == "Manhattan")       return dx + dy;
      if (heuristic_method == "Chebyshev")       return (dx + dy) -  min(dx, dy);
      if (heuristic_method == "Octile")          return 2*(dx + dy) - (3-2*2)*min(dx, dy);
      if (heuristic_method == "Euclidean")       return sqrt(dx * dx + dy * dy);
      if (heuristic_method == "Octile_breaktie") return tie*(2*(dx + dy) - (3-2*2)*min(dx, dy));
      //

}

/* HAS::theta_to_stack_number(double theta)
Takes an angle (in radians) and returns which "stack" in the 3D configuration space
this angle corresponds to. Angles near 0 go in the lower stacks while angles near
2 * pi go in the higher stacks.
*/
int HAS::theta_to_stack_number(double theta){

  double new_theta = fmod((theta + 2 * M_PI),(2 * M_PI));
  int stack_number = (int)(round(new_theta * NUM_THETA_CELLS / (2*M_PI))) % NUM_THETA_CELLS;
  return stack_number;
}

/*
Returns the index into the grid for continuous position. So if x is 3.621, then this
would return 3 to indicate that 3.621 corresponds to array index 3.
*/
int HAS::idx(double float_num) {

  return int(floor(float_num));
}

double HAS::turning_cost(double next_turn_angle,
                         double max_turnable,
                         double turning_weight){

    return turning_weight * fabs(next_turn_angle)/max_turnable;

}

vector<HAS::Node3D> HAS::expand(HAS::Node3D state,
                                vector<int> goal,
                                string heuristic_method) {

  int    g     = state.g;
  double x     = state.x;
  double y     = state.y;
  double theta = state.theta;

  int g2 = g + 1 ;
  vector<HAS::Node3D> next_states;

  for(double delta_i = -max_turnable;
             delta_i < (max_turnable + turning_res);
             delta_i += turning_res) {

    // Update next state
    double delta  = M_PI / 180.0 * delta_i;
    double omega  = SPEED / LENGTH * tan(delta);

    double theta2 = theta + omega;
    theta2        = fmod(theta2, 2*M_PI);
    if(theta2 < 0) { theta2 += 2*M_PI;}

    double x2 = x + SPEED * cos(theta);
    double y2 = y + SPEED * sin(theta);

    // Update next state cost
    int f2    = g2
                + turning_cost(delta_i, max_turnable, 1)
                + heuristic(x2, y2, goal, heuristic_method);

    // Create a new State object with all of the "next" values.
    HAS::Node3D state2 {g2, f2, x2, y2, theta2,};
    next_states.emplace_back(state2);

  }
  return next_states;

}

HAS::grid_path HAS::search_heap(vector<vector<int>> grid,
                                vector<double> start,
                                vector<int> goal,
                                string heuristic_method) {

  vector<vector<vector<int> > >    closed(NUM_THETA_CELLS,
                                          vector<vector<int>>(grid[0].size(), vector<int>(grid.size())));
  vector<vector<vector<Node3D> > > came_from(NUM_THETA_CELLS,
                                             vector<vector<Node3D>>(grid[0].size(), vector<Node3D>(grid.size())));
  double theta = start[2];
  int stack    = theta_to_stack_number(theta);
  int g        = 0;
  int f        = g + heuristic(start[0], start[1], goal, heuristic_method);

  // Create new state object to start the search with.
  Node3D state {g, f, start[0], start[1], theta};

  closed[stack][idx(state.x)][idx(state.y)]    = 1;
  came_from[stack][idx(state.x)][idx(state.y)] = state;

  int total_closed = 1;

  // Heap Method
  SortedQueue opened_heap;
  opened_heap.push(state);

  bool finished = false;

  while(!opened_heap.empty()) {

    // Heap Method
    Node3D current = opened_heap.top();// get smallest value
    opened_heap.pop();// delete

    int x = current.x;
    int y = current.y;

    // Check if reach the goal
    if(idx(x) == goal[0] && idx(y) == goal[1]){
      cout << " found path to goal in " << total_closed << " expansions" << endl;
      grid_path path {closed, came_from, current,};

      return path;
    }

    // Otherwise, expand the current state to get
    // a list of possible next states.
    vector<Node3D> next_state = expand(current, goal, heuristic_method);

    for(int i = 0; i < next_state.size(); i++) {
      int g2        = next_state[i].g;
      double x2     = next_state[i].x;
      double y2     = next_state[i].y;
      double theta2 = next_state[i].theta;

      // If we have expanded outside the grid, skip this next_state.
      if((x2 < 0 || x2 >= grid.size()) || (y2 < 0 || y2 >= grid[0].size())) {
        //invalid cell
        continue;
      }

      int stack2 = theta_to_stack_number(theta2);

      //Otherwise, check that we haven't already visited this cell and
      //that there is not an obstacle in the grid there.
      if(closed[stack2][idx(x2)][idx(y2)] == 0 && grid[idx(x2)][idx(y2)] == 0) {

        // The state can be added to the opened stack.
        opened_heap.push(next_state[i]);

        //The stack_number, idx(next_state.x), idx(next_state.y) tuple
        //has now been visited, so it can be closed.
        closed[stack2][idx(x2)][idx(y2)] = 1;

        //The next_state came from the current state, and that is recorded.
        came_from[stack2][idx(x2)][idx(y2)] = current;

        total_closed += 1;
      }

    }

  }
  cout << "no valid path." << endl;
  HAS::grid_path path {closed, came_from, state,};

  return path;

}

vector<HAS::Node3D> HAS::retrace_path(vector< vector< vector<HAS::Node3D> > > came_from,
                                      vector<double> start,
                                      HAS::Node3D final){

	vector<Node3D> path = {final};
	Node3D current = came_from[theta_to_stack_number(final.theta)]
                            [idx(final.x)][idx(final.y)];

  while( current.x !=  start[0] || current.y != start[1] || current.theta != start[2]){
    // add each node from final node
		path.emplace_back(current);
    current = came_from[theta_to_stack_number(current.theta)]
                       [idx(current.x)][idx(current.y)];
	}
  path.emplace_back(current); //add start node
  //reverse path from start
  std::reverse(path.begin(),path.end());

	return path;

}

vector<HAS::Node3D> HAS::smooth_path(vector<Node3D> path,
                                     double weight, double smooth, double tolerance){

  vector<Node3D> newpath;
  // make a copy of old path into newpath
  newpath = path;

  double change = tolerance;
  while (change >= tolerance){

    change = 0.0;
    for (auto node = path.begin()+1; node != path.end()-1; ++node){
      auto index = std::distance(path.begin(), node);

      double aux = newpath[index].x;
      newpath[index].x = newpath[index].x
                         + weight * (path[index].x - newpath[index].x);
      newpath[index].x = newpath[index].x
                         + smooth * (newpath[index-1].x + newpath[index+1].x
                                     - 2.0* newpath[index].x);
      change += abs(aux - newpath[index].x);

      aux = newpath[index].y;
      newpath[index].y = newpath[index].y
                         + weight * (path[index].y - newpath[index].y);
      newpath[index].y = newpath[index].y
                         + smooth * (newpath[index-1].y + newpath[index+1].y
                                     - 2.0* newpath[index].y);
      change += abs(aux - newpath[index].y);

    }
  }

  return newpath;

}


#include "boundary.hpp"
#include "eigen_ext.hpp"
#include "function.hpp"
#include "integrate_func.hpp"
#include "parameters.hpp"
#include "stiff.hpp"

#include <cassert>
#include <cstdlib>
#include <iostream>

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/QR>
#include <Eigen/Sparse>

// TYPE DEFINITIONS
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;
typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SpMat;

int main(int args, char *argv[]) {

  // PRELIMINARIES
  int max_iter = 1E+2; // 1E+3;
  double tol_min = 1E-10;
  double tol = 1;

  // EXTRACT DATA
  MatI nodes =
      pear::load_csv<MatI>("/Users/hdeplaen/Documents/KULeuven/Project/"
                           "the_winning_pear/imports/node.csv");
  MatI boundary =
      pear::load_csv<MatI>("/Users/hdeplaen/Documents/KULeuven/Project/"
                           "the_winning_pear/imports/boundary.csv");
  Mat points = pear::load_csv<Mat>("/Users/hdeplaen/Documents/KULeuven/Project/"
                                   "the_winning_pear/imports/points.csv");

  int np = points.rows();
  Vec xp(np);
  Vec yp(np);
  xp = points.col(0);
  yp = points.col(1);

  // INITIALIZE MATRICES
  Mat Ku(np, np);
  Mat Kv(np, np);
  Mat int_F(np, np);
  Vec Bu(np);
  Vec Bv(np);
  Mat Kbu(np, np);
  Mat Kbv(np, np);

  pear::stiff(xp, yp, nodes, Ku, Kv);
  int_F = pear::int_func(xp, yp, nodes);
  pear::boundary_vector(xp, yp, boundary, Bu, Bv, Kbu, Kbv);

  // INITIAL SOLUTION
  Vec Cu(np);
  Vec Cv(np);
  Vec sol(2 * np);
  Vec sol_buff(2 * np);

  Eigen::BiCGSTAB<Mat> solver;

  solver.compute(Ku + (pear::Vmu / pear::Kmu) * int_F - Kbu);
  Cu = solver.solve(Bu);

  /* std::cout << Cu << std::endl; */

  solver.compute(Kv - Kbv);
  Cv = solver.solve(pear::rq * (pear::Vmu / pear::Kmu) * int_F * Cu + Bv);

  std::cout << "Initial solution computed" << std::endl << std::endl;

  // NON-LINEAR SOLUTION
  Vec Ru(np);
  Vec Rv(np);
  Mat RudCu(np, np);
  Mat RudCv(np, np);
  Mat RvdCu(np, np);
  Mat RvdCv(np, np);

  Vec F(2 * np);
  Mat JF(2 * np, 2 * np);

  for (int iter = 0; iter < max_iter; iter++) {
    pear::fun_diff(Cu, Cv, Ru, Rv, RudCu, RudCv, RvdCu, RvdCv);

    F.block(0, 0, np, 1) = Ku * Cu + int_F * Ru - Kbu * Cu - Bu;
    F.block(np, 0, np, 1) = Kv * Cv - int_F * Rv - Kbv * Cv - Bv;

    JF.block(0, 0, np, np) = Ku + int_F * RudCu - Kbu;
    JF.block(0, np, np, np) = int_F * RudCv;
    JF.block(np, 0, np, np) = -int_F * RvdCu;
    JF.block(np, np, np, np) = Kv - int_F * RvdCv - Kbv;

    solver.compute(JF);

    sol_buff.block(0, 0, np, 1) = Cu;
    sol_buff.block(np, 0, np, 1) = Cv;

    sol = sol_buff - solver.solve(F);

    tol = (sol - sol_buff).norm();

    std::cout << "Iteration: " << iter + 1 << std::endl;
    std::cout << "Tolerance: " << tol << std::endl << std::endl;

    if (tol < tol_min) {
      break;
    }

    Cu = sol.block(0, 0, np, 1);
    Cv = sol.block(np, 0, np, 1);
  }

  // EXPORTS
  pear::write_csv<Vec>("/Users/hdeplaen/Documents/KULeuven/Project/"
                       "the_winning_pear/exports/cu.csv",
                       Cu);

  pear::write_csv<Vec>("/Users/hdeplaen/Documents/KULeuven/Project/"
                       "the_winning_pear/exports/cv.csv",
                       Cv);
  return 0;
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/ARModel.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <random>
#include <vector>

namespace fluid {
namespace algorithm {

class TransientExtraction
{

  using ARModel = algorithm::ARModel;
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;

public:
  void init(index order, index blockSize, index padSize)
  {
    mModel = ARModel(order);
    prepareStream(blockSize, padSize);
    mInitialized = true;
  }

  void setDetectionParameters(double power, double threshHi, double threshLo,
                              index halfWindow = 7, index hold = 25)
  {
    mDetectPowerFactor = power;
    mDetectThreshHi = threshHi;
    mDetectThreshLo = threshLo;
    mDetectHalfWindow = halfWindow;
    mDetectHold = hold;
  }

  void prepareStream(index blockSize, index padSize)
  {
    mBlockSize = std::max(blockSize, modelOrder());
    mPadSize = std::max(padSize, modelOrder());
    resizeStorage();
  }

  index modelOrder() const { return static_cast<index>(mModel.order()); }
  index blockSize() const { return mBlockSize; }
  index hopSize() const { return mBlockSize - modelOrder(); }
  index padSize() const { return mPadSize; }
  index inputSize() const { return hopSize() + mPadSize; }
  index analysisSize() const { return mBlockSize + mPadSize + mPadSize; }

  const double* getDetect() const { return mDetect.data(); }
  const double* getForwardError() const
  {
    return mForwardError.data() + modelOrder();
  }
  const double* getBackwardError() const
  {
    return mBackwardError.data() + modelOrder();
  }
  const double* getForwardWindowedError() const
  {
    return mForwardWindowedError.data();
  }
  const double* getBackwardWindowedError() const
  {
    return mBackwardWindowedError.data();
  }

  index detect(const double* input, index inSize)
  {
    frame(input, inSize);
    analyze();
    detection();
    return mCount;
  }

  void process(const RealVectorView input, RealVectorView transients,
               RealVectorView residual)
  {
    assert(mInitialized);
    index inSize = input.extent(0);
    frame(input.data(), inSize);
    analyze();
    detection();
    interpolate(transients.data(), residual.data());
  }

  void process(const RealVectorView input, const RealVectorView unknowns,
               RealVectorView transients, RealVectorView residual)
  {
    index inSize = input.extent(0);
    std::copy(unknowns.data(), unknowns.data() + hopSize(), mDetect.data());
    mCount = 0;
    for (index i = 0, size = hopSize(); i < size; i++)
      if (mDetect[asUnsigned(i)] != 0) mCount++;
    frame(input.data(), inSize);
    if (mCount) analyze();
    interpolate(transients.data(), residual.data());
  }

  bool initialized() { return mInitialized; }

private:
  void frame(const double* input, index inSize)
  {
    using namespace std;
    inSize = std::min(inSize, inputSize());
    copy(mInput.data() + hopSize(),
         mInput.data() + modelOrder() + padSize() + blockSize(), mInput.data());
    copy(input, input + inSize,
         mInput.data() + modelOrder() + padSize() + modelOrder());
    fill(mInput.data() + modelOrder() + padSize() + modelOrder() + inSize,
         mInput.data() + modelOrder() + analysisSize(), 0.0);
  }

  void analyze()
  {
    mModel.setMinVariance(0.0000001);
    mModel.estimate(mInput.data() + modelOrder(), analysisSize());
  }

  void detection()
  {
    const double* input = mInput.data() + modelOrder() + padSize();

    // Forward and backward error
    const double normFactor = 1.0 / sqrt(mModel.variance());
    errorCalculation<&ARModel::forwardErrorArray>(
        mForwardError.data(), input, blockSize() + mDetectHalfWindow + 1,
        normFactor);
    errorCalculation<&ARModel::backwardErrorArray>(
        mBackwardError.data(), input, blockSize() + mDetectHalfWindow + 1,
        normFactor);

    // Window error functions (brute force convolution)
    windowError(mForwardWindowedError.data(),
                mForwardError.data() + modelOrder(), hopSize());
    windowError(mBackwardWindowedError.data(),
                mBackwardError.data() + modelOrder(), hopSize());

    // Detection
    index        count = 0;
    const double hiThresh = mDetectThreshHi;
    const double loThresh = mDetectThreshLo;
    const index  offHold = mDetectHold;

    bool click = false;

    for (index i = 0, size = hopSize(); i < size; i++)
    {
      if (!click && (mBackwardWindowedError[asUnsigned(i)] > loThresh) &&
          (mForwardWindowedError[asUnsigned(i)] > hiThresh))
      { click = true; }
      else if (click && (mBackwardWindowedError[asUnsigned(i)] < loThresh))
      {
        click = false;

        for (index j = i; (j < i + offHold) && (j < size); j++)
        {
          if (mBackwardWindowedError[asUnsigned(j)] > loThresh)
          {
            click = true;
            break;
          }
        }
      }

      if (click) count++;

      mDetect[asUnsigned(i)] = click ? 1.0 : 0.0;
    }

    // Count Validation
    if (count > (hopSize() / 2))
    {
      std::fill(mDetect.data(), mDetect.data() + hopSize(), 0.0);
      count = 0;
    }

    // RMS validation
    /*
    const double frameRMS = calcStat<&Descriptors::RMS>(input, blockSize());

    for (index i = 0, size = hopSize(); i < size;)
    {
      for (; i < size; i++)
          if (mDetect[i])
            break;

      index beg = i;

      for (; i < size; i++)
        if (!mDetect[i])
          break;

      if (i <= beg)
        continue;

      const double clickRMS = calcStat<&Descriptors::RMS>(input + modelOrder() +
    beg, i - beg);

      if ((clickRMS / frameRMS) < 0.001)
      {
        count -= (i - beg);
        std::fill(mDetect.data() + beg, mDetect.data() + i, 0.0);
      }
    }
    */
    mCount = count;
  }

  template <double Method(const RealVectorView&)>
  double calcStat(const double* input, index size)
  {
    RealVectorView view(const_cast<double*>(input), 0, size);
    return Method(view);
  }

  void interpolate(double* transients, double* residual)
  {
    const double* input = mInput.data() + padSize() + modelOrder();
    const double* parameters = mModel.getParameters();
    index         order = modelOrder();
    index         size = blockSize();

    if (!mCount)
    {
      std::copy(input + order, input + order + hopSize(), residual);
      std::fill_n(transients, hopSize(), 0.0);
      return;
    }

    // Declare matrices
    MatrixXd A = MatrixXd::Zero(size - order, size);
    MatrixXd U = MatrixXd::Zero(size, mCount);
    MatrixXd K = MatrixXd::Zero(size, size - mCount);
    VectorXd xK(size - mCount);

    // Form data
    for (index i = 0; i < size - order; i++)
    {
      for (index j = 0; j < order; j++)
        A(i, j + i) = -parameters[order - (j + 1)];

      A(i, order + i) = 1.0;
    }

    for (index i = 0, uCount = 0, kCount = 0; i < size; i++)
    {
      if (i >= order && mDetect[asUnsigned(i - order)] != 0)
        U(i, uCount++) = 1.0;
      else
      {
        K(i, kCount) = 1.0;
        xK[kCount++] = input[i];
      }
    }

    // Solve
    MatrixXd Au = A * U;
    MatrixXd M = -(Au.transpose() * Au);
    MatrixXd u = M.fullPivLu().solve(Au.transpose() * (A * K) * xK);

    // Write the output
    for (index i = 0, uCount = 0; i < (size - order); i++)
    {
      if (mDetect[asUnsigned(i)] != 0)
        residual[i] = u(uCount++);
      else
        residual[i] = input[i + order];
    }

    if (mRefine) refine(residual, size, Au, u);

    for (index i = 0; i < (size - order); i++)
      transients[i] = input[i + order] - residual[i];

    // Copy the residual indexo the correct place
    std::copy(residual, residual + (size - order),
              mInput.data() + padSize() + order + order);
  }

  void refine(double* io, index size, Eigen::MatrixXd& Au, Eigen::MatrixXd& ls)
  {
    const double energy = mModel.variance() * mCount;
    double       energyLS = 0.0;
    index        order = modelOrder();

    for (index i = 0; i < (size - order); i++)
    {
      if (mDetect[asUnsigned(i)] != 0)
      {
        const double error = mModel.forwardError(io + i);
        energyLS += error * error;
      }
    }

    if (energyLS < energy)
    {
      // Create the square matrix and solve
      Eigen::LLT<Eigen::MatrixXd> M(Au.transpose() *
                                    Au); // Cholesky decomposition

      Eigen::VectorXd u(mCount);

      Eigen::MatrixXd correction = M.solve(u) + ls;

      // Write the output
      for (index i = 0, uCount = 0; i < (size - order); i++)
      {
        if (mDetect[asUnsigned(i)] != 0) io[asUnsigned(i)] = u(uCount++);
      }
    }
  }

  double randomSampling(Eigen::VectorXd& output, double variance)
  {
    std::normal_distribution<double> gaussian(0.0, sqrt(variance));
    double                           sum = 0.0;

    for (index i = 0; i < output.size(); i++)
    {
      output[i] = gaussian(mRandomGenerator);
      sum += output[i] * output[i];
    }

    return sum;
  }

  template <void (ARModel::*Method)(double*, const double*, index)>
  void errorCalculation(double* error, const double* input, index size,
                        double normFactor)
  {
    (mModel.*Method)(error, input, size);

    // Take absolutes and normalise
    for (index i = 0; i < size; i++)
      error[i] = std::fabs(error[i]) * normFactor;
  }

  // Triangle window
  double calcWindow(double norm) { return std::min(norm, 1.0 - norm); }

  void windowError(double* errorWindowed, const double* error, index size)
  {
    const index  windowSize = mDetectHalfWindow * 2 + 1;
    const index  windowOffset = mDetectHalfWindow;
    const double powFactor = mDetectPowerFactor;

    // Calculate window normalisation factor
    double windowNormFactor = 0.0;

    for (index j = 0; j < windowSize; j++)
      windowNormFactor += calcWindow((double) j / windowSize);

    windowNormFactor = 1.0 / windowNormFactor;

    // Do window processing
    for (index i = 0; i < size; i++)
    {
      double windowed = 0.0;

      for (index j = 1; j < windowSize; j++)
      {
        const double value = pow(fabs(error[i - windowOffset + j]), powFactor);
        windowed += value * calcWindow((double) j / windowSize);
        ;
      }

      errorWindowed[i] = pow((windowed * windowNormFactor), 1.0 / powFactor);
    }
  }

  void resizeStorage()
  {
    mInput.resize(asUnsigned(analysisSize() + modelOrder()), 0.0);
    mDetect.resize(asUnsigned(hopSize()), 0.0);
    mForwardError.resize(asUnsigned(mBlockSize + modelOrder()), 0.0);
    mBackwardError.resize(asUnsigned(mBlockSize + modelOrder()), 0.0);
    mForwardWindowedError.resize(asUnsigned(hopSize()), 0.0);
    mBackwardWindowedError.resize(asUnsigned(hopSize()), 0.0);
  }

  ARModel mModel{20};

  std::mt19937_64 mRandomGenerator{std::random_device()()};

  index  mBlockSize{0};
  index  mPadSize{0};
  index  mCount{0};
  bool   mRefine{false};
  index  mDetectHalfWindow{1};
  index  mDetectHold{25};
  double mDetectPowerFactor{1.4};
  double mDetectThreshHi{1.5};
  double mDetectThreshLo{3.0};

  std::vector<double> mInput;
  std::vector<double> mDetect;
  std::vector<double> mForwardError;
  std::vector<double> mBackwardError;
  std::vector<double> mForwardWindowedError;
  std::vector<double> mBackwardWindowedError;
  bool                mInitialized{false};
};

} // namespace algorithm
} // namespace fluid


#include <boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54_classic.hpp>


#include <geometry_msgs/PoseStamped.h>

#include <OgreMovableObject.h>
#include <OgreSceneManager.h>

#include <rviz/display_context.h>
#include <rviz/load_resource.h>
#include <rviz/geometry.h>
#include <rviz/viewport_mouse_event.h>
#include <rviz/selection/selection_manager.h>

#include <rviz/properties/color_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/string_property.h>

#include <rviz_tool_cursor/rviz_tool_cursor.h>

#include <Eigen/Dense>

namespace
{

Eigen::Matrix3f createMatrix(const Eigen::Vector3f& norm)
{
  Eigen::Matrix3f mat (Eigen::Matrix3f::Identity());
  mat.col(2) = norm;

  // Create plane from point normal
  Eigen::Hyperplane<float, 3> plane (norm, Eigen::Vector3f(0, 0, 0));

  // If the normal and global x-axis are not closely aligned
  if (std::abs(norm.dot(Eigen::Vector3f::UnitX())) < 0.90f)
  {
    // Project the global x-axis onto the plane to generate the x-axis
    Eigen::Vector3f x_axis = plane.projection(Eigen::Vector3f::UnitX()).normalized();
    mat.col(0) = x_axis;
    mat.col(1) = norm.cross(x_axis);
  }
  else
  {
    // Project the global y-axis onto the plane to generate the y-axis
    Eigen::Vector3f y_axis = plane.projection(Eigen::Vector3f::UnitY()).normalized();
    mat.col(0) = y_axis.cross(norm);
    mat.col(1) = y_axis;
  }

  return mat;
}

Ogre::Quaternion estimateNormal(const std::vector<Ogre::Vector3>& points,
                                const Ogre::Vector3& camera_norm)
{
  Eigen::MatrixXf data;
  data.resize(points.size(), 3);

  for(std::size_t i = 0; i < points.size(); ++i)
  {
    data.row(i) = Eigen::Map<const Eigen::Vector3f>(points[i].ptr());
  }

  Eigen::MatrixXf centered = data.rowwise() - data.colwise().mean();

  // Use principal component analysis to the get eigenvectors
  Eigen::MatrixXf cov = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov);

  // Get the eigenvector associated with the smallest eigenvalue
  // (should be Z-axis, assuming points are relatively planar)
  Eigen::Vector3f norm = eig.eigenvectors().col(0);
  norm.normalize();

  Eigen::Vector3f camera_normal;
  camera_normal << camera_norm.x, camera_norm.y, camera_norm.z;
  camera_normal.normalize();

  if(norm.dot(camera_normal) < 0)
  {
    norm *= -1;
  }

  // Create an arbitrary orientation matrix with the normal being in the direction of the smallest eigenvector
  Eigen::Matrix3f mat = createMatrix(norm);

  Eigen::Quaternionf q(mat); //Eigen::AngleAxisf(0.0, evecs.col(2)));
  Ogre::Quaternion out;
  out.w = q.w();
  out.x = q.x();
  out.y = q.y();
  out.z = q.z();

  return out;
}

} // namepsace anonymous

namespace rviz_tool_cursor
{

ToolCursor::ToolCursor()
  : rviz::Tool()
{
  shortcut_key_ = 'c';

  topic_property_ = new rviz::StringProperty("Topic", "/selection_point",
                                             "The topic on which to publish points",
                                             getPropertyContainer(), SLOT(updateTopic()), this);

  patch_size_property_ = new rviz::IntProperty("Patch Size", 10,
                                               "The number of pixels with which to estimate the surface normal",
                                               getPropertyContainer());

  color_property_ = new rviz::ColorProperty("Color", QColor(255, 255, 255),
                                            "The color of the tool visualization",
                                            getPropertyContainer(), SLOT(updateToolVisualization()), this);

  updateTopic();
}

ToolCursor::~ToolCursor()
{

}

void ToolCursor::onInitialize()
{
  // Initialize the scene node
  cursor_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();

  // Create the visual tool object
  Ogre::MovableObject* obj = createToolVisualization();

  // Attach the tool visualization to the scene
  cursor_node_->attachObject(obj);
  cursor_node_->setVisible(false);

  // Set the cursors
  hit_cursor_ = cursor_;
  std_cursor_ = rviz::getDefaultCursor();
}

void ToolCursor::activate()
{
  cursor_node_->setVisible(true);
}

void ToolCursor::deactivate()
{
  cursor_node_->setVisible(false);
}

void ToolCursor::updateTopic()
{
  pub_ = nh_.advertise<geometry_msgs::PoseStamped>(topic_property_->getStdString(), 1, true);
}

int ToolCursor::processMouseEvent(rviz::ViewportMouseEvent& event)
{
  // Get the 3D point in space indicated by the mouse and a patch of points around it
  // with which to estimate the surface normal
  Ogre::Vector3 position;
  std::vector<Ogre::Vector3> points;

  const unsigned patch_size = static_cast<unsigned>(patch_size_property_->getInt());

  // Set the visibility of this node off so the selection manager won't choose a point on our cursor mesh in the point and patch
  cursor_node_->setVisible(false);

  bool got_point = context_->getSelectionManager()->get3DPoint(event.viewport, event.x, event.y, position);
  bool got_patch = context_->getSelectionManager()->get3DPatch(event.viewport, event.x, event.y, patch_size, patch_size, true, points);

  // Revisualize the cursor node
  cursor_node_->setVisible(true);

  if(got_point && got_patch && points.size() > 3)
  {
    // Set the cursor
    rviz::Tool::setCursor(hit_cursor_);

    // Estimate the surface normal from the patch of points
    Ogre::Quaternion q = estimateNormal(points, event.viewport->getCamera()->getDirection());
    cursor_node_->setOrientation(q);
    cursor_node_->setPosition(position);

    if(event.leftUp())
    {
      // Publish a point message upon release of the left mouse button
      geometry_msgs::PoseStamped msg;
      msg.header.frame_id = context_->getFixedFrame().toStdString();
      msg.header.stamp = ros::Time::now();

      msg.pose.position.x = static_cast<double>(position.x);
      msg.pose.position.y = static_cast<double>(position.y);
      msg.pose.position.z = static_cast<double>(position.z);

      msg.pose.orientation.w = static_cast<double>(q.w);
      msg.pose.orientation.x = static_cast<double>(q.x);
      msg.pose.orientation.y = static_cast<double>(q.y);
      msg.pose.orientation.z = static_cast<double>(q.z);

      pub_.publish(msg);
    }
  }
  else
  {
    // Set the standard cursor
    rviz::Tool::setCursor(std_cursor_);

    // Project the tool visualization onto the ground
    Ogre::Plane plane (Ogre::Vector3::UNIT_Z, 0.0f);
    rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, position);
    cursor_node_->setOrientation(1.0f, 0.0f, 0.0f, 0.0f);
    cursor_node_->setPosition(position);
  }

  return rviz::Tool::Render;
}

} // namespace rviz_tool_cursor


/* test_weibull.cpp
 *
 * Copyright Steven Watanabe 2010
 * 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)
 *
 * $Id: test_weibull.cpp 71018 2011-04-05 21:27:52Z steven_watanabe $
 *
 */

#include <boost/random/weibull_distribution.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/math/distributions/weibull.hpp>

#define BOOST_RANDOM_DISTRIBUTION boost::random::weibull_distribution<>
#define BOOST_RANDOM_DISTRIBUTION_NAME weibull
#define BOOST_MATH_DISTRIBUTION boost::math::weibull
#define BOOST_RANDOM_ARG1_TYPE double
#define BOOST_RANDOM_ARG1_NAME a
#define BOOST_RANDOM_ARG1_DEFAULT 1000.0
#define BOOST_RANDOM_ARG1_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)
#define BOOST_RANDOM_ARG2_TYPE double
#define BOOST_RANDOM_ARG2_NAME b
#define BOOST_RANDOM_ARG2_DEFAULT 1000.0
#define BOOST_RANDOM_ARG2_DISTRIBUTION(n) boost::uniform_real<>(0.00001, n)

#include "test_real_distribution.ipp"


/*
 * Copyright (c) 2011 Seiya Tokui <beam.web@gmail.com>
 * Copyright (c) 2014 Burkhard Ritter <burkhard@ualberta.ca>
 * This code is distributed under the MIT license.
 *
 * Performance test for Arpaca's eigensolver.
 *
 * To compile this program:
 * g++ \
 *    -std=c++11 \
 *    -I [/path/to/eigen] \
 *    -O3 \
 *    -DNDEBUG \
 *    performance_main.cpp \
 *    -L [/path/to/libarpack.a] \
 *    -larpack \
 *    -o performance_main
 *
 * This assumes that arpaca.hpp is in the same directory.
 */

#include <iostream>
#include <vector>
#include <random>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include "arpaca.hpp"

namespace chrono = std::chrono;

typedef Eigen::Triplet<double> T;
typedef Eigen::SparseMatrix<double, Eigen::RowMajor> SMatrix;

template<typename RandomNumberGenerator>
SMatrix
MakeSparseSymmetricRandomMatrix(int n, int k, RandomNumberGenerator& rnd)
{
    std::uniform_int_distribution<int> r_int(0,n-1);
    std::normal_distribution<double> r_real;

    std::vector<T> ts;
    ts.reserve(n*k);
    for (int l=0; l<n*k; l++)
    {
        // If we randomly create two or more triplets for the same matrix
        // element, the values of all triplets will be summed...
        int i = r_int(rnd);
        int j = r_int(rnd);
        double v = r_real(rnd);
        ts.push_back(T(i,j,v));
    }

    SMatrix mat(n, n);
    mat.setFromTriplets(ts.begin(),ts.end());
    return mat.selfadjointView<Eigen::Upper>();
}

arpaca::EigenvalueType
GetEigenvalueType(const std::string& name)
{
    if (name == "LA")
        return arpaca::ALGEBRAIC_LARGEST;
    else if (name == "SA")
        return arpaca::ALGEBRAIC_SMALLEST;
    else if (name == "BE")
        return arpaca::ALGEBRAIC_BOTH_END;
    else if (name == "LM")
        return arpaca::MAGNITUDE_LARGEST;
    else if (name == "SM")
        return arpaca::MAGNITUDE_SMALLEST;
    throw std::invalid_argument("invalid eigenvalue type");
}

int main(int argc, char** argv)
{
    if (argc != 5) {
        std::cerr << "usage: " << argv[0]
                  << " <dimension>"
                  << " <# of non-zero values in each row>"
                  << " <# of eigenvectors>"
                  << " <type of eigenvalues>"
                  << std::endl;
        std::cerr << "\ttype of eigenvalues: LA SA BE LM SM" << std::endl;
        return 1;
    }

    const int n = std::atoi(argv[1]),
    k = std::atoi(argv[2]),
    r = std::atoi(argv[3]);
    const arpaca::EigenvalueType type = GetEigenvalueType(argv[4]);

    std::cerr << "Making matrix" << std::endl;
    std::mt19937 generator(42);
    SMatrix X = MakeSparseSymmetricRandomMatrix(n, k, generator);

    std::cerr << "Start performance test" << std::endl;
    chrono::steady_clock::time_point begin = chrono::steady_clock::now();

    arpaca::SymmetricEigenSolver<double> solver = arpaca::Solve(X, r, type);

    chrono::steady_clock::time_point end = chrono::steady_clock::now();

    chrono::duration<double> duration_ =
        chrono::duration_cast<chrono::duration<double>>(end - begin);
    const double duration = duration_.count();

    std::cout << "        DIMENSION: " << X.rows() << std::endl;
    std::cout << "         NONZEROS: " << X.nonZeros() << std::endl;
    std::cout << "         DURATION: "
              << duration << " SEC." << std::endl;
    std::cout << "             ITER: "
              << solver.num_actual_iterations() << std::endl;
    std::cout << "CONVERGED EIGVALS: "
              << solver.num_converged_eigenvalues() << std::endl;
    std::cout << "             INFO: " << solver.GetInfo() << std::endl;
}


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>

#include <limits>
#include <vector>

namespace gil = boost::gil;

const std::ptrdiff_t size = 256;

void line_bresenham(std::ptrdiff_t width, std::ptrdiff_t height, const std::string& output_name)
{
    const auto rasterizer = gil::bresenham_line_rasterizer{};
    std::vector<gil::point_t> line_points(rasterizer.point_count(width, height));

    gil::gray8_image_t image(size, size);
    auto view = gil::view(image);

    rasterizer({0, 0}, {width - 1, height - 1}, line_points.begin());
    for (const auto& point : line_points)
    {
        view(point) = std::numeric_limits<gil::uint8_t>::max();
    }

    gil::write_view(output_name, view, gil::png_tag{});
}

int main()
{
    line_bresenham(256, 256, "line-bresenham-256-256.png");
    line_bresenham(256, 128, "line-bresenham-256-128.png");
    line_bresenham(256, 1, "line-bresenham-256-1.png");
    line_bresenham(1, 256, "line-bresenham-1-256.png");
}


//! \file examples/Arrangement_on_surface_2/bgl_primal_adapter.cpp
// Adapting an arrangement to a BGL graph.

#include <vector>

#include <CGAL/config.h>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/vector_property_map.hpp>

#include <CGAL/graph_traits_Arrangement_2.h>
#include <CGAL/Arr_vertex_index_map.h>
#include <CGAL/property_map.h>

#include "arr_exact_construction_segments.h"
#include "Edge_length.h"

typedef CGAL::Arr_vertex_index_map<Arrangement>  Vertex_index_map;
typedef Edge_length<Arrangement>                 My_edge_length;

int main() {
  // Construct an arrangement of seven intersecting line segments.
  // We keep a handle for the vertex v0 that corresponds to the point (1,1).
  Point p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
  Arrangement arr;
  Segment s(p1, p6);
  Arrangement::Halfedge_handle e = insert_non_intersecting_curve(arr, s);
  Arrangement::Vertex_handle v0 = e->source();
  insert(arr, Segment(p1, p4));  insert(arr, Segment(p2, p6));
  insert(arr, Segment(p3, p7));  insert(arr, Segment(p3, p5));
  insert(arr, Segment(p6, p7));  insert(arr, Segment(p4, p7));

  // Create a mapping of the arrangement vertices to indices.
  Vertex_index_map index_map(arr);

  // Create a property map based on std::vector to keep the result distances.
  boost::vector_property_map<Number_type, Vertex_index_map>
    dist_map(static_cast<unsigned int>(arr.number_of_vertices()), index_map);

  // Perform Dijkstra's algorithm from the vertex v0.
  My_edge_length edge_length;
  boost::dijkstra_shortest_paths(arr, v0, boost::vertex_index_map(index_map).
                                 weight_map(edge_length).distance_map(dist_map).
                                 distance_zero(Number_type(0)).
                                 distance_inf(Number_type(1000)));

  // Print the distance of each vertex from v0.
  std::cout << "The graph distances of the arrangement vertices from ("
            << v0->point() << ") :\n";
  for (auto vit = arr.vertices_begin(); vit != arr.vertices_end(); ++vit)
    std::cout << "(" << vit->point() << ") at distance "
              << CGAL::to_double(dist_map[vit]) << std::endl;

  return 0;
}


//
// Created by Alex Beccaro on 25/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <primes.hpp>
#include <numeric>
#include "../../src/problems/101-150/133/problem133.hpp"

BOOST_AUTO_TEST_SUITE( Problem133 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem133::solve(100);
        std::vector<uint32_t> primes = primes::primes_up_to<uint32_t>(100);
        auto solution = std::accumulate(primes.begin(), primes.end(), 0);
        BOOST_CHECK_EQUAL(res, solution - 142);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem133::solve();
        BOOST_CHECK_EQUAL(res, 453647705);
    }

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "test_centroid"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <periortree/boundary_condition.hpp>
#include <periortree/centroid.hpp>
#include <test/point_type.hpp>
#include <test/aabb_type.hpp>

BOOST_AUTO_TEST_CASE(test_centroid_unlimited)
{
    {
        const perior::test::xyz  l(0., 0., 0.);
        const perior::test::xyz  u(10., 10., 10.);
        const perior::test::aabb box(l, u);
        const perior::unlimited_boundary<perior::test::xyz> boundary;

        const perior::test::xyz c = perior::centroid(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(c.x, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.y, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.z, 5.0, 1e-12);
    }
}

BOOST_AUTO_TEST_CASE(test_centroid_cubic_periodic)
{
    const perior::test::xyz lw(0., 0., 0.);
    const perior::test::xyz up(10., 10., 10.);
    const perior::cubic_periodic_boundary<perior::test::xyz> boundary(lw, up);

    {
        const perior::test::xyz  l(3.0, 3.0, 3.0);
        const perior::test::xyz  u(7.0, 7.0, 7.0);
        const perior::test::aabb box(l, u);

        const perior::test::xyz c = perior::centroid(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(c.x, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.y, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.z, 5.0, 1e-12);
    }

    {
        const perior::test::xyz  l(7.0, 7.0, 7.0);
        const perior::test::xyz  u(3.0, 3.0, 3.0);
        const perior::test::aabb box(l, u);
        const perior::test::xyz c = perior::centroid(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(c.x, 10.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.y, 10.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.z, 10.0, 1e-12);
    }

    {
        const perior::test::xyz l(8.0, 4.0, 8.0);
        const perior::test::xyz u(2.0, 8.0, 2.0);
        const perior::test::aabb box(l, u);
        const perior::test::xyz c = perior::centroid(box, boundary);
        BOOST_CHECK_CLOSE_FRACTION(c.x, 10.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.y,  6.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(c.z, 10.0, 1e-12);
    }
}


#include "Util.h"

#include <Eigen/SVD>

#include <iostream>

using namespace std;

bool Ubpa::PDIPM_QP::CheckSettingsValid() const {
	//float gamma{ 1.05f }; // t multiplier
	//float epsilon_feas{ 0.001f }; // residule error
	//float epsilon{ 0.001f }; // gap error
	//float beta{ 0.99f }; // alpha shrink multiplier
	//float tau{ 0.02f }; // backtracking

	if (gamma <= 1.f)
		return false;

	if (epsilon_feas <= 0 || epsilon <= 0)
		return false;

	if (beta <= 0 || beta >= 1)
		return false;

	if (tau <= 0 || tau >= 1)
		return false;
#ifndef NDEBUG
	else {
		if (tau < 0.01 || tau > 0.1)
			assert("tau is typically chosen in the range 0.01 to 0.1");
	}
#endif // !NDEBUG

	return true;
}

bool Ubpa::PDIPM_QP::SetProblem(
	Eigen::MatrixXf P, // n x n
	Eigen::VectorXf q, // n x 1
	float           r,

	Eigen::MatrixXf G, // m x n
	Eigen::VectorXf h, // m x 1

	Eigen::MatrixXf A, // p x n
	Eigen::VectorXf b  // p x 1
) {
	size_t n = P.rows();
	size_t m = G.rows();
	size_t p = A.rows();

	if (P.cols() != n
		|| q.size() != n
		|| G.cols() != n
		|| h.size() != m
		|| A.cols() != n
		|| b.size() != p)
	{
		return false;
	}

	this->P = std::move(P);
	this->q = std::move(q);
	this->r = r;

	this->G = std::move(G);
	this->h = std::move(h);

	this->A = std::move(A);
	this->b = std::move(b);

	this->n = n;
	this->m = m;
	this->p = p;

	return true;
}

void Ubpa::PDIPM_QP::Init() {
	// f(x) < 0
	// => Gx - h = -1
	// => x = pinv(G) * (h - 1)

	// presudo inverse of G
	Eigen::JacobiSVD<Eigen::MatrixXf> G_svd(G, Eigen::ComputeFullU | Eigen::ComputeFullV);
	const auto& SV = G_svd.singularValues();
	Eigen::MatrixXf ISV;
	ISV.setZero(G.cols(), G.rows());
	for (size_t i = 0; i < SV.size(); i++) {
		if (SV(i) > 0.000001f)
			ISV(i, i) = 1.f / SV(i);
	}
	Eigen::MatrixXf pinv_G = G_svd.matrixV() * ISV * G_svd.matrixU().transpose();

	x = pinv_G * (h - Eigen::VectorXf::Ones(m));

	lambda.setOnes(m); // > 0
	nu.setZero(p);

	Eigen::VectorXf f_x = -Eigen::VectorXf::Ones(m); // G * x - h

	eta = m; // -f_x.dot(lambda);
	float t = gamma; /* *m / eta */;

	Eigen::MatrixXf diag_lambda;
	diag_lambda.setZero(m, m);
	for (size_t i = 0; i < m; i++)
		diag_lambda(i, i) = lambda(i);

	r_dual = P * x + q + G.transpose() * lambda; /* + A.transpose() * nu */
	r_cent = -diag_lambda * f_x - (1.f / t) * Eigen::VectorXf::Ones(m);
	r_pri = A * x - b;
}

void Ubpa::PDIPM_QP::Iterate() {
	// [[ 1. compute Newton step ]]

	Eigen::MatrixXf M(n + m + p, n + m + p);
	M.setZero();

	Eigen::VectorXf f_x = G * x - h;

	// (0, 0) - (n-1, n-1)
	// P
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < n; j++)
			M(i, j) = P(i, j);
	}

	// (0, n) - (n-1, n+m-1)
	// G^T
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < m; j++)
			M(i, j + n) = G(j, i);
	}

	// (0, n+m) - (n-1, n+m+p-1)
	// A^T
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < p; j++)
			M(i, j + n + m) = A(j, i);
	}

	// (n, 0) - (n+m-1, n-1)
	// - diag(lambda)G

	Eigen::MatrixXf diag_lambda;
	diag_lambda.setZero(m, m);
	for (size_t i = 0; i < m; i++)
		diag_lambda(i, i) = lambda(i);
	Eigen::MatrixXf diag_lambda_G = diag_lambda * G;
	for (size_t i = 0; i < m; i++) {
		for (size_t j = 0; j < n; j++)
			M(i + n, j) = -diag_lambda_G(i, j);
	}

	// (n, n) - (n+m-1, n+m-1)
	// - diag(f(x))
	for (size_t i = 0; i < m; i++)
		M(i + n, i + n) = -f_x(i);

	// (n+m, 0) - (n+m+p-1, n-1)
	// A
	for (size_t i = 0; i < p; i++) {
		for (size_t j = 0; j < n; j++)
			M(i + n + m, j) = A(i, j);
	}

	Eigen::VectorXf r(n + m + p);
	for (size_t i = 0; i < n; i++)
		r(i, 0) = r_dual(i);
	for (size_t i = 0; i < m; i++)
		r(i + n, 0) = r_cent(i);
	for (size_t i = 0; i < p; i++)
		r(i + n + m, 0) = r_pri(i);

	// y = (x, lambda, nu)
	// (n + m + p) x 1
	Eigen::VectorXf delta_y = M.colPivHouseholderQr().solve(-r);

	Eigen::VectorXf delta_x(n);
	Eigen::VectorXf delta_lambda(m);
	Eigen::VectorXf delta_nu(p);

	for (size_t i = 0; i < n; i++)
		delta_x(i, 0) = delta_y(i);
	for (size_t i = 0; i < m; i++)
		delta_lambda(i, 0) = delta_y(i + n);
	for (size_t i = 0; i < p; i++)
		delta_nu(i, 0) = delta_y(i + n + m);

	// [[ 2. backtracking line search ]]

	float alpha_max = 1.f;
	for (size_t i = 0; i < m; i++) {
		if (delta_lambda(i) < 0) {
			float cur = -lambda(i) / delta_lambda(i);
			if (cur < alpha_max)
				alpha_max = cur;
		}
	}

	float alpha = 0.99f * alpha_max;
	bool flag1 = false;
	bool flag2 = false;

	while (!flag1) {
		Eigen::VectorXf x_plus = x + alpha * delta_x;
		Eigen::VectorXf f_x_plus = G * x_plus - h;
		for (size_t i = 0; i < m; i++) {
			if (f_x_plus(i) >= 0) {
				alpha *= beta;
				break;
			}
			if (i == m - 1)
				flag1 = true;
		}
	}

	float rt_norm = Get_residule();

	while (!flag2) {
		Eigen::VectorXf x_plus = x + alpha * delta_x;
		Eigen::VectorXf lambda_plus = lambda + alpha * delta_lambda;
		Eigen::VectorXf nu_plus = nu + alpha * delta_nu;

		Eigen::MatrixXf diag_lambda_plus;
		diag_lambda_plus.setZero(m, m);
		for (size_t i = 0; i < m; i++)
			diag_lambda_plus(i, i) = lambda_plus(i);

		Eigen::VectorXf f_x_plus = G * x_plus - h;

		eta = -f_x_plus.dot(lambda_plus);
		float t = gamma * m / eta;

		r_dual = P * x_plus + q + G.transpose() * lambda_plus + A.transpose() * nu_plus;
		r_cent = -diag_lambda_plus * f_x_plus - (1.f / t) * Eigen::VectorXf::Ones(m);
		r_pri = A * x_plus - b;

		float rt_norm_plus = Get_residule();

		if (rt_norm_plus <= (1 - tau * alpha) * rt_norm)
			flag2 = true;
		else
			alpha *= beta;
	}

	// [[ 3. update ]]
	// x, lambda, nu

	x += alpha * delta_x;
	lambda += alpha * delta_lambda;
	nu += alpha * delta_nu;
}

bool Ubpa::PDIPM_QP::IsStoppable() {
	if (r_dual.norm() > epsilon_feas
		|| r_pri.norm() > epsilon_feas
		|| eta > epsilon)
	{
		return false;
	}

	return true;
}

float Ubpa::PDIPM_QP::Get_residule() const {
	return std::sqrt(
		r_dual.squaredNorm()
		+ r_cent.squaredNorm()
		+ r_pri.squaredNorm());
}


#ifndef CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H
#define CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H

#include <random>
#include <cassert>

#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <Eigen/Dense>

#include <cannon/physics/rk4_integrator.hpp>
#include <cannon/physics/systems/system.hpp>
#include <cannon/graphics/geometry/plane.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

namespace oc = ompl::control;
namespace ob = ompl::base;

namespace cannon {
  namespace physics {
    namespace systems {

      struct DynamicCarSystem : System {
        DynamicCarSystem(double l = 1.0) : l_(l) {}

        virtual void operator()(const VectorXd& s, VectorXd& dsdt, const double /*t*/) override {
          double th = s[2];
          double v = s[3];
          double dth = s[4];
          double ua = s[5];
          double uth = s[6];

          // From http://planning.cs.uiuc.edu/node658.html
          dsdt.resize(7);
          dsdt[0] = v * std::cos(th);
          dsdt[1] = v * std::sin(th);
          dsdt[2] = (v / l_) * std::tan(dth);
          dsdt[3] = ua;
          dsdt[4] = uth;
          dsdt[5] = 0.0;
          dsdt[6] = 0.0;
        }

        virtual void ompl_ode_adaptor(const oc::ODESolver::StateType& q,
            const oc::Control* control, oc::ODESolver::StateType& qdot) override {

          const double ua = control->as<oc::RealVectorControlSpace::ControlType>()->values[0];
          const double uth = control->as<oc::RealVectorControlSpace::ControlType>()->values[1];

          VectorXd s(7);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = q[2];
          s[3] = q[3];
          s[4] = q[4];
          s[5] = ua;
          s[6] = uth;
          VectorXd dsdt(7);

          (*this)(s, dsdt, 0.0);

          qdot.resize(q.size(), 0);
          for (unsigned int i = 0; i < q.size(); i++) {
            qdot[i] = dsdt[i];
          }
        }

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          assert(x.size() == 5);

          double th = x[2];
          double v = x[3];
          double dth = x[4];

          // TODO Don't hardcode timestep at some point
          double timestep = 0.01;

          MatrixXd A(5, 5);
          A << 1, 0, -v * std::sin(th) * timestep, std::cos(th) * timestep, 0,
               0, 1, v * std::cos(th) * timestep, std::sin(th) * timestep, 0,
               0, 0, 1, (tan(dth) / l_) * timestep, (v / l_) * (1.0 / (cos(dth) * cos(dth))) * timestep,
               0, 0, 0, 1, 0,
               0, 0, 0, 0, 1;

          MatrixXd B(5, 2);
          B << 0, 0,
               0, 0,
               0, 0,
               timestep, 0,
               0, timestep;

          VectorXd c(5);
          c << x[0] + v * std::cos(th) * timestep,
               x[1] + v * std::cos(th) * timestep,
               th + (v / l_) * tan(dth) * timestep,
               v,
               dth;

          // We linearize around u = 0, so no additional term is subtracted from c
          return std::make_tuple(A, B, c - A * x);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A, Ref<MatrixXd> B) override {
          double c = cos(q[2]), s = sin(q[2]);
          A = 1e-3 * Eigen::MatrixXd::Identity(5, 5);
          A(0, 2) = -q[3] * s;
          A(0, 3) = c;
          A(1, 2) = q[3] * c;
          A(1, 3) = s;
          A(2, 3) = (1.0 / l_) * tan(q[4]);
          A(2, 4) = q[3] * (1.0 / l_) / (cos(q[4]) * cos(q[4]));

          B = Eigen::MatrixXd::Zero(5, 2);
          B(3, 0) = B(4, 1) = 1.;
        }

        static void ompl_post_integration(const ob::State* /*state*/, const
            oc::Control* /*control*/, const double /*duration*/, ob::State *result) {

          ob::SO2StateSpace SO2;
          SO2.enforceBounds(result->as<ob::CompoundStateSpace::StateType>()
                                ->as<ob::SE2StateSpace::StateType>(0)
                                ->as<ob::SO2StateSpace::StateType>(1));
        }

        // Parameters
        double l_;
      };

      class DynamicCar {
        public:
          DynamicCar() = delete;

          DynamicCar(VectorXd s, VectorXd g, double l = 1.0)
              : s_(l), e_(s_, 7, time_step), start_(s), goal_(g) {
            std::random_device rd;
            gen_ = std::mt19937(rd());

            xy_dis_ = std::uniform_real_distribution<double>(-1.0, 1.0);
            th_dis_ = std::uniform_real_distribution<double>(-M_PI, M_PI);

            state_ = VectorXd::Zero(7);
            reset();
          }

          std::pair<VectorXd, double> step(double ua, double uth) {
            //double clipped_uth = std::max(-M_PI * 2.0 / 180.0, std::min(uth, M_PI * 2.0 / 180.0));

            //double clipped_uth = std::max(-M_PI, std::min(uth, M_PI));

            state_[5] = ua;
            state_[6] = uth;

            double goal_r = -std::pow((state_.head(2) - goal_.head(2)).norm(), 2.0);
            double control_r = -std::pow((std::abs(uth) + std::abs(ua)), 2.0);
            double reward = goal_r + 0.001*control_r;

            e_.set_state(state_);
            state_ = e_.step();

            return std::make_pair(state_.head(5), reward);
          }

          VectorXd reset() {
            state_[0] = xy_dis_(gen_);
            state_[1] = xy_dis_(gen_);
            state_[2] = th_dis_(gen_);
            state_[3] = 0.0;
            state_[4] = 0.0;

            state_[4] = 0.0;
            state_[5] = 0.0;

            return state_.head(5);
          }

          VectorXd reset(const VectorXd& s) {
            assert(s.size() == 5);

            state_[0] = s[0];
            state_[1] = s[1];
            state_[2] = s[2];
            state_[3] = s[3];
            state_[4] = s[4];

            state_[5] = 0.0;
            state_[6] = 0.0;

            return state_.head(5);
          }

          // In seconds
          const double time_step = 0.01;

          DynamicCarSystem s_;

        private:
          RK4Integrator e_;

          VectorXd state_;

          VectorXd start_;
          VectorXd goal_;

          std::mt19937 gen_;
          std::uniform_real_distribution<double> xy_dis_;
          std::uniform_real_distribution<double> th_dis_;
      };

    } // namespace physics
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H */


// Copyright(c) 2021, Richardson Lab at Duke
// Licensed under the Apache 2 license
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissionsand
// limitations under the License.

#include <cmath>
#include <scitbx/constants.h>
#include <algorithm>
#include <boost/lexical_cast.hpp>
#include "DotSpheres.h"

namespace molprobity {
namespace probe {

DotSphere::DotSphere(double radius, double density)
  : m_rad(radius), m_dens(density)
{
  // Clamp our inputs to non-negative
  if (m_rad < 0) { m_rad = 0; }
  if (m_dens < 0) { m_dens = 0; }

  // If we have a zero radius or density, we're done
  if (m_rad == 0 || m_dens == 0) { return; }

  // Estimate the number of dots to be placed given the radius and density.
  // This code is pulled from DotDph.cpp in Reduce.
  size_t num_dots = static_cast<size_t>(floor(4.0 * scitbx::constants::pi * density * (radius * radius)));

  // Generate the dots, spreading them across the sphere.
  double offset = 0.2;
  double ang, cosang, sinang, phi, theta, xy0, x0, y0, z0;
  int nequator, nvert, nhoriz;
  bool odd = true;

  nequator = static_cast<int>(floor(sqrt(num_dots * scitbx::constants::pi)));

  ang = 5.0 * scitbx::constants::pi / 360.0;
  cosang = cos(ang);
  sinang = sin(ang);

  nvert = nequator / 2;
  for (int j = 0; j <= nvert; j++) {
    phi = (scitbx::constants::pi * j) / nvert;
    z0 = cos(phi) * radius;
    xy0 = sin(phi) * radius;

    nhoriz = static_cast<int>(floor(nequator * sin(phi)));
    if (nhoriz < 1) { nhoriz = 1; }
    for (int k = 0; k < nhoriz; k++) {
      if (odd) { theta = (2.0 * scitbx::constants::pi * k + offset) / nhoriz; }
      else { theta = (2.0 * scitbx::constants::pi * k) / nhoriz; }
      x0 = cos(theta) * xy0;
      y0 = sin(theta) * xy0;

      m_vec.push_back(molprobity::probe::Point(x0, y0 * cosang - z0 * sinang, y0 * sinang + z0 * cosang));
    }
    odd = !odd;
  }
}

std::string DotSphere::test()
{
  // Test creation with negative density and/or radius
  {
    DotSphere d(-1, 10);
    if (d.radius() != 0 || d.dots().size() != 0) {
      return "molprobity::probe::DotSphere::test(): Construction with negative radius failed";
    }
  }
  {
    DotSphere d(5, -3);
    if (d.density() != 0 || d.dots().size() != 0) {
      return "molprobity::probe::DotSphere::test(): Construction with negative density failed";
    }
  }
  {
    DotSphere d(-5, -3);
    if (d.radius() != 0 || d.density() != 0 || d.dots().size() != 0) {
      return "molprobity::probe::DotSphere::test(): Construction with negative radius and density failed";
    }
  }

  // Test creating with extremely small density to be sure we get a single dot
  {
    DotSphere d(1, 1e-10);
    if (d.radius() != 1 || d.density() != 1e-10 || d.dots().size() > 2) {
      return "molprobity::probe::DotSphere::test(): Construction with small density failed";
    }
  }

  // Test creating with a different densities and see whether the number of dots is close.
  {
    for (double den = 1; den < 128; den *= 1.3) {
      DotSphere d(1, den);
      double expected = ceil(4.0 * scitbx::constants::pi * den * (1 * 1));
      long found = static_cast<long>(d.dots().size());
      double diff = abs(found - expected);
      if (diff > std::max(3.0, expected*0.2)) {
        return std::string("molprobity::probe::DotSphere::test(): Construction with density ")
          +boost::lexical_cast<std::string>(den) + " failed: "
          + "found " + boost::lexical_cast<std::string>(found) + " dots but expected "
          + boost::lexical_cast<std::string>(expected);
      }
    }
  }

  // Test creating with reasonable density to be sure we get dots in all octants
  {
    DotSphere d(1, 5);
    bool px = false, mx = false, py = false, my = false, pz = false, mz = false;
    const scitbx::af::shared<Point>& dots = d.dots();
    for (size_t i = 0; i < dots.size(); i++) {
      Point dot = dots[i];
      if (dot[0] > 0) { px = true; }
      if (dot[0] < 0) { mx = true; }
      if (dot[1] > 0) { py = true; }
      if (dot[1] < 0) { my = true; }
      if (dot[2] > 0) { pz = true; }
      if (dot[2] < 0) { mz = true; }
    }
    if (!px || !mx || !py || !my || !pz || !mz) {
      return "molprobity::probe::DotSphere::test(): Construction with reasoneble density "
        "did not have dots in all octants";
    }
  }

  // All tests passed.
  return "";
}

const DotSphere& DotSphereCache::get_sphere(double radius)
{
  std::map<double, DotSphere>::const_iterator ret = m_spheres.find(radius);
  if (ret == m_spheres.end()) {
    // We don't have a sphere with this radius -- create one and insert it
    std::pair<std::map<double, DotSphere>::iterator, bool> iRet =
      m_spheres.insert(std::pair<double, DotSphere>(radius, DotSphere(radius, m_dens)));
    ret = iRet.first;
  }
  return ret->second;
}

std::string DotSphereCache::test()
{
  // Object to use for our tests
  DotSphereCache dsc(10);

  // Test creation of a single sphere
  const DotSphere& sp1 = dsc.get_sphere(1.0);
  if (dsc.size() != 1) {
    return "molprobity::probe::DotSphereCache::test(): Single sphere creation failed";
  }

  // Ask for another sphere of the same size and make sure we get the same one.
  const DotSphere& sp2 = dsc.get_sphere(1.0);
  if (dsc.size() != 1 || sp1 != sp2) {
    return "molprobity::probe::DotSphereCache::test(): Identical sphere creation failed";
  }

  // Ask a sphere of a differnt size and make sure we get a different one.
  const DotSphere& sp3 = dsc.get_sphere(2.0);
  if (dsc.size() != 2 || sp1 == sp3) {
    return "molprobity::probe::DotSphereCache::test(): Unique sphere creation failed";
  }

  // All tests passed.
  return "";
}

std::string DotSpheres_test()
{
  std::string ret;

  /// Test DotSphere class
  ret = DotSphere::test();
  if (!ret.empty()) {
    return std::string("molprobity::probe::DotSpheres_test(): failed: ") + ret;
  }

  /// Test DotSphereCache class
  ret = DotSphereCache::test();
  if (!ret.empty()) {
    return std::string("molprobity::probe::DotSpheres_test(): failed: ") + ret;
  }

  // All tests passed.
  return "";
}

} // end namespace probe
} // end namespace molprobity


/**
 * @file
 * Test of rotation-chain example using Ceres autodiff, for comparison
 */
#include <benchmark/benchmark.h>
#include <ceres/ceres.h>
#include <Eigen/Core>

#include "../bechmark_helpers.hpp"

template <typename T>
using EigenVector = std::vector<T, Eigen::aligned_allocator<T>>;

template <typename Scalar>
using Mat3 = Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>;

template <typename Scalar>
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;

template <typename Scalar>
using Mat9 = Eigen::Matrix<Scalar, 9, 9, Eigen::RowMajor>;

template <typename Scalar>
using InVec3 = Eigen::Map<const Vec3<Scalar>>;

template <typename Scalar>
using OutVec3 = Eigen::Map<Vec3<Scalar>>;

template <typename Scalar>
using InQuat = Eigen::Map<const Eigen::Quaternion<Scalar>>;

template <typename Scalar>
using OutQuat = Eigen::Map<Eigen::Quaternion<Scalar>>;

template <typename Scalar>
using InMat3 = Eigen::Map<const Mat3<Scalar>>;

template <typename Scalar>
using OutMat3 = Eigen::Map<Mat3<Scalar>>;

using InMat34d = Eigen::Map<const Eigen::Matrix<double, 3, 4, Eigen::RowMajor>>;
using InMat43d = Eigen::Map<const Eigen::Matrix<double, 4, 3, Eigen::RowMajor>>;

struct ChainFunctor1 {
    template <typename T>
    bool operator()(const T *const x0, const T *const x1, T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1};
        OutVec3<T> v2{e};

        v2 = q1 * v1;
        return true;
    }
};

struct ChainFunctor2 {
    template <typename T>
    bool operator()(const T *const x0, const T *const x1, const T *const x2, T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * v1;
        return true;
    }
};

struct ChainFunctor3 {
    template <typename T>
    bool operator()(const T *const x0,
                    const T *const x1,
                    const T *const x2,
                    const T *const x3,
                    T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2}, q3{x3};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * q3 * v1;
        return true;
    }
};

struct ChainFunctor4 {
    template <typename T>
    bool operator()(const T *const x0,
                    const T *const x1,
                    const T *const x2,
                    const T *const x3,
                    const T *const x4,
                    T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2}, q3{x3}, q4{x4};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * q3 * q4 * v1;
        return true;
    }
};

struct ChainFunctor5 {
    template <typename T>
    bool operator()(const T *const x0,
                    const T *const x1,
                    const T *const x2,
                    const T *const x3,
                    const T *const x4,
                    const T *const x5,
                    T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2}, q3{x3}, q4{x4}, q5{x5};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * q3 * q4 * q5 * v1;
        return true;
    }
};

struct ChainFunctor6 {
    template <typename T>
    bool operator()(const T *const x0,
                    const T *const x1,
                    const T *const x2,
                    const T *const x3,
                    const T *const x4,
                    const T *const x5,
                    const T *const x6,
                    T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2}, q3{x3}, q4{x4}, q5{x5}, q6{x6};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * q3 * q4 * q5 * q6 * v1;
        return true;
    }
};

struct ChainFunctor7 {
    template <typename T>
    bool operator()(const T *const x0,
                    const T *const x1,
                    const T *const x2,
                    const T *const x3,
                    const T *const x4,
                    const T *const x5,
                    const T *const x6,
                    const T *const x7,
                    T *e) const {
        InVec3<T> v1{x0};
        InQuat<T> q1{x1}, q2{x2}, q3{x3}, q4{x4}, q5{x5}, q6{x6}, q7{x7};
        OutVec3<T> v2{e};

        v2 = q1 * q2 * q3 * q4 * q5 * q6 * q7 * v1;
        return true;
    }
};

class RotateChain : public benchmark::Fixture {
 protected:
    const int N = 1000;
    // Use wave::RotationM just for the Random() method, which produces valid SO(3)
    using waveRot = wave::RotationQd;
    const EigenVector<waveRot> R1_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R2_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R3_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R4_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R5_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R6_vec = randomMatrices<waveRot>(N);
    const EigenVector<waveRot> R7_vec = randomMatrices<waveRot>(N);
    const EigenVector<Eigen::Vector3d> v1_vec = randomMatrices<Eigen::Vector3d>(N);
};

BENCHMARK_F(RotateChain, ceres1)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor1, 3, 3, 4>{new ChainFunctor1()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J1_global[3 * 4];
            Mat3<double> J1, J2;

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0, J1_global};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
            J1 = InMat34d{J1_global} * InMat43d{J_global_local};

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J1);
        }
    }
}

BENCHMARK_F(RotateChain, ceres2)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor2, 3, 3, 4, 4>{new ChainFunctor2()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J1_global[3 * 4], J2_global[3 * 4];
            Mat3<double> J1, J2;

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0, J1_global, J2_global};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
            J1 = InMat34d{J1_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R2_vec[i].value().coeffs().data(), J_global_local);
            J2 = InMat34d{J2_global} * InMat43d{J_global_local};

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J1);
            benchmark::DoNotOptimize(J2);
        }
    }
}

BENCHMARK_F(RotateChain, ceres3)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor3, 3, 3, 4, 4, 4>{new ChainFunctor3()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J1_global[3 * 4], J2_global[3 * 4], J3_global[3 * 4];
            Mat3<double> J1, J2, J3;

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data(),
                                            R3_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0, J1_global, J2_global, J3_global};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
            J1 = InMat34d{J1_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R2_vec[i].value().coeffs().data(), J_global_local);
            J2 = InMat34d{J2_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R3_vec[i].value().coeffs().data(), J_global_local);
            J3 = InMat34d{J3_global} * InMat43d{J_global_local};

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J1);
            benchmark::DoNotOptimize(J2);
            benchmark::DoNotOptimize(J3);
        }
    }
}

BENCHMARK_F(RotateChain, ceres4)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor4, 3, 3, 4, 4, 4, 4>{
        new ChainFunctor4()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J1_global[3 * 4], J2_global[3 * 4], J3_global[3 * 4], J4_global[3 * 4];
            Mat3<double> J1, J2, J3, J4;

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data(),
                                            R3_vec[i].value().coeffs().data(),
                                            R4_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0, J1_global, J2_global, J3_global, J4_global};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
            J1 = InMat34d{J1_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R2_vec[i].value().coeffs().data(), J_global_local);
            J2 = InMat34d{J2_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R3_vec[i].value().coeffs().data(), J_global_local);
            J3 = InMat34d{J3_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R4_vec[i].value().coeffs().data(), J_global_local);
            J4 = InMat34d{J4_global} * InMat43d{J_global_local};

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J1);
            benchmark::DoNotOptimize(J2);
            benchmark::DoNotOptimize(J3);
            benchmark::DoNotOptimize(J4);
        }
    }
}

BENCHMARK_F(RotateChain, ceres5)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor5, 3, 3, 4, 4, 4, 4, 4>{
        new ChainFunctor5()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J1_global[3 * 4], J2_global[3 * 4], J3_global[3 * 4], J4_global[3 * 4],
              J5_global[3 * 4];
            Mat3<double> J1, J2, J3, J4, J5;

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data(),
                                            R3_vec[i].value().coeffs().data(),
                                            R4_vec[i].value().coeffs().data(),
                                            R5_vec[i].value().coeffs().data()};
            double *jacobians[] = {
              J0, J1_global, J2_global, J3_global, J4_global, J5_global};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
            J1 = InMat34d{J1_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R2_vec[i].value().coeffs().data(), J_global_local);
            J2 = InMat34d{J2_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R3_vec[i].value().coeffs().data(), J_global_local);
            J3 = InMat34d{J3_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R4_vec[i].value().coeffs().data(), J_global_local);
            J4 = InMat34d{J4_global} * InMat43d{J_global_local};
            qp.ComputeJacobian(R5_vec[i].value().coeffs().data(), J_global_local);
            J5 = InMat34d{J5_global} * InMat43d{J_global_local};

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J1);
            benchmark::DoNotOptimize(J2);
            benchmark::DoNotOptimize(J3);
            benchmark::DoNotOptimize(J4);
            benchmark::DoNotOptimize(J5);
        }
    }
}

BENCHMARK_F(RotateChain, ceres6)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor6, 3, 3, 4, 4, 4, 4, 4, 4>{
        new ChainFunctor6()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J_global[6][3 * 4];
            EigenVector<Mat3<double>> J_local(6);

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data(),
                                            R3_vec[i].value().coeffs().data(),
                                            R4_vec[i].value().coeffs().data(),
                                            R5_vec[i].value().coeffs().data(),
                                            R6_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0,
                                   J_global[0],
                                   J_global[1],
                                   J_global[2],
                                   J_global[3],
                                   J_global[4],
                                   J_global[5]};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            for (int i = 0; i < 6; ++i) {
                qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
                J_local[i] = InMat34d{J_global[i]} * InMat43d{J_global_local};
            }

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J_local);
        }
    }
}

BENCHMARK_F(RotateChain, ceres7)(benchmark::State &state) {
    std::unique_ptr<ceres::CostFunction> cost_function{
      new ceres::AutoDiffCostFunction<ChainFunctor7, 3, 3, 4, 4, 4, 4, 4, 4, 4>{
        new ChainFunctor7()}};

    ceres::EigenQuaternionParameterization qp{};

    for (auto _ : state) {
        for (auto i = N; i-- > 0;) {
            double res[3];
            double J0[3 * 3];
            double J_global[7][3 * 4];
            EigenVector<Mat3<double>> J_local(7);

            double const *const inputs[] = {v1_vec[i].data(),
                                            R1_vec[i].value().coeffs().data(),
                                            R2_vec[i].value().coeffs().data(),
                                            R3_vec[i].value().coeffs().data(),
                                            R4_vec[i].value().coeffs().data(),
                                            R5_vec[i].value().coeffs().data(),
                                            R6_vec[i].value().coeffs().data(),
                                            R7_vec[i].value().coeffs().data()};
            double *jacobians[] = {J0,
                                   J_global[0],
                                   J_global[1],
                                   J_global[2],
                                   J_global[3],
                                   J_global[4],
                                   J_global[5],
                                   J_global[6]};

            // Compute result and global (3*4) jacobians
            cost_function->Evaluate(inputs, res, jacobians);

            // Compute local (3*3) jacobians
            double J_global_local[4 * 3];
            for (int i = 0; i < 7; ++i) {
                qp.ComputeJacobian(R1_vec[i].value().coeffs().data(), J_global_local);
                J_local[i] = InMat34d{J_global[i]} * InMat43d{J_global_local};
            }

            benchmark::DoNotOptimize(res);
            benchmark::DoNotOptimize(J0);
            benchmark::DoNotOptimize(J_local);
        }
    }
}

WAVE_BENCHMARK_MAIN()


#include <string>

#include <CGAL/boost/graph/properties.h>
#include <Eigen/SparseCore>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Util/Assert.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/MatOp/SparseSymShiftSolve.h>
#include <Spectra/MatOp/SymShiftInvert.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>

namespace Euclid
{

namespace _impl
{

template<typename T, typename DerivedA, typename DerivedB>
unsigned sym_solve(const Eigen::SparseMatrix<T>& L,
                   int k,
                   int nv,
                   unsigned max_iter,
                   double tolerance,
                   Eigen::MatrixBase<DerivedA>& lambdas,
                   Eigen::MatrixBase<DerivedB>& phis)
{
    // use shift-invert mode to get the smallest eigenvalues fast
    auto convergence = std::min(2 * k + 1, nv);
    using Operator = Spectra::SparseSymShiftSolve<T>;
    using Solver = Spectra::SymEigsShiftSolver<Operator>;
    Operator op(L);
    Solver eigensolver(op, k, convergence, -1.0);
    eigensolver.init();
    unsigned n = eigensolver.compute(Spectra::SortRule::LargestMagn,
                                     max_iter,
                                     static_cast<T>(tolerance),
                                     Spectra::SortRule::SmallestMagn);
    if (eigensolver.info() != Spectra::CompInfo::Successful) {
        throw std::runtime_error("Eigen decomposition failed.");
    }
    lambdas = eigensolver.eigenvalues();
    phis = eigensolver.eigenvectors();
    return n;
}

template<typename T, typename DerivedA, typename DerivedB>
unsigned gen_solve(const Eigen::SparseMatrix<T>& S,
                   const Eigen::SparseMatrix<T>& D,
                   int k,
                   int nv,
                   unsigned max_iter,
                   double tolerance,
                   Eigen::MatrixBase<DerivedA>& lambdas,
                   Eigen::MatrixBase<DerivedB>& phis)
{
    int convergence = std::min(2 * k + 1, nv);
    using Operator = Spectra::SymShiftInvert<T, Eigen::Sparse, Eigen::Sparse>;
    using BOperator = Spectra::SparseSymMatProd<T>;
    using Solver =
        Spectra::SymGEigsShiftSolver<Operator,
                                     BOperator,
                                     Spectra::GEigsMode::ShiftInvert>;
    Operator op(S, D);
    BOperator bop(D);
    Solver eigensolver(op, bop, k, convergence, -1.0);
    eigensolver.init();
    unsigned n = eigensolver.compute(Spectra::SortRule::LargestMagn,
                                     max_iter,
                                     static_cast<T>(tolerance),
                                     Spectra::SortRule::SmallestMagn);
    if (eigensolver.info() != Spectra::CompInfo::Successful) {
        throw std::runtime_error("Eigen decomposition failed.");
    }
    lambdas = eigensolver.eigenvalues();
    phis = eigensolver.eigenvectors();
    return n;
}

} // namespace _impl

template<typename Mesh, typename DerivedA, typename DerivedB>
unsigned spectrum(const Mesh& mesh,
                  unsigned k,
                  Eigen::MatrixBase<DerivedA>& lambdas,
                  Eigen::MatrixBase<DerivedB>& phis,
                  SpecOp op,
                  unsigned max_iter,
                  double tolerance)
{
    using T = typename CGAL::Kernel_traits<typename boost::property_traits<
        typename boost::property_map<Mesh, boost::vertex_point_t>::type>::
                                               value_type>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<T>;
    auto nv = num_vertices(mesh);

    if (k > nv) {
        std::string err("You've requested ");
        err.append(std::to_string(k));
        err.append(" eigenvalues but there are only ");
        err.append(std::to_string(nv));
        err.append(" vertices in your mesh.");
        EWARNING(err);
        k = nv;
    }

    unsigned n;
    if (op == SpecOp::mesh_laplacian) {
        SpMat C = Euclid::cotangent_matrix(mesh);
        SpMat D = Euclid::mass_matrix(mesh);
        n = _impl::gen_solve(C, D, k, nv, max_iter, tolerance, lambdas, phis);
    }
    else {
        auto result = Euclid::adjacency_matrix(mesh);
        SpMat A = std::get<0>(result);
        SpMat D = std::get<1>(result);
        SpMat L = D - A;
        n = _impl::sym_solve(L, k, nv, max_iter, tolerance, lambdas, phis);
    }

    if (n < k) {
        auto str = std::to_string(k);
        str.append(" eigenvalues are requested, but only ");
        str.append(std::to_string(n));
        str.append(" values converged in computation.");
        EWARNING(str);
    }
    EASSERT(lambdas.rows() == n);
    EASSERT(phis.cols() == n);
    EASSERT(phis.rows() == nv);

    return n;
}

} // namespace Euclid


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

#include <boost/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <sstream>
#include <string>
#include <type_traits>
using namespace boost::hana;

int main() {

{

//! [comparable]
BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, 2, 3) == make<Tuple>(1, 2, 3));
BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, 2, 3) != make<Tuple>(1, 2, 3, 4));
//! [comparable]

}{

//! [orderable]
BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, 2, 3) < make<Tuple>(2, 3, 4));
BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, 2, 3) < make<Tuple>(1, 2, 3, 4));
//! [orderable]

}{

//! [foldable]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

auto show = [=](auto x, auto y) {
    return "(" + to_string(x) + " + " + to_string(y) + ")";
};

BOOST_HANA_RUNTIME_CHECK(foldl(make<Tuple>(2, "3", '4'), "1", show) == "(((1 + 2) + 3) + 4)");
//! [foldable]

}{

//! [iterable]
BOOST_HANA_CONSTEXPR_CHECK(head(make<Tuple>(1, '2', 3.3)) == 1);
BOOST_HANA_CONSTEXPR_CHECK(tail(make<Tuple>(1, '2', 3.3)) == make<Tuple>('2', 3.3));
BOOST_HANA_CONSTANT_CHECK(!is_empty(make<Tuple>(1, '2', 3.3)));
BOOST_HANA_CONSTANT_CHECK(is_empty(make<Tuple>()));
//! [iterable]

}{

//! [searchable]
using namespace std::string_literals;

BOOST_HANA_RUNTIME_CHECK(
    find_if(make<Tuple>(1, '2', 3.3, "abc"s), is_a<std::string>) == just("abc"s)
);

BOOST_HANA_RUNTIME_CHECK(
    "abc"s ^in^ make<Tuple>(1, '2', 3.3, "abc"s)
);
//! [searchable]

}{

//! [functor]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

BOOST_HANA_RUNTIME_CHECK(
    transform(make<Tuple>(1, '2', "345", std::string{"67"}), to_string) ==
    make<Tuple>("1", "2", "345", "67")
);
//! [functor]

}{

//! [applicative]
BOOST_HANA_CONSTEXPR_CHECK(lift<Tuple>('x') == make<Tuple>('x'));
BOOST_HANA_CONSTEXPR_CHECK(equal(lift<ext::std::Tuple>('x'), std::make_tuple('x')));

BOOST_HANA_CONSTEXPR_LAMBDA auto f = pair;
BOOST_HANA_CONSTEXPR_LAMBDA auto g = flip(pair);
BOOST_HANA_CONSTEXPR_CHECK(
    ap(make<Tuple>(f, g), make<Tuple>(1, 2, 3), make<Tuple>('a', 'b'))
        ==
    make<Tuple>(
        f(1, 'a'), f(1, 'b'), f(2, 'a'), f(2, 'b'), f(3, 'a'), f(3, 'b'),
        g(1, 'a'), g(1, 'b'), g(2, 'a'), g(2, 'b'), g(3, 'a'), g(3, 'b')
    )
);
//! [applicative]

}{

//! [monad]
BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return make<Tuple>(x, -x);
};

BOOST_HANA_CONSTEXPR_CHECK((make<Tuple>(1, 2, 3) | f) == make<Tuple>(1, -1, 2, -2, 3, -3));

BOOST_HANA_CONSTEXPR_CHECK(
    flatten(make<Tuple>(make<Tuple>(1, 2), make<Tuple>(3, 4), make<Tuple>(make<Tuple>(5, 6))))
    ==
    make<Tuple>(1, 2, 3, 4, make<Tuple>(5, 6))
);
//! [monad]

}{

//! [monad_plus]
using namespace std::string_literals;

BOOST_HANA_CONSTANT_CHECK(empty<Tuple>() == make<Tuple>());
BOOST_HANA_CONSTEXPR_CHECK(
    append(make<Tuple>(1, '2', 3.3), nullptr) == make<Tuple>(1, '2', 3.3, nullptr)
);

BOOST_HANA_CONSTEXPR_CHECK(
    concat(make<Tuple>(1, '2', 3.3), make<Tuple>("abcdef"s)) ==
    make<Tuple>(1, '2', 3.3, "abcdef"s)
);
//! [monad_plus]

}{

//! [traversable]
using namespace std::string_literals;

BOOST_HANA_RUNTIME_CHECK(
    sequence<Tuple>(
        make<Tuple>(make<Tuple>("a1"s, "a2"s),
                    make<Tuple>("b1"s),
                    make<Tuple>("c1"s, "c2"s, "c3"s))
    )
    ==
    make<Tuple>(
        make<Tuple>("a1"s, "b1"s, "c1"s),
        make<Tuple>("a1"s, "b1"s, "c2"s),
        make<Tuple>("a1"s, "b1"s, "c3"s),

        make<Tuple>("a2"s, "b1"s, "c1"s),
        make<Tuple>("a2"s, "b1"s, "c2"s),
        make<Tuple>("a2"s, "b1"s, "c3"s)
    )
);

BOOST_HANA_CONSTEXPR_LAMBDA auto half = [](auto x) {
    return if_(x % int_<2> == int_<0>,
        just(x / int_<2>),
        nothing
    );
};

BOOST_HANA_CONSTANT_CHECK(
    traverse<Maybe>(make<Tuple>(int_<2>, int_<4>, int_<6>), half)
    ==
    just(make<Tuple>(int_<1>, int_<2>, int_<3>))
);

BOOST_HANA_CONSTANT_CHECK(
    traverse<Maybe>(make<Tuple>(int_<2>, int_<3>, int_<6>), half)
    ==
    nothing
);
//! [traversable]

}{

//! [make]
BOOST_HANA_CONSTANT_CHECK(make<Tuple>() == make<Tuple>());
BOOST_HANA_CONSTEXPR_CHECK(make<Tuple>(1, '2', 3.3) == make<Tuple>(1, '2', 3.3));
//! [make]

}{

//! [init]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(init(make<Tuple>(1)) == make<Tuple>());
BOOST_HANA_CONSTEXPR_CHECK(init(make<Tuple>(1, '2', 3.3, 4_c)) == make<Tuple>(1, '2', 3.3));
//! [init]

}{

//! [intersperse]
BOOST_HANA_CONSTEXPR_CHECK(
    intersperse(make<Tuple>(1, '2', 3.3), 'x') == make<Tuple>(1, 'x', '2', 'x', 3.3)
);
BOOST_HANA_CONSTANT_CHECK(intersperse(make<Tuple>(), 'x') == make<Tuple>());
//! [intersperse]

}{

//! [remove_at]
BOOST_HANA_CONSTEXPR_CHECK(
    remove_at(int_<2>, make<Tuple>(0, '1', 2.2, 3u)) == make<Tuple>(0, '1', 3u)
);
//! [remove_at]

}{

//! [remove_at_c]
BOOST_HANA_CONSTEXPR_CHECK(
    remove_at_c<2>(make<Tuple>(0, '1', 2.2, 3u)) == make<Tuple>(0, '1', 3u)
);
//! [remove_at_c]

}{

//! [reverse]
BOOST_HANA_CONSTEXPR_CHECK(reverse(make<Tuple>(1, '2', 3.3)) == make<Tuple>(3.3, '2', 1));
//! [reverse]

}{

//! [group_by]
BOOST_HANA_CONSTEXPR_CHECK(
    group_by(equal ^on^ decltype_,
        make<Tuple>(1, 2, 3, 'x', 'y', 4.4, 5.5)
    )
    == make<Tuple>(
        make<Tuple>(1, 2, 3),
        make<Tuple>('x', 'y'),
        make<Tuple>(4.4, 5.5)
    )
);
//! [group_by]

}{

//! [group]
BOOST_HANA_CONSTANT_CHECK(
    group(make<Tuple>(int_<1>, long_<1>, type<int>, char_<'x'>, char_<'x'>))
    ==
    make<Tuple>(
        make<Tuple>(int_<1>, long_<1>),
        make<Tuple>(type<int>),
        make<Tuple>(char_<'x'>, char_<'x'>)
    )
);
//! [group]

}{

//! [zip]
BOOST_HANA_CONSTEXPR_CHECK(
    zip(make<Tuple>(1, 'a'), make<Tuple>(2, 3.3))
    ==
    make<Tuple>(make<Tuple>(1, 2), make<Tuple>('a', 3.3))
);

BOOST_HANA_CONSTEXPR_CHECK(
    zip(make<Tuple>(1, 'a'), make<Tuple>(2, 3.3), make<Tuple>(3, 'c', "ignored"))
    ==
    make<Tuple>(make<Tuple>(1, 2, 3), make<Tuple>('a', 3.3, 'c'))
);
//! [zip]

}{

//! [zip_with]
BOOST_HANA_CONSTEXPR_CHECK(
    zip.with(_ * _, make<Tuple>(1, 2, 3, 4), make<Tuple>(5, 6, 7, 8, "ignored"))
    ==
    make<Tuple>(5, 12, 21, 32)
);
//! [zip_with]

}{

//! [unzip]
BOOST_HANA_CONSTEXPR_CHECK(
    unzip(make<Tuple>(make<Tuple>(1, '2', 3.3), make<Tuple>('4', 5.5, 6)))
    ==
    make<Tuple>(make<Tuple>(1, '4'), make<Tuple>('2', 5.5), make<Tuple>(3.3, 6))
);

BOOST_HANA_CONSTEXPR_CHECK(
    unzip(make<Tuple>(make<Tuple>(1, '2', 3.3), make<Tuple>('4', 5.5, 6, "ignored")))
    ==
    make<Tuple>(make<Tuple>(1, '4'), make<Tuple>('2', 5.5), make<Tuple>(3.3, 6))
);
//! [unzip]

}{

//! [take_while]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(
    take_while(tuple_c<int, 0, 1, 2, 3>, _ < 2_c)
    ==
    tuple_c<int, 0, 1>
);
//! [take_while]

}{

//! [take_until]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(
    take_until(tuple_c<int, 3, 2, 1, 0>, _ < 2_c)
    ==
    tuple_c<int, 3, 2>
);
//! [take_until]

}{

//! [take]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(take(0_c, make<Tuple>(1, '2', 3.3)) == make<Tuple>());
BOOST_HANA_CONSTEXPR_CHECK(take(1_c, make<Tuple>(1, '2', 3.3)) == make<Tuple>(1));
BOOST_HANA_CONSTEXPR_CHECK(take(2_c, make<Tuple>(1, '2', 3.3)) == make<Tuple>(1, '2'));
BOOST_HANA_CONSTEXPR_CHECK(take(3_c, make<Tuple>(1, '2', 3.3)) == make<Tuple>(1, '2', 3.3));
BOOST_HANA_CONSTEXPR_CHECK(take(4_c, make<Tuple>(1, '2', 3.3)) == make<Tuple>(1, '2', 3.3));
//! [take]

}{

//! [take_c]
BOOST_HANA_CONSTEXPR_CHECK(take_c<2>(make<Tuple>(1, '2', 3.3)) == make<Tuple>(1, '2'));
//! [take_c]

}{

//! [span]
BOOST_HANA_CONSTEXPR_LAMBDA auto xs = make<Tuple>(int_<1>, int_<2>, int_<3>, int_<4>);
BOOST_HANA_CONSTANT_CHECK(
    span(xs, _ < int_<3>)
    ==
    pair(make<Tuple>(int_<1>, int_<2>), make<Tuple>(int_<3>, int_<4>))
);

BOOST_HANA_CONSTANT_CHECK(
    span(xs, _ < int_<0>)
    ==
    pair(make<Tuple>(), xs)
);

BOOST_HANA_CONSTANT_CHECK(
    span(xs, _ < int_<5>)
    ==
    pair(xs, make<Tuple>())
);
//! [span]

}{

//! [sort]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(
    sort(make<Tuple>(1_c, -2_c, 3_c, 0_c)) == make<Tuple>(-2_c, 0_c, 1_c, 3_c)
);
//! [sort]

}{

//! [sort_by]
using namespace literals;
BOOST_HANA_CONSTANT_CHECK(
    sort_by(_>_, make<Tuple>(1_c, -2_c, 3_c, 0_c))
    ==
    make<Tuple>(3_c, 1_c, 0_c, -2_c)
);
//! [sort_by]

}{

//! [slice]
BOOST_HANA_CONSTEXPR_CHECK(
    slice(make<Tuple>(1, '2', 3.3, type<float>), int_<1>, int_<3>)
    ==
    make<Tuple>('2', 3.3)
);
//! [slice]

}{

//! [slice_c]
BOOST_HANA_CONSTEXPR_CHECK(
    slice_c<1, 3>(make<Tuple>(1, '2', 3.3, type<float>))
    ==
    make<Tuple>('2', 3.3)
);
//! [slice_c]

}{

//! [permutations]
BOOST_HANA_CONSTEXPR_LAMBDA auto is_permutation_of = curry<2>([](auto xs, auto perm) {
    return elem(permutations(xs), perm);
});

BOOST_HANA_CONSTEXPR_CHECK(
    all_of(
        make<Tuple>(
            make<Tuple>('1', 2, 3.0),
            make<Tuple>('1', 3.0, 2),
            make<Tuple>(2, '1', 3.0),
            make<Tuple>(2, 3.0, '1'),
            make<Tuple>(3.0, '1', 2),
            make<Tuple>(3.0, 2, '1')
        ),
        is_permutation_of(make<Tuple>('1', 2, 3.0))
    )
);
//! [permutations]

}{

//! [unfoldl]
BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return if_(x == int_<0>, nothing, just(pair(x - int_<1>, x)));
};

BOOST_HANA_CONSTANT_CHECK(
    unfoldl<Tuple>(f, int_<10>)
    ==
    tuple_c<int, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10>
);
//! [unfoldl]

}{

//! [unfoldr]
BOOST_HANA_CONSTEXPR_LAMBDA auto f = [](auto x) {
    return if_(x == int_<0>, nothing, just(pair(x, x - int_<1>)));
};

BOOST_HANA_CONSTANT_CHECK(
    unfoldr<Tuple>(f, int_<10>)
    ==
    tuple_c<int, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1>
);
//! [unfoldr]

}{

//! [scanl]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

auto show = [=](auto x, auto y) {
    return "(" + to_string(x) + " + " + to_string(y) + ")";
};

BOOST_HANA_RUNTIME_CHECK(scanl(make<Tuple>(2, "3", '4'), 1, show) == make<Tuple>(
    1,
    "(1 + 2)",
    "((1 + 2) + 3)",
    "(((1 + 2) + 3) + 4)"
));
//! [scanl]

}{

//! [scanl1]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

auto show = [=](auto x, auto y) {
    return "(" + to_string(x) + " + " + to_string(y) + ")";
};

BOOST_HANA_RUNTIME_CHECK(scanl1(make<Tuple>(1, "2", '3'), show) == make<Tuple>(
    1,
    "(1 + 2)",
    "((1 + 2) + 3)"
));
//! [scanl1]

}{

//! [scanr]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

auto show = [=](auto x, auto y) {
    return "(" + to_string(x) + " + " + to_string(y) + ")";
};

BOOST_HANA_RUNTIME_CHECK(scanr(make<Tuple>(1, "2", '3'), 4, show) == make<Tuple>(
    "(1 + (2 + (3 + 4)))",
    "(2 + (3 + 4))",
    "(3 + 4)",
    4
));
//! [scanr]

}{

//! [scanr1]
auto to_string = [](auto x) {
    return static_cast<std::ostringstream const&>(std::ostringstream{} << x).str();
};

auto show = [=](auto x, auto y) {
    return "(" + to_string(x) + " + " + to_string(y) + ")";
};

BOOST_HANA_RUNTIME_CHECK(scanr1(make<Tuple>(1, "2", '3'), show) == make<Tuple>(
    "(1 + (2 + 3))",
    "(2 + 3)",
    '3'
));
//! [scanr1]

}{

//! [partition]
BOOST_HANA_CONSTANT_CHECK(
    partition(tuple_c<int, 1, 2, 3, 4, 5, 6, 7>, [](auto x) {
        return x % int_<2> != int_<0>;
    })
    ==
    pair(
        tuple_c<int, 1, 3, 5, 7>,
        tuple_c<int, 2, 4, 6>
    )
);

BOOST_HANA_CONSTANT_CHECK(
    partition(tuple_t<void, int, float, char, double>, trait<std::is_floating_point>)
    ==
    pair(
        tuple_t<float, double>,
        tuple_t<void, int, char>
    )
);
//! [partition]

}

}


#include <boost/variant.hpp>
#include <vector>
#include <string>
#include <iostream>

typedef boost::variant<int, float, std::string> cell_t;
typedef std::vector<cell_t> db_row_t;

db_row_t get_row(const char* query) {
	db_row_t row;
	row.push_back(10);
	row.push_back(10.1f);
	row.push_back("hello again");
	return row;
}

struct db_sum_visitor : public boost::static_visitor<double> {
	double operator() (int value) const {
		return value;
	}

	double operator() (float value) const {
		return value;
	}

	double operator() (const std::string&) const {
		return 0.0;
	}
};

int main(int argc, char **argv) {
	db_row_t row = get_row("Give me that!");
	double res = 0.0;
	db_row_t::const_iterator it = row.begin(), end = row.end();
	for(; it!=end; ++it) {
		res += boost::apply_visitor(db_sum_visitor(), *it);
	}

	std::cout << "Sum of arithmetic types in database row is: " << res << std::endl;
	return 0;
}