/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
/**
 * @file integrator.hpp
 * @author Boston Cleek
 * @date 28 Oct 2020
 * @brief Numerical integration methods
 */
#ifndef INTEGRATOR_HPP
#define INTEGRATOR_HPP

#include <cmath>
#include <functional>
#include <armadillo>

#include <ergodic_exploration/collision.hpp>
#include <ergodic_exploration/numerics.hpp>

namespace ergodic_exploration
{
using arma::linspace;
using arma::mat;
using arma::span;
using arma::vec;

/**
 * @brief Function representing the time derivatve of the co-state variable
 * @details inputs are co-state, ergodic measure derivatve, barrier derivatve,
 * and the jacobian of the dynamics w.r.t state
 */
typedef std::function<vec(const vec&, const vec&, const vec&, const mat&)> CoStateFunc;

/** @brief 4th order Runge-Kutta integration */
class RungeKutta
{
public:
  /**
   * @brief Constructor
   * @param dt - time step
   */
  RungeKutta(double dt);

  /**
   * @brief Simulate the dynamics forward in time
   * @param model - dynamic model
   * @param x0 - initial state
   * @param ut - control signal (each column is applied at a single time step)
   * @param horizon - length of trajectory in time
   * @return trajectory
   * @details the boundary condition is not added to the trajectory
   */
  template <class ModelT>
  mat solve(const ModelT& model, const vec& x0, const mat& ut, double horizon) const;

  /**
   * @brief Solve the co-state variable backwards in time
   * @param func - time derivatve of co-state variable
   * @param model - dynamic model
   * @param rhoT - co-state variable terminal condition (zero vector)
   * @param xt - forward porpagated dynamic model trajectory
   * @param ut - control signal
   * @param edx - gradient of the ergodic metric for each state in xt
   * @param bdx - derivatve of barrier function for each state in xt
   * @param horizon - length of trajectory in time
   * @return co-state variable solution
   * @details co-state is sorted from [t0 tf] no need to index backwards and the
   * boundary condition is not added to the trajectory
   */
  template <class ModelT>
  mat solve(const CoStateFunc& func, const ModelT& model, const vec& rhoT, const mat& xt,
            const mat& ut, const mat& edx, const mat& bdx, double horizon) const;

  /**
   * @brief Performs one step of RK4 forward in time
   * @param model - dynamic model
   * @param x - state
   * @param u - control
   * @return new state
   */
  template <class ModelT>
  vec step(const ModelT& model, const vec& x, const vec& u) const;

  /**
   * @brief Performs one step of RK4 backwards in time
   * @param func - time derivatve of the co-state variable
   * @param rho - co-state variable
   * @param gdx - gradient of the ergodic metric
   * @param dbar - derivatve of barrier function for a state
   * @param fdx - jacobian of the model with respect to the control
   * @return co-state variable
   * @details The robot model is used to compose A = D1[f(x,u)].
   * The columns of xt, ut, and edx correspond to
   * the state, control, or derivative at a given time.
   */
  vec step(const CoStateFunc& func, const vec& rho, const vec& gdx, const vec& dbar,
           const mat& fdx) const;

private:
  double dt_;  // time step
};

RungeKutta::RungeKutta(double dt) : dt_(dt)
{
}

template <class ModelT>
mat RungeKutta::solve(const ModelT& model, const vec& x0, const mat& ut,
                      double horizon) const
{
  // TODO: Add terminal x0?
  vec x = x0;
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt_));
  mat xt(x.n_rows, steps);

  for (unsigned int i = 0; i < steps; i++)
  {
    x = step(model, x, ut.col(i));
    x(2) = normalize_angle_PI(x(2));
    xt.col(i) = x;
  }

  return xt;
}

template <class ModelT>
mat RungeKutta::solve(const CoStateFunc& func, const ModelT& model, const vec& rhoT,
                      const mat& xt, const mat& ut, const mat& edx, const mat& bdx,
                      double horizon) const
{
  // TODO: Add terminal p(T)?
  vec rho = rhoT;
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt_));
  mat rhot(rho.n_rows, steps);

  // Iterate backwards
  // this way rhot from t0 to tf in the returned matrix
  for (unsigned int i = steps; i-- > 0;)
  {
    // Index states, controls, and ergodic measures at end of array
    rho = step(func, rho, edx.col(i), bdx.col(i), model.fdx(xt.col(i), ut.col(i)));
    rhot.col(i) = rho;
  }

  return rhot;
}

template <class ModelT>
vec RungeKutta::step(const ModelT& model, const vec& x, const vec& u) const
{
  const vec k1 = model(x, u);
  const vec k2 = model(x + dt_ * (0.5 * k1), u);
  const vec k3 = model(x + dt_ * (0.5 * k2), u);
  const vec k4 = model(x + dt_ * k3, u);
  return x + (dt_ / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}

vec RungeKutta::step(const CoStateFunc& func, const vec& rho, const vec& gdx,
                     const vec& dbar, const mat& fdx) const
{
  const vec k1 = func(rho, gdx, dbar, fdx);
  const vec k2 = func(rho - dt_ * (0.5 * k1), gdx, dbar, fdx);
  const vec k3 = func(rho - dt_ * (0.5 * k2), gdx, dbar, fdx);
  const vec k4 = func(rho - dt_ * k3, gdx, dbar, fdx);
  return rho - dt_ / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
}

class RungeKutta45
{
public:
  RungeKutta45(double hmax, double hmin, double epsilon, unsigned int max_iter);

  template <class ModelT>
  bool solve(mat& xt, const ModelT& model, const vec& x0, const mat& ut, double dt,
             double horizon) const;

  template <class ModelT>
  bool solve(mat& rhot, const CoStateFunc& func, const ModelT& model, const vec& rhoT,
             const mat& xt, const mat& ut, const mat& edx, const mat& bdx, double dt,
             double horizon) const;

  template <class ModelT>
  double step(vec& x_new, const ModelT& model, const vec& x, const vec& u, double h) const;

  double step(vec& rho_new, const CoStateFunc& func, const vec& rho, const vec& gdx,
              const vec& dbar, const mat& fdx, double h) const;

private:
  double hmin_, hmax_;
  double epsilon_;
  unsigned int max_iter_;
};

RungeKutta45::RungeKutta45(double hmin, double hmax, double epsilon, unsigned int max_iter)
  : hmin_(hmin), hmax_(hmax), epsilon_(epsilon), max_iter_(max_iter)
{
}

template <class ModelT>
bool RungeKutta45::solve(mat& xt, const ModelT& model, const vec& x0, const mat& ut,
                         double dt, double horizon) const
{
  // TODO: how to initialize h?
  auto h = (hmax_ + hmin_) / 2.0;
  // auto h = hmin_;
  vec x = x0;

  // desired length of trajectory
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));
  const vec tvec = linspace(0.0, horizon, steps);

  // allocate memory for trajectory based on max possible steps
  const auto max_steps = static_cast<unsigned int>(horizon / hmin_) + 1;
  mat xt_max(x0.n_rows, max_steps);
  vec tvec_max(max_steps);

  // add x0
  xt_max.col(0) = x0;
  tvec_max(0) = 0.0;

  vec x_new;
  auto t = 0.0;
  unsigned int stored = 1;
  unsigned int iter = 0;
  bool flag = 0;
  while (!flag)
  {
    const auto i = static_cast<unsigned int>(
        std::floor(std::round(static_cast<double>(steps - 1) * std::abs(t / horizon))));
    // std::cout << "i: " << i << std::endl;

    const auto r = step(x_new, model, x, ut.col(i), h);
    // std::cout << "r: " << r << std::endl;

    if (r < epsilon_ || almost_equal(r, epsilon_))
    {
      x = x_new;
      xt_max.col(stored) = x;

      t += h;
      tvec_max(stored) = t;

      stored++;
    }

    h *= 0.84 * std::pow(epsilon_ / r, 0.25);
    h = std::clamp(h, hmin_, hmax_);

    if (t > horizon || almost_equal(t, horizon))
    {
      flag = 1;
    }

    // detect final time
    else if (t + h > horizon)
    {
      h = horizon - t;
    }

    // std::cout << "t: " << t << std::endl;
    // std::cout << "h: " << h << std::endl;

    if (iter == max_iter_)
    {
      std::cout << "WARNING: max iterations reached " << std::endl;
      return false;
    }

    iter++;
  }

  // xt_max.cols(0, stored-1).t().print("traj");
  // tvec_max.rows(0, stored - 1).print("t rk45");
  // tvec.print("t desired");
  //
  // std::cout << "tvec_max: " << tvec_max.n_rows << std::endl;
  // std::cout << "tvec: " << tvec.n_rows << std::endl;
  //
  // std::cout << "steps: " << steps << std::endl;
  // std::cout << "stored: " << stored << std::endl;

  // fit quartic polynomials
  xt.resize(x0.n_rows, steps);
  for (unsigned int i = 0; i < x0.n_rows; i++)
  {
    const vec p =
        polyfit(tvec_max.rows(0, stored - 1), xt_max(span(i, i), span(0, stored - 1)), 4);
    // p.print("p");
    xt.row(i) = polyval(p, tvec).t();
  }

  return true;
}

template <class ModelT>
double RungeKutta45::step(vec& x_new, const ModelT& model, const vec& x, const vec& u,
                          double h) const
{
  const vec k1 = h * model(x, u);

  const vec k2 = h * model(x + ((1.0 / 4.0) * k1), u);

  const vec k3 = h * model(x + ((3.0 / 32.0) * k1) + ((9.0 / 32.0) * k2), u);

  const vec k4 = h * model(x + ((1932.0 / 2197.0) * k1) - ((7200.0 / 2197.0) * k2) +
                               ((7296.0 / 2197.0) * k3),
                           u);
  const vec k5 = h * model(x + ((439.0 / 216.0) * k1) - (8.0 * k2) +
                               ((3680.0 / 513.0) * k3) - ((845.0 / 4104.0) * k4),
                           u);
  const vec k6 =
      h * model(x - ((8.0 / 27.0) * k1) + (2.0 * k2) - ((3544.0 / 2565.0) * k3) +
                    ((1859.0 / 4104.0) * k4) - ((11.0 / 40.0) * k5),
                u);

  x_new = x + ((25.0 / 216.0) * k1) + ((1408.0 / 2565.0) * k3) +
          ((2197.0 / 4101.0) * k4) - ((1.0 / 5.0) * k5);

  const vec z = x + ((16.0 / 135.0) * k1) + ((6656.0 / 12825.0) * k3) +
                ((28561.0 / 56430.0) * k4) - ((9.0 / 50.0) * k5) + ((2.0 / 55.0) * k6);

  // std::cout << "max diff: " << max(abs(z - x_new)) << std::endl;
  // std::cout << "error norm: " << norm(z - x_new, 2) << std::endl;

  return max(abs(z - x_new)) / h;
  // return min(abs(z - x_new)) / h;
}

template <class ModelT>
bool RungeKutta45::solve(mat& rhot, const CoStateFunc& func, const ModelT& model,
                         const vec& rhoT, const mat& xt, const mat& ut, const mat& edx,
                         const mat& bdx, double dt, double horizon) const
{
  // TODO: how to initialize h?
  // auto h = (hmax_ + hmin_) / 2.0;
  auto h = hmin_;
  vec rho = rhoT;

  // desired length of trajectory
  const auto steps = static_cast<unsigned int>(std::abs(horizon / dt));
  const vec tvec = linspace(0.0, horizon, steps);

  // allocate memory for trajectory based on max possible steps
  const auto max_steps = static_cast<unsigned int>(horizon / hmin_) + 1;
  mat rhot_max(rhoT.n_rows, max_steps);
  vec tvec_max(max_steps);

  // add x0
  rhot_max.col(0) = rhoT;
  tvec_max(0) = horizon;

  vec rho_new;
  auto t = horizon;
  unsigned int stored = 1;
  unsigned int iter = 0;
  bool flag = 0;
  while (!flag)
  {
    const auto i = static_cast<unsigned int>(
        std::floor(std::round(static_cast<double>(steps - 1) * std::abs(t / horizon))));
    // std::cout << "i: " << i << std::endl;

    const auto r = step(rho_new, func, rho, edx.col(i), bdx.col(i),
                        model.fdx(xt.col(i), ut.col(i)), h);
    // std::cout << "r: " << r << std::endl;

    if (r < epsilon_ || almost_equal(r, epsilon_))
    {
      rho = rho_new;
      rhot_max.col(stored) = rho;

      t -= h;
      tvec_max(stored) = t;

      stored++;
    }

    h *= 0.84 * std::pow(epsilon_ / r, 0.25);
    h = std::clamp(h, hmin_, hmax_);

    if (t < 0.0 || almost_equal(t, 0.0))
    {
      flag = 1;
    }

    // detect final time
    else if (t - h < 0.0)
    {
      h = t;
    }

    // std::cout << "t: " << t << std::endl;
    // std::cout << "h: " << h << std::endl;

    if (iter == max_iter_)
    {
      std::cout << "WARNING: max iterations reached " << std::endl;
      return false;
    }

    iter++;
  }

  // rhot_max.cols(0, stored-1).t().print("rho traj");
  // tvec_max.rows(0, stored - 1).print("t rk45");
  // tvec.print("t desired");
  //
  // std::cout << "tvec_max: " << tvec_max.n_rows << std::endl;
  // std::cout << "tvec: " << tvec.n_rows << std::endl;
  //
  // std::cout << "steps: " << steps << std::endl;
  // std::cout << "stored: " << stored << std::endl;

  // fit quartic polynomials
  rhot.resize(rho.n_rows, steps);
  for (unsigned int i = 0; i < rhoT.n_rows; i++)
  {
    const vec p = polyfit(tvec_max.rows(0, stored - 1),
                          rhot_max(span(i, i), span(0, stored - 1)), 4);
    // p.print("p");
    rhot.row(i) = polyval(p, tvec).t();
  }

  return true;
}

double RungeKutta45::step(vec& rho_new, const CoStateFunc& func, const vec& rho,
                          const vec& gdx, const vec& dbar, const mat& fdx, double h) const
{
  const vec k1 = -h * func(rho, gdx, dbar, fdx);

  const vec k2 = -h * func(rho + ((1.0 / 4.0) * k1), gdx, dbar, fdx);

  const vec k3 =
      -h * func(rho + ((3.0 / 32.0) * k1) + ((9.0 / 32.0) * k2), gdx, dbar, fdx);

  const vec k4 = -h * func(rho + ((1932.0 / 2197.0) * k1) - ((7200.0 / 2197.0) * k2) +
                               ((7296.0 / 2197.0) * k3),
                           gdx, dbar, fdx);

  const vec k5 = -h * func(rho + ((439.0 / 216.0) * k1) - (8.0 * k2) +
                               ((3680.0 / 513.0) * k3) - ((845.0 / 4104.0) * k4),
                           gdx, dbar, fdx);

  const vec k6 =
      -h * func(rho - ((8.0 / 27.0) * k1) + (2.0 * k2) - ((3544.0 / 2565.0) * k3) +
                    ((1859.0 / 4104.0) * k4) - ((11.0 / 40.0) * k5),
                gdx, dbar, fdx);

  rho_new = rho + ((25.0 / 216.0) * k1) + ((1408.0 / 2565.0) * k3) +
            ((2197.0 / 4101.0) * k4) - ((1.0 / 5.0) * k5);

  const vec z = rho + ((16.0 / 135.0) * k1) + ((6656.0 / 12825.0) * k3) +
                ((28561.0 / 56430.0) * k4) - ((9.0 / 50.0) * k5) + ((2.0 / 55.0) * k6);

  // std::cout << "max diff: " << max(abs(z - rho_new)) << std::endl;
  // std::cout << "error norm: " << norm(z - rho_new, 2) << std::endl;

  return max(abs(z - rho_new)) / h;
}

}  // namespace ergodic_exploration
#endif


#include "EventShapes/EventShapes.h"

#include <Eigen/Dense>

#include <iostream>
#include <math.h>

using namespace Eigen;

ClassImp(EventShapes)

EventShapes::EventShapes()
	: ndims(2),
	  m_thrust_axis(nullptr),
	  m_thrust_major_axis(nullptr),
	  m_thrust_minor_axis(nullptr) {}

EventShapes::EventShapes(const std::vector<std::vector<float>>& momenta, unsigned int ndims)
	: ndims(ndims),
	  m_thrust_axis(nullptr),
	  m_thrust_major_axis(nullptr),
	  m_thrust_minor_axis(nullptr) {

	m_three_momenta.reserve(momenta.size());
	// for (auto& p : momenta) {
	// 	Vector3f tvector;
	// 	if (p.size() ==  2) {
	// 		tvector = Vector3f(p[0], p[1], 0.);
	// 		ndims = 2;
	// 	} else {
	// 		tvector = Vector3f(p[0], p[1], p[2]);
	// 		ndims = 3;
	// 	}
	// 	m_three_momenta.push_back(tvector);
	// }

	// If problem is presented in 2d
	if (ndims == 2) {
		for (auto& p : momenta) {
			m_three_momenta.push_back(Vector3f(p[0], p[1], 0.));
		}

	// Else if problem is presented in 3d
	} else {
		for (auto& p : momenta) {
			m_three_momenta.push_back(Vector3f(p[0], p[1], p[2]));
		}
	}

	m_ntracks = m_three_momenta.size();
	m_min_ntracks = ndims;

	m_randg = TRandom{};
}

double EventShapes::calcThrustValue(const std::vector<Vector3f>& pvec, const Vector3f& axis) {
	/* Function that calculates the thrust value of a set of input vectors
	 * 'pvec' about the thrust axis 'axis'
	 */

	double t_num = 0.;
	double t_denom = 0.;
	for (const auto& p : pvec) {
		t_num += fabs(axis.dot(p));
		t_denom += p.norm();
	}
	double thrust = t_num > 0 ? t_num / t_denom : 0.;

	return thrust;
}

void EventShapes::compare_calcT() {

	// Calculate thrust axis by each method
	// std::pair<Vector3f, double> axis_new_ret = calcT_new(m_three_momenta);
	std::pair<Vector3f, double> axis_orig_ret = calcT_orig(m_three_momenta);

	// Vector3f axis_new = axis_new_ret.first;
	// Vector3f axis_orig = axis_orig_ret.first;

	//<- Commented while many differences are found
	// for (unsigned int i = 0; i < 3; i++) {
	// 	if (fabs(fabs(axis_new[i]) - fabs(axis_orig[i])) > 1e-10) {
	// 		std::cout << "Difference found between two methods of thrust calculation" << std::endl;
	// 		std::cout << "The new and original methods yield, respectively:" << std::endl;
	// 		axis_new.rint();
	// 		axis_orig.rint();
	// 		break;
	// 	}
	// }

	// Set member data variable
	if (axis_orig_ret.first == Vector3f()) m_thrust_axis = nullptr;
	else m_thrust_axis = new Vector3f(axis_orig_ret.first);
	m_thrust = axis_orig_ret.second;
}

const std::pair<Vector3f, double> EventShapes::calcT_new(const std::vector<Vector3f>& pvec) {
	/* Function that attempts to calculate the thrust axis more rigorously
	 * based on the intuition that all 2^(n - 1) combinations of input
	 * vectors and their inverses do not need to be tested, and that the
	 * combinations that do need to be tested comprise a subset of the
	 * 2^(n - 1) combinations that can be found.
	 *
	 * This method partitions the set of input vectors into n hemispheres
	 * by dot product with each of the n input vectors in turn, builds
	 * the Longest Vector Sum in each hemisphere, and takes the axis
	 * that corresponds to the greatest value of thrust as the thrust axis.
	 *
	 * This method has produced the same thrust axis as the original
	 * algorithm, implemented here as calcT_orig().  But it is yet to be
	 * proven that this method is perfectly accurate, therefore it is
	 * disfavoured for now.
	 */

	// Do not consider the case of zero tracks
	if (m_ntracks < m_min_ntracks) {
		return std::pair<Vector3f, double> (Vector3f(), -1.);
	}

	std::vector<Vector3f> tvecs;
	std::vector<double> tvals;

	for (unsigned int j = 0; j < pvec.size(); j++) {

		// Transform initial vector into thrust axis
		Vector3f axis (0, 0, 0);

		// Define a hemisphere by dot product with each input vector
		Vector3f init (pvec[j]);

		for (unsigned int i = 0; i < pvec.size(); i++) {
			init.dot(pvec[i]) >= 0. ? axis += pvec[i] : axis -= pvec[i];
		}
		axis /= axis.norm();

		// oint in the direction of greatest energy flow
		double eflow = 0.;
		for (const auto& p : pvec) eflow += axis.dot(p);
		if (eflow < 0.) axis = -axis;

		// Get value of thrust about the calculated thrust axis
		double t = calcThrustValue(pvec, axis);

		tvecs.push_back(axis);
		tvals.push_back(t);
	}

	// Find the best thrust axis
	double thrust = 0.;
	Vector3f thrust_axis;
	for (unsigned int i = 0; i < tvecs.size(); i++) {
		if (tvals[i] > thrust) {
			thrust = tvals[i];
			thrust_axis = tvecs[i];
		}
	}

	return std::pair<Vector3f, double> (thrust_axis, thrust);
}

const std::pair<Vector3f, double> EventShapes::calcT_orig(const std::vector<Vector3f>& three_momenta) {
	/* Method based on the original, validated algorithm supplied by
	 * Deepak Kar and Sukanya Sinha.  Based on the iterative method
	 * described in the ythia 6.4 Manual.
	 *
	 * This implementation starts from n random initial axes to build
	 * a resultant vector as a candidate thrust axis.  The normalised
	 * resultant that has the greatest value of thrust is taken as the
	 * thrust axis.
	 */

	// Alias vector of input three-vectors
	std::vector<Vector3f> pvec = three_momenta;

	// Do not consider the case of zero tracks
	if (m_ntracks < m_min_ntracks) {
		return std::pair<Vector3f, double> (Vector3f(), -1.);
	}

	// Get thrust axis
	Vector3f tvec;
	double best_thrust = 0.;

	// Start from multiple random initial axes
	for (unsigned int i = 0; i < pow(pvec.size(), 2); i++) {

		double x, y, z;
		double r = 1.;
		m_randg.Sphere(x, y, z, r);
		Vector3f init (x, y, z);

		// Vector3f init (m_randg.Rndm(), m_randg.Rndm(), m_randg.Rndm());
		Vector3f axis (init);

		// Iterate the axis to local maximum
		double diff = 999.;
		while (diff > 1e-5) {
			Vector3f foo(0, 0, 0);
			for (const auto& p : pvec) {
				axis.dot(p) > 0 ? foo += p : foo -= p;
			}
			foo /= foo.norm();
			diff = (axis - foo).norm();
			axis = foo;
		}

		// Keep the axis if it increases the thrust value
		double thrust = calcThrustValue(pvec, axis);
		if (thrust > best_thrust) {
			tvec = axis;
			best_thrust = thrust;
		}
	}

	// oint in the direction of greatest energy flow
	double eflow = 0.;
	for (const auto& p : pvec) eflow += tvec.dot(p);
	if (eflow <= 0.) tvec = -tvec;

	return std::pair<Vector3f, double> (tvec, best_thrust);
}

void EventShapes::calcThrust() {
	/* Function that calculates the thrust proper axis and values.
	 * The class member data are set with the calculated values.
	 */

	std::pair<Vector3f, double> pair = calcT_orig(m_three_momenta);

	// Check validity of results
	if (pair.first == Vector3f()) { // Calculation unsuccessful
		m_thrust_axis = nullptr;
		m_thrust = -1.;
	} else {
		m_thrust_axis = new Vector3f(pair.first);
		m_thrust = pair.second;
	}

	std::cout << "Thrust axis: " << std::endl;
	if (m_thrust_axis) {
		std::cout << *m_thrust_axis << std::endl;
	} else {
		std::cout << "0" << std::endl;
	}

}

void EventShapes::calcThrustMajor() {
	/* Function that calculates the thrust major axis and value.
	 * This is the axis in the plane perpendicular to the thrust axis
	 * along which the energy flow is greatest in that plane.
	 * hys. Rev. Lett. 43, 830 for a nice discussion.
	 */

	// // Do not consider the case of zero tracks
	// if (m_ntracks < m_min_ntracks) {
	// 	m_thrust_major_axis = nullptr;
	// 	m_thrust_major = -1.;
	// 	return;
	// }

	const std::vector<Vector3f> pvec = m_three_momenta;

	// Function depends on the thrust axis
	if (!m_thrust_axis) calcThrust();

	const Vector3f thrust_axis (*m_thrust_axis);
	std::cout << "Thrust axis used for thrust major axis: " << std::endl;
	std::cout << thrust_axis << std::endl;

	// // Check that the thrust major axis exists
	// // i.e. that there are vectors in the plane perp to the thrust axis
	// // Unnecessary except when n input vectors < 3
	// if (m_ntracks < 3) {
	// 	bool no_thrust_major (true);
	// 	for (const auto& p : pvec) {
	// 		if ((p - p.dot(thrust_axis) * thrust_axis).squaredNorm() > 1e-10) {
	// 			no_thrust_major = false;
	// 			break;
	// 		}
	// 	}

	// 	if (no_thrust_major) {
	// 		m_thrust_major_axis = nullptr;
	// 		m_thrust_major = -1.;
	// 		return;
	// 	}
	// }

	// roject input vectors into plane perpendicular to thrust axis proper
	std::vector<Vector3f> pvec_perp;
	pvec_perp.reserve(pvec.size());
	for (const auto& p : pvec) {
		pvec_perp.push_back(p - p.dot(thrust_axis) * thrust_axis);
	}

	std::pair<Vector3f, double> thrust_major = calcT_orig(pvec_perp);
	// std::cout << "Thrust major value, n, vector: " << thrust_major.second << ", " << pvec.size() << ", ";
	// thrust_major.first.rint();

	Vector3f thrust_major_axis;
	if (thrust_major.first == Vector3f()) {
		m_thrust_major_axis = nullptr;
		m_thrust_major = -1.;
		return;
	} else {
		thrust_major_axis = thrust_major.first;
	}

	// Sanity check
	if (fabs(thrust_axis.dot(thrust_major_axis)) > 1e-6) {
		std::cout << "Major axis not orthogonal to proper axis!" << std::endl;
		std::cout << fabs(thrust_axis.dot(thrust_major_axis)) << std::endl;
	}

	// Set class data members
	m_thrust_major_axis = new Vector3f(thrust_major.first);
	m_thrust_major = thrust_major.second;

	std::cout << "Thrust major axis: " << std::endl;
	if (m_thrust_major_axis) {
		std::cout << *m_thrust_major_axis << std::endl;
	} else {
		std::cout << "0" << std::endl;
	}
}

void EventShapes::calcThrustMinor() {
	/* Function that calculates the thrust minor axis and value.
	 * This axis is defined as being orthogonal to the thrust and
	 * thrust major axes.
	 * hys. Rev. Lett. 43, 830 for a nice discussion.
	 */

	// Do not consider the case where the thrust or thrust major values were invalid
	if (m_thrust == -1. || m_thrust_major == -1.) {
		m_thrust_minor_axis = nullptr;
		m_thrust_minor = -1.;
		return;
	}

	const std::vector<Vector3f> pvec = m_three_momenta;

	// This function depends on the thrust and thrust major axes
	if (!m_thrust_major_axis) calcThrustMajor();

	// Alias the existing axes
	const Vector3f thrust_axis = *m_thrust_axis;
	const Vector3f thrust_major_axis = *m_thrust_major_axis;

	Vector3f thrust_minor_axis = thrust_axis.cross(thrust_major_axis);
	double thrust_minor = calcThrustValue(pvec, thrust_minor_axis);

	// Sanity check
	if (fabs(thrust_axis.dot(thrust_minor_axis)) > 1e-10 || fabs(thrust_major_axis.dot(thrust_minor_axis)) > 1e-10) {
		std::cout << "Minor axis not orthogonal to major and proper!" << std::endl;
	}

	// Set class data members
	m_thrust_minor_axis = new Vector3f(thrust_minor_axis);
	m_thrust_minor = thrust_minor;
}

void EventShapes::calcOblateness() {
	/* Function that calculates the oblateness about the thrust axis.
	 */

	// This function depends on the major and minor thrust calculations
	if (!m_thrust_minor_axis) calcThrustMinor();

	m_oblateness = m_thrust_major - m_thrust_minor;
}

void EventShapes::calcBrd() {
	/* Function that calculates the event broadening with respect to
	 * the thrust axis
	 */

	// Do not consider the case of zero tracks
	if (m_ntracks < m_min_ntracks) {
		m_broadening = -1.;
		return;
	}

	// Alias vector of input vectors
	const std::vector<Vector3f> pvec = m_three_momenta;

	// Depends on the thrust axis
	if (!m_thrust_axis) calcT_orig(m_three_momenta);

	// Alias thrust axis
	const Vector3f thrust_axis = *m_thrust_axis;

	// Calculate broadening in Up and Down hemispheres separately
	double B_U (0.), B_D (0.);
	double B_norm (0.);
	// std::cout << "New event:" << std::endl;
	for (const auto& p : pvec) {
		// std::cout << "Dot: " << thrust_axis.Dot(p) << std::endl;
		B_norm += p.norm();
		if (thrust_axis.dot(p) > 0) {
			B_U += p.cross(thrust_axis).norm();
		} else {
			B_D += p.cross(thrust_axis).norm();
		}
	}

	// std::cout << "B_D, B_D_norm = " << B_D << ", " << B_norm << std::endl;
	// std::cout << "B_U, B_U_norm = " << B_U << ", " << B_norm << std::endl;
	// std::cout << std::endl;

	B_U = B_U > 0. ? B_U / B_norm : 0.;
	B_D = B_D > 0. ? B_D / B_norm : 0.;

	double B = B_D + B_U;

	// std::cout << "Broadening: " << B << std::endl;

	m_broadening = B;

}

void EventShapes::calcLinSph() {
	/* Function that calculates the sphericity tensor and its eigen
	 * vectors and values.
	 */

	// Alias vector of input vectors
	const std::vector<Vector3f> pvec = m_three_momenta;

	// Set sphericities to dummy values
	m_lin_spher_S = -1.;
	m_lin_spher_A = -1.;
	m_lin_spher_C = -1.;
	m_lin_spher_D = -1.;

	// Do not consider the case of zero tracks
	if (m_ntracks < m_min_ntracks) return;

	// Construct the sphericity tensor
	double a11 = 0.; double a12 = 0.; double a13 = 0.;
	double a21 = 0.; double a22 = 0.; double a23 = 0.;
	double a31 = 0.; double a32 = 0.; double a33 = 0.;
	double norm = 0.;

	for (const auto& p : pvec){
		double mod (p.norm());
		norm += mod;

		std::cout << "Adding vector to sphericity tensor" << std::endl;
		std::cout << p << std::endl;

		a11 += p.x() * p.x() / mod;
		a22 += p.y() * p.y() / mod;
		a33 += p.z() * p.z() / mod;

		a12 += p.x() * p.y() / mod;
		a13 += p.x() * p.z() / mod;
		a23 += p.y() * p.z() / mod;
	}

	// Fill symmetric elements of sphericity tensor
	a21 = a12; a31 = a13; a32 = a23;

	double s11 = a11 / norm; double s12 = a12 / norm; double s13 = a13 / norm;
	double s21 = a21 / norm; double s22 = a22 / norm; double s23 = a23 / norm;
	double s31 = a31 / norm; double s32 = a32 / norm; double s33 = a33 / norm;

	// Calculate the eigenvalues
	Matrix3f eigen_problem;
	eigen_problem <<
		s11, s12, s13,
		s21, s22, s23,
		s31, s32, s33;
	std::cout << "eigen problem: " << std::endl;
	std::cout << eigen_problem << std::endl;

	SelfAdjointEigenSolver<Matrix3f> eigen_solver(eigen_problem);

	std::cout << "Eigenvalues are: " << std::endl;
	auto eigen_values = eigen_solver.eigenvalues();
	std::cout << eigen_values << std::endl;

	// Compute sphericity variables and set class data members
	if (ndims == 3) {
		double S = (eigen_values[1] + eigen_values[0]) * 3./2.;
		double A = eigen_values[0] * 3./2.;
		double C = (eigen_values[2] * eigen_values[1]
					+ eigen_values[2] * eigen_values[0]
					+ eigen_values[1] * eigen_values[0]) * 3.;
		double D = 27. * eigen_values[2] * eigen_values[1] * eigen_values[0];

		m_lin_spher_S = S;
		m_lin_spher_A = A;
		m_lin_spher_C = C;
		m_lin_spher_D = D;

	} else if (ndims == 2) {
		double S = eigen_values[1] * 2.;
		double C = eigen_values[0] * eigen_values[1] * 4.;

		m_lin_spher_S = S;
		m_lin_spher_C = C;

	}

	std::cout << "New event:" << std::endl;
	std::cout << "S = " << m_lin_spher_S << std::endl;
	std::cout << "A = " << m_lin_spher_A << std::endl;
	std::cout << "C = " << m_lin_spher_C << std::endl;
	std::cout << "D = " << m_lin_spher_D << std::endl;
	std::cout << std::endl;

}

void EventShapes::calc_all() {
	calcThrust();
	calcThrustMajor();
	calcThrustMinor();
	calcOblateness();
	calcBrd();
	calcLinSph();
}

EventShapes::~EventShapes() {
	delete m_thrust_axis;
	delete m_thrust_major_axis;
	delete m_thrust_minor_axis;
}


#include <iostream>

#include <posit/posit>
#include <boost/range/combine.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;
using namespace sw::unum;
using boost::multiprecision::cpp_dec_float_100;

cpp_dec_float_100 decimal_accuracy(cpp_dec_float_100 exact, cpp_dec_float_100 computed) {
    if (boost::math::isnan(exact) || boost::math::isnan(computed) ||
        (boost::math::sign(exact) != boost::math::sign(computed))) {
        return std::numeric_limits<cpp_dec_float_100>::quiet_NaN();
    } else if (exact == computed) {
        return std::numeric_limits<cpp_dec_float_100>::infinity();
    } else if ((exact == std::numeric_limits<cpp_dec_float_100>::infinity() &&
                computed != std::numeric_limits<cpp_dec_float_100>::infinity()) ||
               (exact != std::numeric_limits<cpp_dec_float_100>::infinity() &&
                computed == std::numeric_limits<cpp_dec_float_100>::infinity()) || (exact == 0 && computed != 0) ||
               (exact != 0 && computed == 0)) {
        return -std::numeric_limits<cpp_dec_float_100>::infinity();
    } else {
        return -log10(abs(log10(computed / exact)));
    }
}

int main() {

    ofstream outfile("da.txt", ios::out);
    outfile << "Q,da_f,da_p2,da_p3" << endl;

    for(int Q = 1; Q <= 100; Q++) {
        cpp_dec_float_100 dec = pow(10.0, -(cpp_dec_float_100)Q/10);

        float f = powl(10.0, -(long double)Q/10);
        posit<32,2> p2 = powl(10.0, -(long double)Q/10);
        posit<32,3> p3 = powl(10.0, -(long double)Q/10);

        cpp_dec_float_100 da_f, da_p2, da_p3;
        da_f = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(f));
        da_p2 = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(p2));
        da_p3 = decimal_accuracy(dec, static_cast<cpp_dec_float_100>(p3));

        outfile << Q << ",";
        outfile << setprecision(100) << fixed << da_f << "," << da_p2 << "," << da_p3 << endl << flush;
    }
    outfile.close();

}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOGIT_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOGIT_HPP

#include <boost/math/tools/promotion.hpp>

namespace stan {
namespace math {

/**
 * Return the log odds of the argument.
 *
 * The logit function is defined as for \f$x \in [0, 1]\f$ by
 * returning the log odds of \f$x\f$ treated as a probability,
 *
 * \f$\mbox{logit}(x) = \log \left( \frac{x}{1 - x} \right)\f$.
 *
 * The inverse to this function is inv_logit.
 *
 *
 \f[
 \mbox{logit}(x) =
 \begin{cases}
 \textrm{NaN}& \mbox{if } x < 0 \textrm{ or } x > 1\\
 \ln\frac{x}{1-x} & \mbox{if } 0\leq x \leq 1 \\[6pt]
 \textrm{NaN} & \mbox{if } x = \textrm{NaN}
 \end{cases}
 \f]

 \f[
 \frac{\partial\, \mbox{logit}(x)}{\partial x} =
 \begin{cases}
 \textrm{NaN}& \mbox{if } x < 0 \textrm{ or } x > 1\\
 \frac{1}{x-x^2}& \mbox{if } 0\leq x\leq 1 \\[6pt]
 \textrm{NaN} & \mbox{if } x = \textrm{NaN}
 \end{cases}
 \f]
 *
 * @param u argument
 * @return log odds of argument
 */
inline double logit(double u) {
  using std::log;
  return log(u / (1 - u));
}

/**
 * Return the log odds of the argument.
 *
 * @param u argument
 * @return log odds of argument
 */
inline double logit(int u) { return logit(static_cast<double>(u)); }

}  // namespace math
}  // namespace stan
#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_STATISTICS_FUNCTIONS_GENERIC_EVSTAT_HPP_INCLUDED
#define NT2_STATISTICS_FUNCTIONS_GENERIC_EVSTAT_HPP_INCLUDED
#include <nt2/statistics/functions/evstat.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <nt2/include/constants/euler.hpp>
#include <nt2/include/constants/oneo_6.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/functions/sqr.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)(A2)(A3)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                      (scalar_ < floating_<A2> > )
                                      (scalar_ < floating_<A3> > )
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & m, A3 & v) const
    {
      m = mu-Euler<A0>()*sigma;
      v = sqr(Pi<A0>()*sigma)*Oneo_6<A0>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)(A2)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                      (scalar_ < floating_<A2> > )
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()( A0 const& mu, A1 const& sigma
                                            , A2 & v) const
    {
      v = sqr(Pi<result_type>()*sigma)*Oneo_6<result_type>();
      return mu-Euler<result_type>()*sigma;
    }
  };

  BOOST_DISPATCH_IMPLEMENT   ( evstat_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_ < floating_<A0> > )
                                      (scalar_ < floating_<A1> > )
                                    )
  {
    typedef std::pair<A0,A0> result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& mu, A1 const& sigma) const
    {
      return result_type(mu-Euler<A0>()*sigma
                        , sqr(Pi<A0>()*sigma)*Oneo_6<A0>());
    }
  };

} }

#endif


/*!
 * \file khovanov.cpp
 * \author Jun Yoshida
 * \copyright (c) 2020 Jun Yoshida.
 * The project is released under the 2-clause BSD License.
 * \date August, 2020: created
 */

#include <vector>

#include <Eigen/Dense>

#include "khovanov.hpp"
#include "cubes.hpp"
#include "enhancements.hpp"

/* Debug
#include "debug/debug.hpp"
//*/

using namespace khover;

using matrix_t = ChainIntegral::matrix_t;

/***********************************
 *** Implementation of functions ***
 ***********************************/
// Compute Khovanov complex of a given link diagram.
std::optional<ChainIntegral>
khover::khChain(
    LinkDiagram const& diagram,
    SmoothCube const& cube,
    std::vector<EnhancementProperty> const& enh_prop
    //int qdeg
    ) noexcept
{
    // Compute the matrices representing matrices.
    ChainIntegral result(
        -diagram.npositive()-1,
        matrix_t(
            0, binom(cube.back().ncomp, enh_prop.back().xcnt)));
    for(int i = cube.dim(); i > 0; --i) {
        std::size_t maxst_cod = cube.maxState(i).to_ulong();
        std::size_t maxst_dom = cube.maxState(i-1).to_ulong();

        matrix_t diffmat = matrix_t::Zero(
            enh_prop[maxst_cod].headidx
            + binom(cube[maxst_cod].ncomp, enh_prop[maxst_cod].xcnt),
            enh_prop[maxst_dom].headidx
            + binom(cube[maxst_dom].ncomp, enh_prop[maxst_dom].xcnt));

        // Traverse all the state pairs
        for(std::size_t stidx_cod = 0;
            stidx_cod < binom(cube.dim(), i);
            ++stidx_cod)
        {
            // The state associated with the index.
            auto st_cod = bitsWithPop<max_crosses>(i, stidx_cod).to_ullong();

            for(std::size_t stidx_dom = 0;
                stidx_dom < binom(cube.dim(), i-1);
                ++stidx_dom)
            {
                // The state associated with the index.
                auto st_dom = bitsWithPop<max_crosses>(i-1, stidx_dom).to_ullong();

                // The coefficient between the domain/codomain states.
                auto coeff = diagram.stateCoeff(
                    state_t{st_dom}, state_t{st_cod});

                // Skip the case where the codomain state is not adjacent to the domain state.
                if(coeff == 0)
                    continue;

                // Find the position where saddle operation is applied.
                for(std::size_t arc = 0; arc < diagram.narcs(); ++arc) {
                    // The saddle causes multiplication.
                    if(cube[st_cod].arccomp[arc] < cube[st_dom].arccomp[arc])
                    {
                        for(auto [r,c] : matrix_mult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_cod].arccomp[arc],
                                cube[st_dom].arccomp[arc])) {
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coeff;
                        }
                        break;
                    }
                    // The saddle causes comultiplication
                    else if(cube[st_cod].arccomp[arc]
                            > cube[st_dom].arccomp[arc])
                    {
                        for(auto [r,c] : matrix_comult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_dom].arccomp[arc],
                                cube[st_cod].arccomp[arc]))
                        {
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coeff;
                        }
                        break;
                    }
                }
            }
        }

        // Append the matrix as a differential.
        if (!result.prepend(std::move(diffmat))) {
            std::cerr << "Wrong size matrix..." << std::endl;
            return std::nullopt;
        }
    }

    return std::make_optional(std::move(result));
}

// Compute crux complex of a given link diagram and a given crossing.
std::optional<ChainIntegral>
khover::cruxChain(
    LinkDiagram const& diagram,
    std::size_t dblpt,
    CruxCube const& cube,
    std::vector<EnhancementProperty> const& enh_prop
    //int qdeg
    ) noexcept
{
    // The double point is out-of-range.
    if(dblpt >= diagram.ncrosses()) {
        return std::nullopt;
    }

    // Compute the matrices representing matrices.
    ChainIntegral result(
        -diagram.npositive() - (diagram.getSign(dblpt) > 0 ? 0 : 1),
        matrix_t(0, binom(cube.back().ncomp, enh_prop.back().xcnt))
        );

    for(int i = diagram.ncrosses()-1; i > 0; --i) {
        std::size_t maxst_cod = cube.maxState(i).to_ulong();
        std::size_t maxst_dom = cube.maxState(i-1).to_ulong();
        matrix_t diffmat = matrix_t::Zero(
            enh_prop[maxst_cod].headidx
            + binom(cube[maxst_cod].ncomp, enh_prop[maxst_cod].xcnt),
            enh_prop[maxst_dom].headidx
            + binom(cube[maxst_dom].ncomp, enh_prop[maxst_dom].xcnt)
            );

        // Traverse all the state pairs
        for(std::size_t stidx_cod = 0;
            stidx_cod < binom(diagram.ncrosses()-1, i);
            ++stidx_cod)
        {
            // The state associated with the index.
            auto stbits_cod = bitsWithPop<max_crosses>(i, stidx_cod);
            auto st_cod = stbits_cod.to_ulong();

            // Skip states that has no enhancement in the q-degree.
            if(enh_prop[st_cod].xcnt < 0
               || (enh_prop[st_cod].xcnt
                   > static_cast<int>(cube[st_cod].ncomp)))
            {
                continue;
            }

            for(std::size_t c = 0; c < diagram.ncrosses()-1; ++c) {
                if(!stbits_cod.test(c))
                    continue;

                auto stbits_dom = stbits_cod;
                stbits_dom.set(c,false);
                auto st_dom = stbits_dom.to_ulong();

                // Skip states that has no enhancement in the q-degree.
                if(enh_prop[st_dom].xcnt < 0
                   || (enh_prop[st_dom].xcnt
                       > static_cast<int>(cube[st_cod].ncomp)))
                {
                    continue;
                }

                // The coefficient between the domain/codomain states.
                auto coeff = diagram.stateCoeff(
                    cube[st_dom].state, cube[st_cod].state);

                // Skip the case where the codomain state is not adjacent to the domain state.
                if(coeff == 0)
                    continue;

                // Find the position where saddle operation is applied.
                for(std::size_t arc = 0; arc < diagram.narcs(); ++arc) {
                    // The saddle causes multiplication.
                    if(cube[st_cod].arccomp[arc]
                       < cube[st_dom].arccomp[arc])
                    {
                        // Enabled if the operation is involved with twisted arcs.
                        std::optional<std::size_t> action_arc;
                        if ((cube[st_dom].twist
                             ^ cube[st_cod].twist).any())
                        {
                            // In that case, action_arc is a non-twisted arc that acts on the twisted arc.
                            if(cube[st_dom].twist.test(arc))
                                action_arc = cube[st_cod].arccomp[arc];
                            else
                                action_arc = cube[st_dom].arccomp[arc];
                        }

                        for(auto [r,c] : matrix_mult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_cod].arccomp[arc],
                                cube[st_dom].arccomp[arc]))
                        {
                            // -1 if 'x' on the act circle.
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += action_arc && bitsWithPop<max_components>(
                                    enh_prop[st_dom].xcnt, c).test(*action_arc)
                                ? -coeff : coeff;
                        }
                        break;
                    }
                    // The saddle causes comultiplication
                    else if (cube[st_cod].arccomp[arc]
                             > cube[st_dom].arccomp[arc])
                    {
                        // Enabled if the operation is involved with twisted arcs.
                        std::optional<std::size_t> coact_arc;
                        if ((cube[st_dom].twist
                             ^ cube[st_cod].twist).any())
                        {
                            // In that case, coact_arc is a non-twisted arc that coacts on the twisted arc.
                            if(cube[st_cod].twist.test(arc))
                                coact_arc = cube[st_dom].arccomp[arc];
                            else
                                coact_arc = cube[st_cod].arccomp[arc];
                        }

                        for(auto [r,c] : matrix_comult(
                                enh_prop[st_cod].xcnt,
                                cube[st_cod].ncomp,
                                cube[st_dom].arccomp[arc],
                                cube[st_cod].arccomp[arc]))
                        {
                            // -1 if '1' on the coact circle.
                            diffmat.coeffRef(
                                enh_prop[st_cod].headidx+r,
                                enh_prop[st_dom].headidx+c
                                ) += coact_arc && bitsWithPop<max_components>(
                                    enh_prop[st_cod].xcnt, r).test(*coact_arc)
                                ? coeff : -coeff;
                        }
                        break;
                    }
                }
            }
        }

        // Append the matrix as a differential.
        if (!result.prepend(std::move(diffmat))) {
            std::cerr << "Wrong size matrix..." << std::endl;
            return std::nullopt;
        }
    }

    return std::optional<ChainIntegral>(std::move(result));
}


#ifndef REGISTRATION_COST_HPP
#define REGISTRATION_COST_HPP

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

struct LiTAMIN2CostFunction
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LiTAMIN2CostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov,
                           Eigen::Matrix3d lambdaI,double sigma_ICP) :
                        p_mean_(p_mean), q_mean_(q_mean), p_cov_(p_cov), q_cov_(q_cov),lambdaI_(lambdaI),sigma_ICP_(sigma_ICP) {}

    template <typename T>
    bool operator()(const T *const q, const T *const t, T *residuals) const
    {
        Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(residuals);
        Eigen::Matrix<T, 3, 1> p_m(p_mean_.cast<T>());
        Eigen::Matrix<T, 3, 1> q_m(q_mean_.cast<T>());
        Eigen::Matrix<T, 3, 3> p_c = p_cov_.cast<T>();
        Eigen::Matrix<T, 3, 3> q_c = q_cov_.cast<T>();
        Eigen::Matrix<T, 3, 3> lambI = lambdaI_.cast<T>();
        Eigen::Quaternion<T> quat(q);
        Eigen::Matrix<T, 3, 1> translation(t);

        Eigen::Matrix<T, 3, 3> mahalanobis = (q_c + quat * p_c * (quat.inverse()) + lambdaI_).inverse();
        mahalanobis.normalize();
        Eigen::Matrix<T,3,3> LT = mahalanobis.llt().matrixL().transpose();
        residuals_map = LT * (q_m - (quat * p_m + translation));
        T EICP = T(residuals_map.squaredNorm());
        T sigma_square = T(sigma_ICP_*sigma_ICP_);
        residuals_map = (T(1.)-(EICP/(EICP+sigma_square)))*residuals_map;

        return true;
    }

    static ceres::CostFunction *Create(Eigen::Vector3d p_mean_, Eigen::Vector3d q_mean_, Eigen::Matrix3d p_cov_,
    Eigen::Matrix3d q_cov_,Eigen::Matrix3d lambdaI_, double sigma_ICP_){
        // 残差是三维的,变量分别是思维和三维
        return (new ceres::AutoDiffCostFunction<LiTAMIN2CostFunction,3,4,3>
        (new LiTAMIN2CostFunction(p_mean_,q_mean_,p_cov_,q_cov_,lambdaI_,sigma_ICP_)));
    }

    Eigen::Vector3d p_mean_, q_mean_;
    Eigen::Matrix3d p_cov_, q_cov_;
    Eigen::Matrix3d lambdaI_;
    double sigma_ICP_;
};
#endif


#include <NTL/ZZ.h>
#include <cstdint>
#include <cmath>

#define NUM_BITS_REAL_MANTISSA  128
#define IGNORE_DECODING_COST      0
#define SKIP_BJMM 0
#define LOG_COST_CRITERION 1

#include "proper_primes.hpp"
#include "binomials.hpp"
#include "bit_error_probabilities.hpp"
#include "partitions_permanents.hpp"
#include "isd_cost_estimate.hpp"
#include <cmath>

uint32_t estimate_t_val(const uint32_t c_sec_level,
                        const uint32_t q_sec_level,
                        const uint32_t n_0,
                        const uint32_t p){
    double achieved_c_sec_level = c_sec_level;
    double achieved_q_sec_level = q_sec_level;
    uint32_t lo = 1, t, t_prec;
    uint32_t hi;
    hi = p < 4*c_sec_level ? p : 4*c_sec_level;
    t = lo;
    t_prec = lo;
    while (hi - lo > 1){
       t_prec = t;
       t = (lo + hi)/2;
       std::cerr << "testing t " <<  t << std::endl;
       achieved_c_sec_level = c_isd_log_cost(n_0*p,((n_0-1)*p),t,p,0);
       achieved_q_sec_level = q_isd_log_cost(n_0*p,((n_0-1)*p),t,p,0);
       if ( (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)    ){
         hi = t;
       } else {
         lo = t;
       }
    }
    if( (achieved_c_sec_level >= c_sec_level) &&
        (achieved_q_sec_level >= q_sec_level)    ){
        return t;
    }
    return t_prec;
}

int ComputeDvMPartition(const uint64_t d_v_prime,
                        const uint64_t n_0,
                        uint64_t mpartition[],
                        uint64_t &d_v){
     d_v = floor(sqrt(d_v_prime));
     d_v = (d_v & 0x01) ? d_v : d_v + 1;
     uint64_t m = ceil( (double) d_v_prime / (double) d_v );

     int partition_ok;
     partition_ok = FindmPartition(m,mpartition,n_0);

     while(!partition_ok  && (d_v_prime/d_v) >= n_0){
          d_v += 2;
          m = ceil( (double) d_v_prime / (double) d_v );
          partition_ok = FindmPartition(m,mpartition,n_0);
     }
     return partition_ok;
}

uint64_t estimate_dv (const uint32_t c_sec_level, // expressed as
                      const uint32_t q_sec_level,
                      const uint32_t n_0,
                      const uint32_t p,
                      uint64_t mpartition[]){
    double achieved_c_sec_level = 0.0;
    double achieved_q_sec_level = 0.0;
    double achieved_c_enum_sec_level = 0.0;
    double achieved_q_enum_sec_level = 0.0;

    NTL::ZZ keyspace;

    uint32_t lo = 1, d_v_prime, hi;
    uint64_t d_v,d_v_prec =0;
    int found_dv_mpartition = 0;
    // recalling that the weight of the sought codeword in a KRA is
    // d_c_prime = n_0 * d_v_prime, d_c_prime < p
    // d_v_prime should not be greater than p/n_0
    hi = (p/n_0) < 4*c_sec_level ? (p/n_0) : 4*c_sec_level;
    d_v_prime = lo;
    d_v = (int) sqrt(lo);

    while (hi - lo > 1){
       d_v_prec = d_v;
       d_v_prime = (lo + hi)/2;
       found_dv_mpartition = ComputeDvMPartition(d_v_prime,n_0,mpartition,d_v);
       if(found_dv_mpartition) {
          keyspace = 1;
          for(int i =0; i < (int)n_0; i++){
              keyspace *= binomial_wrapper(p,mpartition[i]);
          }
          keyspace = NTL::power(keyspace,n_0);
          keyspace += NTL::power(binomial_wrapper(p,d_v),n_0);
          achieved_c_enum_sec_level = NTL::conv<double>(log2_RR(NTL::to_RR(keyspace)));
          achieved_q_enum_sec_level = achieved_c_enum_sec_level/2;
          if ((achieved_q_enum_sec_level >= q_sec_level) &&
              (achieved_c_enum_sec_level >= c_sec_level) ){
              /* last parameter indicates a KRA, reduce margin by p due to
              quasi cyclicity */
              achieved_c_sec_level = c_isd_log_cost(n_0*p,p,n_0*d_v_prime,p,1);
              achieved_q_sec_level = q_isd_log_cost(n_0*p,p,n_0*d_v_prime,p,1);
          }
       }

       if ( (found_dv_mpartition) &&
            (achieved_q_enum_sec_level >= q_sec_level) &&
            (achieved_c_enum_sec_level >= c_sec_level) &&
            (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)   ){
          hi = d_v_prime;
       } else {
          lo = d_v_prime;
       }
    }

    if ( (found_dv_mpartition) &&
            (achieved_q_enum_sec_level >= q_sec_level) &&
            (achieved_c_enum_sec_level >= c_sec_level) &&
            (achieved_c_sec_level >= c_sec_level) &&
            (achieved_q_sec_level >= q_sec_level)   ){
        return d_v;
    }
    return d_v_prec;
}

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

  if(argc != 6){
     std::cout << "Code Parameter Computer for LEDA[kem|pkc]" << std::endl << " Usage "
               << argv[0] << " security_level_classic security_level_pq n_0 epsilon starting_prime_lb" << std::endl;
    return -1;
  }
  uint32_t c_sec_level = atoi(argv[1]);
  uint32_t q_sec_level = atoi(argv[2]);
  uint32_t n_0 = atoi(argv[3]);
  float epsilon = atof(argv[4]);
  uint32_t starting_prime_lower_bound = atoi(argv[5]);

  std::cerr << "Computing the parameter set for security level classic:2^" <<
  c_sec_level << " post-q:2^" << q_sec_level << " n_0 " << n_0 << " epsilon "
  << epsilon << std::endl;

  uint64_t p, p_th, t, d_v_prime, d_v;
  uint64_t mpartition[n_0] = {0};

  int current_prime_pos = 0;
  while (proper_primes[current_prime_pos] < starting_prime_lower_bound){
          current_prime_pos++;
  }
  p_th = proper_primes[current_prime_pos];

  InitBinomials();
  NTL::RR::SetPrecision(NUM_BITS_REAL_MANTISSA);
  pi = NTL::ComputePi_RR();

  /* since some values of p may yield no acceptable partitions for m, binary
   * search on p is not feasible. Fall back to fast increase of the value of
   * p exploiting the value of the p expected to be correcting the required t
   * errors */

  std::cout << "finding parameters" << std::endl;
  do {
      /* estimate the current prime as the closest
       * to the previous p_th * (1+epsilon) */
      uint32_t next_prime = ceil(p_th * (1.0+epsilon));
      current_prime_pos = 0;
      while (proper_primes[current_prime_pos] < next_prime){
          current_prime_pos++;
      }
      p = proper_primes[current_prime_pos];
      std::cout << " -- testing p: " << p << std::endl;

      // Estimate number of errors to ward off ISD decoding
      t = estimate_t_val(c_sec_level,q_sec_level,n_0,p);
      std::cout << " -- found t: " << t << std::endl;

      /* Estimate H*Q density to avoid key recovery via ISD and enumeration
       * of H and Q */
      d_v = estimate_dv(c_sec_level,q_sec_level,n_0,p,mpartition);
      std::cout << " -- found d_v: " << d_v << std::endl;

      // Estimate the bit flipping thresholds and correction capability
      d_v_prime=0;
      for(int i=0;i< (int)n_0;i++){
         d_v_prime += mpartition[i];
      }
      d_v_prime = d_v * d_v_prime;
      p_th=Findpth(n_0, d_v_prime, t);
      std::cout << " -- p should be at least " << (1.0+epsilon)* p_th <<
         "to correct the errors" << std::endl;
  }  while ((p <= (1.0+epsilon)* p_th) &&
            (current_prime_pos < PRIMES_NO) );

  std::cout << "refining parameters" << std::endl;

  uint64_t p_ok, t_ok, d_v_ok, mpartition_ok[n_0] = {0};
  /* refinement step taking into account possible invalid m partitions */

  do {
      p = proper_primes[current_prime_pos];
      std::cout << " -- testing p: " << p << std::endl;

      // Estimate number of errors to ward off ISD decoding
      t = estimate_t_val(c_sec_level,q_sec_level,n_0,p);
      std::cout << " -- found t: " << t << std::endl;

      /* Estimate H*Q density to avoid key recovery via ISD and enumeration
       * of H and Q */
      d_v = estimate_dv(c_sec_level,q_sec_level,n_0,p,mpartition);
      std::cout << " -- found d_v: " << d_v << std::endl;

      // Estimate the bit flipping thresholds and correction capability
      d_v_prime=0;
      for(int i=0;i< (int)n_0;i++){
         d_v_prime += mpartition[i];
      }
      d_v_prime = d_v * d_v_prime;
      p_th=Findpth(n_0, d_v_prime, t);
       std::cout << " -- the threshold value for p to be correcting errors is " << p_th << std::endl;

      if(p > (1.0+epsilon)* p_th ) { //store last valid parameter set
       std::cout << " -- p is at least " << (1.0+epsilon)*p_th <<
       "; it corrects the errors" << std::endl;
           p_ok = p; t_ok = t; d_v_ok = d_v;
           for(unsigned i = 0; i < n_0 ; i++){
               mpartition_ok[i] = mpartition[i];
          }
      }
      current_prime_pos--;
  }  while ((p > (1.0+epsilon)* p_th )  && (current_prime_pos > 0));

  std::cout << "parameter set found: p:" << p_ok << " t: " << t_ok;
  std::cout << " d_v : " << d_v_ok << " mpartition: [ ";
  for (unsigned i = 0; i < n_0 ; i++ ){
   std::cout << mpartition_ok[i] << " ";
  }
  std::cout << " ]" << std::endl;
  return 0;
}


/**
 * @author Alessandro Bianco
 */

/**
 * @addtogroup DFNs
 * @{
 */

#include "LeastSquaresMinimization.hpp"

#include <Errors/Assert.hpp>
#include <Macros/YamlcppMacros.hpp>

#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>

#include <stdlib.h>
#include <fstream>

using namespace PoseWrapper;
using namespace MatrixWrapper;
using namespace CorrespondenceMap3DWrapper;
using namespace Helpers;
using namespace BaseTypesWrapper;

namespace CDFF
{
namespace DFN
{
namespace Transform3DEstimation
{

LeastSquaresMinimization::LeastSquaresMinimization()
{
        parameters = DEFAULT_PARAMETERS;

	parametersHelper.AddParameter<float>("GeneralParameters", "MaximumAllowedError", parameters.maximumAllowedError, DEFAULT_PARAMETERS.maximumAllowedError);

	configurationFilePath = "";
	SetPosition(emptyPose, 0, 0, 0);
	SetOrientation(emptyPose, 0, 0, 0, 0);
}

LeastSquaresMinimization::~LeastSquaresMinimization()
{
}

void LeastSquaresMinimization::configure()
{
	parametersHelper.ReadFile(configurationFilePath);
	ValidateParameters();
}

void LeastSquaresMinimization::process()
{
	int numberOfCorrespondences = GetNumberOfCorrespondenceMaps(inMatches);
	Clear(outTransforms);
	for(int mapIndex = 0; mapIndex < numberOfCorrespondences; mapIndex++)
		{
		const CorrespondenceMap3D& map = GetCorrespondenceMap(inMatches, mapIndex);

		cv::Mat coefficientMatrix, valueMatrix;
		bool success = CreateLinearSystem(map, coefficientMatrix, valueMatrix);

		cv::Mat transformMatrix;
		if (success)
			{
			transformMatrix = SolveLinearSystem(coefficientMatrix, valueMatrix, outError);
			}

		if (!success || outError > parameters.maximumAllowedError)
			{
			if (numberOfCorrespondences == 1)
				{
				outSuccess = false;
				return;
				}
			else
				{
				AddPose(outTransforms, emptyPose);
				}
			}
		else
			{
			AddTransformOutput(transformMatrix);
			}
		}
	outSuccess = true;
}

bool LeastSquaresMinimization::CreateLinearSystem(const CorrespondenceMap3D& map, cv::Mat& coefficientMatrix, cv::Mat& valueMatrix)
	{
	const float EPSILON = 0.00001;
	const int NUMBER_OF_DEGREES_OF_FREEDOM = 6;
	const int NUMBER_OF_VARIABLES = 12;

	coefficientMatrix = cv::Mat();
	valueMatrix = cv::Mat();
	for(int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(map); correspondenceIndex++)
		{
		Point3D source = GetSource(map, correspondenceIndex);
		Point3D sink = GetSink(map, correspondenceIndex);

		if (source.x != source.x || source.y != source.y || source.z != source.z || sink.x != sink.x || sink.y != sink.y || sink.z != sink.z)
			{
			continue;
			}

		cv::Mat coefficientMatrixPart = ( cv::Mat_<float>(3, 12) <<
			source.x, source.y, source.z, 1,    0, 0, 0, 0,    0, 0, 0, 0,
			0, 0, 0, 0,   source.x, source.y, source.z, 1,     0, 0, 0, 0,
			0, 0, 0, 0,   0, 0, 0, 0,     source.x, source.y, source.z, 1 );
		cv::Mat valueMatrixPart = ( cv::Mat_<float>(3, 1) << sink.x, sink.y, sink.z);

		if (coefficientMatrix.rows == 0)
			{
			coefficientMatrix = coefficientMatrixPart;
			valueMatrix = valueMatrixPart;
			}
		else
			{
			cv::Mat matrixList[2] = {coefficientMatrix, coefficientMatrixPart};
			cv::vconcat(matrixList, 2, coefficientMatrix);
			matrixList[0] = valueMatrix;
			matrixList[1] = valueMatrixPart;
			cv::vconcat(matrixList, 2, valueMatrix);
			}
		}

	if ( coefficientMatrix.rows < NUMBER_OF_VARIABLES )
		{
		return false;
		}

	cv::Mat singulaValueMatrix;
	cv::SVD::compute(coefficientMatrix, singulaValueMatrix);
	int rank = cv::countNonZero( singulaValueMatrix > EPSILON );
	return ( rank >= NUMBER_OF_DEGREES_OF_FREEDOM );
	}

cv::Mat LeastSquaresMinimization::SolveLinearSystem(cv::Mat coefficientMatrix, cv::Mat valueMatrix, float& error)
	{
	cv::Mat pseudoInverse;
	cv::invert(coefficientMatrix, pseudoInverse, cv::DECOMP_SVD);
	cv::Mat transformMatrix = pseudoInverse * valueMatrix;

	cv::Mat errorMatrix = coefficientMatrix * transformMatrix - valueMatrix;
	error = cv::norm(errorMatrix);
	return transformMatrix;
	}

void LeastSquaresMinimization::AddTransformOutput(cv::Mat transformMatrix)
	{
	cv::Mat rotationMatrix = (cv::Mat_<float>(3, 3) <<
		transformMatrix.at<float>(0), transformMatrix.at<float>(1), transformMatrix.at<float>(2),
		transformMatrix.at<float>(4), transformMatrix.at<float>(5), transformMatrix.at<float>(6),
		transformMatrix.at<float>(8), transformMatrix.at<float>(9), transformMatrix.at<float>(10) );
	cv::Mat translationVector = (cv::Mat_<float>(3,1) << transformMatrix.at<float>(3), transformMatrix.at<float>(7), transformMatrix.at<float>(11));

	cv::Mat position = - rotationMatrix.inv() * translationVector;

	Pose3D pose;
	SetPosition(pose, position.at<float>(0, 0), position.at<float>(1, 0), position.at<float>(2, 0) );

	float qw = std::sqrt(1.00 + rotationMatrix.at<float>(0,0) + rotationMatrix.at<float>(1,1) + rotationMatrix.at<float>(2,2)) / 2;
	float qx = (rotationMatrix.at<float>(2,1) - rotationMatrix.at<float>(1,2)) / ( 4 * qw );
	float qy = (rotationMatrix.at<float>(0,2) - rotationMatrix.at<float>(2,0)) / ( 4 * qw );
	float qz = (rotationMatrix.at<float>(1,0) - rotationMatrix.at<float>(0,1)) / ( 4 * qw );
	SetOrientation(pose, qx, qy, qz, qw);

	AddPose(outTransforms, pose);
	}

const LeastSquaresMinimization::LeastSquaresMinimizationOptionsSet LeastSquaresMinimization::DEFAULT_PARAMETERS =
{
	/*.maximumAllowedError =*/ 0.001
};

void LeastSquaresMinimization::ValidateParameters()
{
	ASSERT(parameters.maximumAllowedError > 0, "LeastSquaresMinimization Configuration Error: maximumAllowedError has to be positive");
}

void LeastSquaresMinimization::ValidateInputs(const CorrespondenceMap3D& map)
{

}

}
}
}

/** @} */


#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <rr_common/CameraGeometry.h>
#include <sensor_msgs/Image.h>

#include <boost/algorithm/string.hpp>
#include <cmath>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
using namespace ros;

double px_per_meter;           // resolution of overhead view
double camera_dist_max;        // distance to look ahead of the car
double camera_dist_min;        // avoid the front bumper
double camera_fov_horizontal;  // radians
double camera_fov_vertical;
double cam_mount_angle;   // angle of camera from horizontal
double cam_mount_height;  // camera height from ground in meters
double cam_mount_x;       // distance from camera to base_footprint

Size mapSize;  // pixels = cm
Size imageSize;
Mat transform_matrix;

map<string, Publisher> transform_pubs;

/*
 * Start with a horizontal line on the groud at dmin meters horizonally in front of the
 * camera. It fills half the camera's FOV, from the center to the right edge. Then back
 * up the car so that the line is dmax meters away horizontally from the camera. The
 * apparent length of this hypothetical line (in pixels) is the output of this function.
 * See https://www.desmos.com/calculator/jsofcq1bi5
 * See https://drive.google.com/file/d/0Bw7-7Y3CUDw1Z0ZqdmdRZ3dUTE0/view?usp=sharing
 */
double pxFromDist_X(double dmin, double dmax) {
    double min_hyp = sqrt(dmin * dmin + cam_mount_height * cam_mount_height);
    double max_hyp = sqrt(dmax * dmax + cam_mount_height * cam_mount_height);
    double theta1 = atan((min_hyp / max_hyp) * tan(camera_fov_horizontal / 2));
    return imageSize.width * (theta1 / camera_fov_horizontal);
}

// calculate the y coord of the input image from the specified distance
// see https://www.desmos.com/calculator/pwjwlnnx77
double pxFromDist_Y(double dist) {
    double tmp = atan(cam_mount_height / dist) - cam_mount_angle + camera_fov_vertical / 2;
    return imageSize.height * tmp / (camera_fov_vertical);
}

void setTransformFromGeometry() {
    // set width and height of the rectangle in front of the robot
    // the actual output image will show more than this rectangle
    float close_corner_dist = sqrt(pow(camera_dist_min, 2) + pow(cam_mount_height, 2));
    float rectangle_w = close_corner_dist * tan(camera_fov_horizontal / 2) * px_per_meter * 2;
    float rectangle_h = (camera_dist_max - camera_dist_min) * px_per_meter;

    // find coordinates for corners above rectangle in input image
    float x_top_spread = pxFromDist_X(camera_dist_min, camera_dist_max);
    float y_bottom = pxFromDist_Y(camera_dist_min);
    float y_top = pxFromDist_Y(camera_dist_max);

    // set the ouput image size to include the whole transformed image,
    // not just the target rectangle
    mapSize.width = static_cast<int>(rectangle_w * (imageSize.width / x_top_spread) / 2.0);
    mapSize.height = static_cast<int>(rectangle_h);

    Point2f src[4] = {
        Point2f(imageSize.width / 2.f - x_top_spread, y_top),  // top left
        Point2f(imageSize.width / 2.f + x_top_spread, y_top),  // top right
        Point2f(0, y_bottom),                                  // bottom left
        Point2f(imageSize.width, y_bottom)                     // bottom right
    };

    Point2f dst[4] = {
        Point2f(mapSize.width / 2.f - rectangle_w / 2.f, 0),               // top left
        Point2f(mapSize.width / 2.f + rectangle_w / 2.f, 0),               // top right
        Point2f(mapSize.width / 2.f - rectangle_w / 2.f, mapSize.height),  // bottom left
        Point2f(mapSize.width / 2.f + rectangle_w / 2.f, mapSize.height)   // bottom right
    };

    transform_matrix = getPerspectiveTransform(src, dst);
}

void TransformImage(const sensor_msgs::ImageConstPtr& msg, string& topic) {
    // if no one is listening or the transform is undefined, give up
    if (transform_pubs[topic].getNumSubscribers() == 0 || transform_matrix.empty()) {
        return;
    }

    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
    const Mat& inimage = cv_ptr->image;

    Mat warp_img;
    warpPerspective(inimage, warp_img, transform_matrix, mapSize);

    double map_length = camera_dist_max + cam_mount_x;
    Mat outimage(static_cast<int>(map_length * px_per_meter), warp_img.cols, CV_8UC1, Scalar(0));
    Rect out_warp_roi(0, 0, warp_img.cols, warp_img.rows);

    warp_img.copyTo(outimage(out_warp_roi));

    sensor_msgs::Image outmsg;
    cv_ptr->image = outimage;
    cv_ptr->toImageMsg(outmsg);
    transform_pubs[topic].publish(outmsg);
}

int main(int argc, char** argv) {
    init(argc, argv, "image_transform");
    NodeHandle nh;
    NodeHandle pnh("~");

    bool all_defined = true;
    all_defined &= pnh.getParam("px_per_meter", px_per_meter);
    all_defined &= pnh.getParam("map_dist_max", camera_dist_max);
    all_defined &= pnh.getParam("map_dist_min", camera_dist_min);

    std::string camera_info_topic;
    all_defined &= pnh.getParam("camera_info_topic", camera_info_topic);
    std::string camera_link_name;
    all_defined &= pnh.getParam("camera_link_name", camera_link_name);

    // the launch file can provide camera information in case camera_info is not published
    all_defined &= pnh.getParam("fallback_fov_horizontal", camera_fov_horizontal);
    all_defined &= pnh.getParam("fallback_fov_vertical", camera_fov_vertical);
    all_defined &= pnh.getParam("fallback_image_width", imageSize.width);
    all_defined &= pnh.getParam("fallback_image_height", imageSize.height);

    if (!all_defined) {
        ROS_WARN("[Image Transform] Not all launch params defined");
    }

    // load camera geometry
    rr::CameraGeometry cam_geom;
    cam_geom.LoadInfo(nh, camera_info_topic, camera_link_name, 60.0);

    // set relevant camera geometry fields for this node
    camera_fov_horizontal = cam_geom.GetFOVHorizontal();
    camera_fov_vertical = cam_geom.GetFOVVertical();
    cam_mount_angle = std::get<1>(cam_geom.GetCameraOrientationRPY());
    cam_mount_height = cam_geom.GetCameraLocation().z;
    cam_mount_x = cam_geom.GetCameraLocation().x;

    setTransformFromGeometry();
    ROS_INFO("Calculated perspective transform. Used height %f and angle %f", cam_mount_height, cam_mount_angle);

    string topicsConcat;
    pnh.getParam("transform_topics", topicsConcat);
    vector<string> topics;
    boost::split(topics, topicsConcat, boost::is_any_of(" ,"));
    vector<Subscriber> transform_subs;
    ROS_INFO_STREAM("Found " << topics.size() << " topics in param.");
    for (const string& topic : topics) {
        if (topic.size() == 0) {
            continue;
        }

        transform_subs.push_back(nh.subscribe<sensor_msgs::Image>(topic, 1, boost::bind(TransformImage, _1, topic)));
        ROS_INFO_STREAM("Image_transform subscribed to " << topic);
        string newTopic(topic + "_transformed");
        ROS_INFO_STREAM("Creating new topic " << newTopic);
        transform_pubs[topic] = nh.advertise<sensor_msgs::Image>(newTopic, 1);
    }

    spin();

    return 0;
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <iomanip>
#include <Eigen/Dense>
#include <cmath>
#include <vector>

using namespace std;

typedef float (* node_function_t)(float a) ;

typedef float (* loss_function_t)(float y, float y_hat);

class Functions
{
private:
    static float loss_function(float y, float y_hat)
    {
        return (y - y_hat)*(y - y_hat);
    }
    static float der_loss_function(float y, float y_hat)
    {
        return 2*(y-y_hat);
    }
    static float node_function(float x)
    {
        return tanh(0.01*x);
    }
    static float der_node_function(float x)
    {
        float der = 1/(cosh(0.01*x)*cosh(0.01*x));

        return der;
    }

public:
    node_function_t f = node_function;
    node_function_t df = der_node_function;
    loss_function_t l = loss_function;
    loss_function_t dl = der_loss_function;
};

class Layer {
public:
    Eigen::VectorXf result;
    Eigen::MatrixXf w;
    Eigen::VectorXf dres;
    Layer (int layer_nodes_count, int prev_nodes_count) {
        w.resize(layer_nodes_count, prev_nodes_count);
        w.setOnes();
        // w = w*0.01;
        // w = w*0.01;

    }

    float operator[](int index){
        return this->result[index];
    }

    Eigen::VectorXf computer_out(const Eigen::VectorXf& x, node_function_t f, node_function_t df) {
        Eigen::VectorXf res = w * x;
        // if(res.size()==784){
        //   std::cout << w*x << '\n';
        // }
        dres.resize(res.size());
        for (int i = 0; i < w.rows(); i++) {
            dres[i] = df(res[i]);
            res[i] = f(res[i]);
        }
        result = res;
        return res;
    }

};

class Network
{
protected:
    Layer input;
    Layer hl ;
    Layer output;
    float learning_rate = 0.3;
    // float learning_rate = 0.002;
    float deviation = 0.02;
    // float deviation = 0.02;
    Functions functions;

public:
    Network(int input_count, int hidden_layer, int output_count, Functions functools)
    : input(input_count, input_count), hl(hidden_layer, input_count),
    output(output_count, hidden_layer){
        functions = functools;
    }

    Eigen::VectorXf learn_sample(const Eigen::VectorXf& x, const Eigen::VectorXf& out)
    {
        gradient_descent(x, out);
        return compute_result(x);
    }

    Eigen::VectorXf compute_input(const Eigen::VectorXf& x)
    {
        return input.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_hidden(const Eigen::VectorXf& x)
    {
        return hl.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_output(const Eigen::VectorXf& x)
    {
        return output.computer_out(x, functions.f, functions.df);
    }

    Eigen::VectorXf compute_result(const Eigen::VectorXf& x)
    {
      // std::cout << x << '\n';
        return compute_output(compute_hidden(compute_input(x)));
    }

    Eigen::VectorXf compute_result(const Eigen::VectorXf& x, const Eigen::VectorXf& out)
    {
      // std::cout << x << '\n';
      learn_sample(x,out);
        return compute_output(compute_hidden(compute_input(x)));
    }

    void set_learning_rate(float rate)
    {
        learning_rate = rate;
    }

    void set_deviation(float max_deviation)
    {
        deviation = max_deviation;
    }

    void gradient_descent(const Eigen::VectorXf& x, const Eigen::VectorXf& out, int max_count = 4)
    {
        int count = 0;
        float D, D1;
        Eigen::VectorXf D_v, dDdz;
        Eigen::MatrixXf dw, du, dv;
        // for (size_t i = 0; i < out.size(); i++) {
        //   if(out[i]!=0)
        //   {
        //     std::cout << " Out: "<< i << '\n';
        //     std::cout << compute_result(x)[i] << '\n';
        //   }
        // }
        do {
            D_v = compute_loss_function(out);
            dDdz = compute_der_d_z(out);
            D = D_v.sum();
            // std::cout << grad_output(dDdz) << '\n';
            descent(grad_output(dDdz), grad_hidden_2(dDdz), grad_input_2(dDdz, x));
            compute_result(x);
            D1 = compute_loss_function(out).sum();
            // std::cout << "D =" << D <<' '<<D1 << "Difference: " <<D-D1<<'\n';
            count++;
        }
        while (
          // D>1
        // );
          D - D1 > deviation &&
           count<max_count);
    }

    void descent(const Eigen::MatrixXf& dw, const Eigen::MatrixXf& du, const Eigen::MatrixXf& dv )
    {
        input.w -= dv;
        hl.w -= du;
        output.w -= dw;
    }

    Eigen::VectorXf compute_loss_function(const Eigen::VectorXf& out)
    {
        Eigen::VectorXf D(output.result.size());
        for (int i = 0; i < output.result.size(); i++)
        {
            D[i] = functions.l(output[i], out[i]);
        }
        return D;
    }

    Eigen::VectorXf compute_der_d_z(const Eigen::VectorXf& out)
    {
        Eigen::VectorXf res(output.result.size());

        for (int i = 0; i < output.result.size(); ++i) {
            res[i] = functions.dl(output.result[i], out[i]);
        }
        return res;
    }

    Eigen::MatrixXf grad_output(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf dw(output.w.rows(), output.w.cols());

        for (int i = 0; i < output.w.rows(); i++)
        {
            for(int j = 0; j < output.w.cols(); j++)
            {
                dw(i,j) = dDdZ[i]*hl.result[j]*output.dres[i];
            }
        }

        return dw*learning_rate;
    }

    Eigen::MatrixXf grad_hidden(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf du(hl.w.rows(), hl.w.cols());
        du.setZero();
        for (int i = 0; i < hl.w.rows(); i++)
        {
            for(int j = 0; j < hl.w.cols(); j++)
            {
                for (int k = 0; k < output.w.rows(); k++)
                {
                  // std::cout << hl.w.rows()<<' '<<j<<' '<<k << '\n';
                    du(i,j) += dDdZ[k]*output.w(k,i)*output.dres[k];
                }
                du(i,j) *= input.result[j]*hl.dres[i];
            }
        }

        return du*learning_rate;
    }

    Eigen::MatrixXf grad_hidden_2(const Eigen::VectorXf& dDdZ)
    {
        Eigen::MatrixXf du(hl.w.rows(), hl.w.cols());
        du.setZero();
        Eigen::MatrixXf temp = output.dres.asDiagonal()*output.w;
        temp = dDdZ.asDiagonal()*temp;
        float temp_sum;
        for (int i = 0; i < hl.w.rows(); i++)
        {
            temp_sum = temp.col(i).sum();
            for(int j = 0; j < hl.w.cols(); j++)
            {
                du(i,j) =temp_sum* input.result[j]*hl.dres[i];
            }
        }

        return du*learning_rate;
    }

    Eigen::MatrixXf grad_input(const Eigen::VectorXf& dDdZ, const Eigen::VectorXf& in)
    {
        int iwc = input.w.cols();
        Eigen::MatrixXf dv(input.w.rows(), iwc);
        dv.setZero();
        int ors = output.result.size();
        for(int i = 0; i < input.w.rows(); i++)
        {
            for(int j = 0; j < iwc; j++)
            {
                for(int k= 0; k < ors; k++)
                {
                    for(int t = 0; t < hl.w.rows(); t++)
                    {
                        dv(i,j) +=hl.w(t,i)*output.w(k,t)*hl.dres[t];
                    }
                    dv(i,j)*=dDdZ[k]*output.dres[k];
                     // std::cout << dv(i,j) << '\n';

                }
                dv(i,j)*=input.dres[i]*in[j];
            }

        }

        return dv*learning_rate;
    }

        Eigen::MatrixXf grad_input_2(const Eigen::VectorXf& dDdZ, const Eigen::VectorXf& in)
        {
            int iwc = input.w.cols();
            Eigen::MatrixXf dv(input.w.rows(), iwc);
            Eigen::MatrixXf temp = hl.dres.asDiagonal()*hl.w ;
            temp =output.w*temp;
            //std::cout << temp << '\n';
            dv.setZero();
            float temp_sum;
            int ors = output.result.size();
            for(int i = 0; i < input.w.rows(); i++)
            {
              temp_sum = temp.col(i).sum();
                for(int j = 0; j < iwc; j++)
                {
                    for(int k= 0; k < ors; k++)
                    {
                        dv(i,j) =dDdZ[k]*output.dres[k]*temp_sum ;
                    }
                    dv(i,j)*=input.dres[i]*in[j];
                }
                //std::cout << i << dv(i,0)<< '\n';
            }

            return dv*learning_rate;
        }

    void debug_output()
    {
      //std::cout << input.w << '\n';
    //  std::cout << hl.w << '\n';
      // std::cout << output.w << '\n';
    }

    void write_in_file()
    {
      ofstream off("weights.in");
      for (size_t i = 0; i < output.w.rows(); i++) {
        for (size_t j = 0; j < output.w.cols(); j++) {
          off<< output.w(i,j)<<endl;
        }
      }
      for (size_t i = 0; i < hl.w.rows(); i++) {
        for (size_t j = 0; j < hl.w.cols(); j++) {
          off<< hl.w(i,j)<<endl;
        }
      }
      for (size_t i = 0; i < input.w.rows(); i++) {
        for (size_t j = 0; j < input.w.cols(); j++) {
          off<< input.w(i,j)<<endl;
        }
      }
      off.close();
    }

    void read_from_file() {
      ifstream off("weights.in");
      if (off.is_open())
      {
      for (size_t i = 0; i < output.w.rows(); i++) {
        for (size_t j = 0; j < output.w.cols(); j++) {
          off>> output.w(i,j);
        }
      }
      for (size_t i = 0; i < hl.w.rows(); i++) {
        for (size_t j = 0; j < hl.w.cols(); j++) {
          off>> hl.w(i,j);
        }
      }
      for (size_t i = 0; i < input.w.rows(); i++) {
        for (size_t j = 0; j < input.w.cols(); j++) {
          off>> input.w(i,j);
        }
      }
    }
      off.close();
    }

};

  Eigen::MatrixXf read_mnist(int samples)
  {
    Eigen::MatrixXf x(28*28, samples);

      ifstream file ("t10k-images-idx3-ubyte");
      int magic_number;
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      if (file.is_open())
      {
          for(int i=0;i<samples;i++)
          {
              for(int r=0;r<784;r++)
              {
                unsigned char temp=0;
                file.read((char*)&temp,sizeof(temp));
                x(r, i)=(float)temp/255;
                 // std::cout <<r<<" "<< (float)temp/255 << ' '<<x(r, i)<< '\n';
              }
          }
      }
      return x;
  }

  Eigen::VectorXf read_mnist_labels(int samples)
  {
      Eigen::VectorXf out(samples);
      ifstream file ("t10k-labels-idx1-ubyte");
      int magic_number;
      file.read((char*)&magic_number,sizeof(int));
      file.read((char*)&magic_number,sizeof(int));
      if (file.is_open())
      {
          for(int i=0;i<samples;i++)
          {
            unsigned char temp=0;
            file.read((char*)&temp,sizeof(temp));
            out[i] = (int)temp;

          }
      }
      return out;
  }

  struct Result
  {
    int predict;
    float pred_pers;
    float error;
    std::vector<int> image;
  };

struct Result test(int sample)
{
  Eigen::MatrixXf temp = read_mnist(sample+1);
  Eigen::VectorXf labels = read_mnist_labels(sample+1);
  Network nt(784, 256, 10, Functions());
  nt.read_from_file();
  Result result;
  Eigen::VectorXf res;
  float error = 0;
    Eigen::VectorXf out_v(10);
    out_v.setZero();
    out_v[labels[sample]] = 1;

    Eigen::VectorXf in = temp.col(sample);

    for (size_t i = 0; i < 784; i++) {
      result.image.push_back(in[i]);
    }
    res = nt.compute_result(in, out_v);

    for (size_t j = 0; j < res.size(); j++) {
      if(res[j]>0.9)
      {
        result.predict = j;
        result.pred_pers = res[j];
      }
      else
      {
        error +=res[j]
      }
    }
    result.error = error;

  return result;
}

void train(int sample)
{
  Eigen::MatrixXf temp = read_mnist(sample);
  Eigen::VectorXf labels = read_mnist_labels(sample);
  Network nt(784, 256, 10, Functions());

   for (size_t i = 0; i < sample; i++)
   {
    Eigen::VectorXf out_v(10);
    out_v.setZero();
    out_v[labels[i]] = 1;
    Eigen::VectorXf in = temp.col(i);
    nt.learn_sample(in, out_v);
    // std::cout << i+1 << '\n';
   }
   nt.write_in_file();
}


#include "triangle.hpp"

#include "geometry/projection.hpp"

#include <Eigen/Geometry>

namespace neon
{
triangle3::triangle3(triangle_quadrature::point const p)
    : surface_interpolation(std::make_unique<triangle_quadrature>(p), 3)
{
    this->precompute_shape_functions();
}

void triangle3::precompute_shape_functions()
{
    // Initialize nodal coordinates array as r and s
    m_quadrature->evaluate([&](auto const& coordinates) {
        auto const& [l, r, s] = coordinates;

        vector N(3);
        matrix rhea(3, 2);

        N(0) = r;
        N(1) = s;
        N(2) = 1.0 - r - s;

        rhea(0, 0) = 1.0;
        rhea(1, 0) = 0.0;
        rhea(2, 0) = -1.0;

        rhea(0, 1) = 0.0;
        rhea(1, 1) = 1.0;
        rhea(2, 1) = -1.0;

        return std::make_tuple(N, rhea);
    });

    extrapolation = matrix::Ones(number_of_nodes(), 1);
}

double triangle3::compute_measure(matrix const& nodal_coordinates) const
{
    // Use the cross product identity 2A = | a x b | to compute face area
    vector3 const direction0 = nodal_coordinates.col(0) - nodal_coordinates.col(2);
    vector3 const direction1 = nodal_coordinates.col(1) - nodal_coordinates.col(2);

    vector3 const normal = direction0.cross(direction1);

    return normal.norm() / 2.0;
}

triangle6::triangle6(triangle_quadrature::point const p)
    : surface_interpolation(std::make_unique<surface_quadrature>(triangle_quadrature(p)), 6)
{
    this->precompute_shape_functions();
}

void triangle6::precompute_shape_functions()
{
    using NodalCoordinate = std::tuple<int, double, double>;

    // Initialize nodal coordinates array as Xi, Eta, Zeta
    std::array<NodalCoordinate, 6> constexpr local_coordinates{{
        {0, 1.0, 0.0},
        {1, 0.0, 1.0},
        {2, 0.0, 0.0},
        {3, 0.5, 0.5},
        {4, 0.0, 0.5},
        {5, 0.5, 0.0},
    }};

    matrix N_matrix(m_quadrature->points(), number_of_nodes());
    matrix local_quadrature_coordinates = matrix::Ones(m_quadrature->points(), 3);

    m_quadrature->evaluate([&](auto const& coordinate) {
        auto const& [l, r, s] = coordinate;

        auto const t = 1.0 - r - s;

        vector N(6);
        matrix rhea(6, 2);

        N(0) = r * (2.0 * r - 1.0);
        N(1) = s * (2.0 * s - 1.0);
        N(2) = t * (2.0 * t - 1.0);
        N(3) = 4.0 * r * s;
        N(4) = 4.0 * s * t;
        N(5) = 4.0 * r * t;

        // r coordinates
        rhea(0, 0) = 4.0 * r - 1.0;
        rhea(1, 0) = 0.0;
        rhea(2, 0) = -4.0 * t + 1.0;
        rhea(3, 0) = 4.0 * s;
        rhea(4, 0) = -4.0 * s;
        rhea(5, 0) = 4.0 * t - 4.0 * r;

        // s coordinates
        rhea(0, 1) = 0.0;
        rhea(1, 1) = 4.0 * s - 1.0;
        rhea(2, 1) = -4.0 * t + 1.0;
        rhea(3, 1) = 4.0 * r;
        rhea(4, 1) = 4.0 * t - 4.0 * s;
        rhea(5, 1) = -4.0 * r;

        local_quadrature_coordinates(l, 0) = r;
        local_quadrature_coordinates(l, 1) = s;

        N_matrix.row(l) = N;

        return std::make_tuple(N, rhea);
    });

    // Compute extrapolation algorithm matkrices
    matrix local_nodal_coordinates = matrix::Ones(number_of_nodes(), 3);

    for (auto const& [a, r, s] : local_coordinates)
    {
        local_nodal_coordinates(a, 0) = r;
        local_nodal_coordinates(a, 1) = s;
    }
    compute_extrapolation_matrix(N_matrix, local_nodal_coordinates, local_quadrature_coordinates);
}

double triangle6::compute_measure(matrix const& nodal_coordinates)
{
    return m_quadrature->integrate(0.0, [&](auto const& femval, auto) {
        auto const& [N, dN] = femval;

        matrix2 const Jacobian = geometry::project_to_plane(nodal_coordinates) * dN;

        return Jacobian.determinant();
    });
}
}


#include <vector>

#include <glm/glm.hpp>

#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    const Vec3 v[4] = {Vec3(0), Vec3(1, 0, 0), Vec3(0, 1, 0), Vec3(0, 0, 1)};
    const Mat3 m[4][3] = {
        { Mat3(v[1], v[0], v[0]), Mat3(v[2], v[0], v[0]), Mat3(v[3], v[0], v[0])},
        { Mat3(v[0], v[1], v[0]), Mat3(v[0], v[2], v[0]), Mat3(v[0], v[3], v[0])},
        { Mat3(v[0], v[0], v[1]), Mat3(v[0], v[0], v[2]), Mat3(v[0], v[0], v[3])},
        { -Mat3(v[1], v[1], v[1]), -Mat3(v[2], v[2], v[2]), -Mat3(v[3], v[3], v[3])}
    };

}

namespace BalloonFEM
{
    void Engine::computeElasticForces(ObjState &state, Vvec3 &f_sum)
    {
        Vvec3 &pos = state.world_space_pos;

        for(TIter t = m_tetra->tetrahedrons.begin();
                t != m_tetra->tetrahedrons.end(); t++)
        {
            iVec4 &id = t->v_id;
            Vec3 &v0 = pos[id[0]];
            Vec3 &v1 = pos[id[1]];
            Vec3 &v2 = pos[id[2]];
            Vec3 &v3 = pos[id[3]];

            /* calculate deformation in world space */
            Mat3 Ds = Mat3(v0 - v3, v1 - v3, v2 - v3);

            /* calculate deformation gradient */
            Mat3 F = Ds * t->Bm;

            /* calculate Piola for this tetra */
            Mat3 P = m_volume_model->Piola(F);

            /* calculate forces contributed from this tetra */
            Mat3 H = - t->W * P * transpose(t->Bm);

            f_sum[id[0]] += H[0];
            f_sum[id[1]] += H[1];
            f_sum[id[2]] += H[2];
            f_sum[id[3]] -= H[0] + H[1] + H[2];
        }
    }

    SpMat Engine::computeElasticDiffMat(ObjState &state)
    {
        printf("building elastic differential matrix \n");
        /* project from constrained freedom state to world space */
		Vvec3 &pos = state.world_space_pos;

        /* compute elastic force Differentials */
		std::vector<T> coefficients;
		coefficients.clear();
		coefficients.reserve( 12 * 12 * m_tetra->num_vertex);

		for(TIter t = m_tetra->tetrahedrons.begin();
				t != m_tetra->tetrahedrons.end(); t++)
		{
			/* assgin world space position */
			iVec4 &id = t->v_id;
			Vec3 &v0 = pos[id[0]];
			Vec3 &v1 = pos[id[1]];
			Vec3 &v2 = pos[id[2]];
			Vec3 &v3 = pos[id[3]];

			/* calculate deformation in world space */
			Mat3 Ds = Mat3(v0 - v3, v1 - v3, v2 - v3);

			/* calculate deformation gradient */
			Mat3 F = Ds * t->Bm;

			/* i is index of vertex, j is index of dimention */
			for (size_t i = 0; i < 4; i++)
				for(size_t j = 0; j < 3; j++)
			{
				/* calculate delta deformation in world space */
				Mat3 dDs = m[i][j];

				/* calculate delta deformation gradient */
				Mat3 dF = dDs * t->Bm;

				/* calculate delta Piola */
				Mat3 dP = m_volume_model->StressDiff(F, dF);

				/* calculate forces contributed from this tetra */
				Mat3 dH = - t->W * dP * transpose(t->Bm);

				for(size_t w = 0; w < 3; w++)
					for(size_t l = 0; l < 3; l++)
						coefficients.push_back( T(3*id[w] + l, 3*id[i] + j,  dH[w][l]));

				Vec3 df_4 = - dH[0] - dH[1] - dH[2];
				for(size_t l = 0; l < 3; l++)
					coefficients.push_back( T(3*id[3] + l, 3*id[i] + j,  df_4[l]));
			}
		}
		SpMat E( 3 * pos.size(), 3 * pos.size());
		E.setFromTriplets(coefficients.begin(), coefficients.end());

        return E;
    }
}


// Example 2: Matrices and vectors
// source:
// http://eigen.tuxfamily.org/dox/GettingStarted.html#title0
// Compile: g++ -I /usr/include/eigen3/ eigen_ex2.cpp -o eigen_ex2
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main(void)
{
    MatrixXd m = MatrixXd::Random(3,3);
    m = (m + MatrixXd::Constant(3, 3, 1.2)) * 50;
    cout << "m = " << '\n' << m << '\n';
    VectorXd v(3);
    v << 1, 2, 3;
    cout << "m * v = " << '\n' << m * v << '\n';
    return 0;
}


/*
 * This file is part of the Visual Computing Library (VCL) release under the
 * MIT license.
 *
 * Copyright (c) 2014 Basil Fierz
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

// VCL configuration
#include <vcl/config/global.h>

// C++ standard library
#include <iostream>
#include <random>

// Eigen library
#include <Eigen/Dense>

// Google benchmark
#include "benchmark/benchmark.h"

// VCL
#include <vcl/core/simd/vectorscalar.h>
#include <vcl/core/interleavedarray.h>
#include <vcl/math/jacobieigen33_selfadjoint.h>
#include <vcl/math/jacobieigen33_selfadjoint_quat.h>
#include <vcl/util/precisetimer.h>

#include "problems.h"

// Global data store for one time problem setup
const size_t nr_problems = 1024 * 1024;

// Problem set
Vcl::Core::InterleavedArray<float, 3, 3, -1> F(nr_problems);

void perfEigenIterative(benchmark::State& state)
{
	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	for (auto _ : state)
	{
		for (int i = 0; i < state.range(0); ++i)
		{
			Vcl::Matrix3f A = F.at<float>(i);

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
			solver.compute(A, Eigen::ComputeEigenvectors);

			resU.at<float>(i) = solver.eigenvectors();
			resS.at<float>(i) = solver.eigenvalues();
		}
	}

	state.counters["Iterations"] = 0;
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

void perfEigenDirect(benchmark::State& state)
{
	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	for (auto _ : state)
	{
		for (int i = 0; i < state.range(0); ++i)
		{
			Vcl::Matrix3f A = F.at<float>(i);

			Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver;
			solver.computeDirect(A, Eigen::ComputeEigenvectors);

			resU.at<float>(i) = solver.eigenvectors();
			resS.at<float>(i) = solver.eigenvalues();
		}
	}

	state.counters["Iterations"] = 0;
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

template<typename WideScalar>
void perfJacobi(benchmark::State& state)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	int avg_nr_iter = 0;
	for (auto _ : state)
	{
		avg_nr_iter = 0;
		for (int i = 0; i < state.range(0) / width; ++i)
		{
			matrix3_t A = F.at<real_t>(i);
			matrix3_t U = matrix3_t::Identity();

			avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigen(A, U);

			resU.at<real_t>(i) = U;
			resS.at<real_t>(i) = A.diagonal();
		}
	}

	state.counters["Iterations"] = (double)(avg_nr_iter * width) / (double)state.range(0);
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

template<typename WideScalar>
void perfJacobiQuat(benchmark::State& state)
{
	using real_t = WideScalar;
	using matrix3_t = Eigen::Matrix<real_t, 3, 3>;

	size_t width = sizeof(real_t) / sizeof(float);

	Vcl::Core::InterleavedArray<float, 3, 3, -1> resU(state.range(0));
	Vcl::Core::InterleavedArray<float, 3, 1, -1> resS(state.range(0));

	int avg_nr_iter = 0;
	for (auto _ : state)
	{
		avg_nr_iter = 0;
		for (int i = 0; i < state.range(0) / width; ++i)
		{
			matrix3_t A = F.at<real_t>(i);
			matrix3_t U = matrix3_t::Identity();

			avg_nr_iter += Vcl::Mathematics::SelfAdjointJacobiEigenQuat(A, U);

			resU.at<real_t>(i) = U;
			resS.at<real_t>(i) = A.diagonal();
		}
	}

	state.counters["Iterations"] = (double)(avg_nr_iter * width) / (double)state.range(0);
	benchmark::DoNotOptimize(resU);
	benchmark::DoNotOptimize(resS);

	state.SetItemsProcessed(state.iterations() * state.range(0));
}

using Vcl::float16;
using Vcl::float4;
using Vcl::float8;

BENCHMARK(perfEigenIterative)->Arg(128); // ->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK(perfEigenDirect)->Arg(128);    // ->Arg(512)->Arg(8192)->ThreadRange(1, 16);

BENCHMARK_TEMPLATE(perfJacobi, float)->Arg(128);   //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float4)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float8)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobi, float16)->Arg(128); //->Arg(512)->Arg(8192)->ThreadRange(1, 16);

BENCHMARK_TEMPLATE(perfJacobiQuat, float)->Arg(128);   //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float4)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float8)->Arg(128);  //->Arg(512)->Arg(8192)->ThreadRange(1, 16);
BENCHMARK_TEMPLATE(perfJacobiQuat, float16)->Arg(128); //->Arg(512)->Arg(8192)->ThreadRange(1, 16);

int main(int argc, char** argv)
{
	// Initialize data
	createSymmetricProblems(nr_problems, F);

	::benchmark::Initialize(&argc, argv);
	::benchmark::RunSpecifiedBenchmarks();
}


#include <iostream>
#include <string>
#include <numeric>
#include <algorithm>
#include <vector>
#include <array>
#include <boost/algorithm/string.hpp>

std::vector<std::vector<int>> generateAll(int limit, const std::vector<std::vector<int>>& input, bool find = false);

// This are the values provided in the quiz input. This solution is not very flexible, but
// get's the problem solved
std::array<std::array<int, 5>, 4> properties = {{{4, -2,  0, 0, 5},
						 {0,  5, -1, 0, 8},
						 {-1, 0,  5, 0, 6},
						 {0,  0, -2, 2, 1}}};

int main (int argc, char** argv) {
  std::string line;

  int limit = 100;
  int elements = 4;
  std::vector<std::vector<int>> v;
  for(int i = 0 ; i < elements - 1; ++i) {
    v = generateAll(limit, v);
  }
  // For the last generation iteration we just want to print out the max value
  generateAll(limit, v, true);
}

std::vector<std::vector<int>> generateAll(int limit, const std::vector<std::vector<int>>& input,
					  bool find) {
  std::vector<std::vector<int>> output;

  // The input vector is empty, fill it please
  if (input.size() == 0) {
    for(int i = 0 ; i <= limit ; ++i) {
      output.push_back({i});
    }
    return std::move(output);
  }

  int max = 0;
  for(auto& v : input) {
    for(int i = 0 ; i <= limit ; ++i) {
      auto vCopy = v;
      vCopy.push_back(i);

      // Do not add options that add up to more than the limit otherwise
      // memory explodes. In fact memory explodes with more than 3 different
      // elements. This is why we use "find" hack, to not store in memory
      // the vector with the 4 elements.
      if(std::accumulate(vCopy.begin(), vCopy.end(), 0) <= limit) {
	if(find) {
	  int total = 1;
	  for(int i = 0 ; i < 4 ; ++i) {
	    int propTotal = 0;
	    for(int j = 0 ; j < vCopy.size() ; ++ j) {
	      propTotal += (vCopy[j] * properties[j][i]);
	    }
	    total *= (propTotal > 0) ? propTotal : 0;
	  }
	  max = std::max(total, max);
	} else {
	  output.push_back(std::move(vCopy));
	}
      } else {
	break;
      }
    }
  }
  if(find)
    std::cout << "Max Total " << max << std::endl;
  return std::move(output);
}