#include <algorithm>
#include <cassert>
#include <limits>
#include <memory>
#include <ostream>
#include <random>
#include <stdexcept>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>

#include "bsgs.hpp"
#include "orbit.hpp"
#include "perm.hpp"
#include "perm_group.hpp"
#include "perm_set.hpp"
#include "util.hpp"

namespace mpsym
{

namespace internal
{

PermGroup::PermGroup(unsigned degree, PermSet const &generators)
{
  _bsgs = BSGS(degree, generators);
  _order = _bsgs.order();
}

bool PermGroup::operator==(PermGroup const &rhs) const
{
  assert(rhs.degree() == degree()
         && "comparing permutation groups of equal degree");

  if (_order != rhs.order())
    return false;

  for (Perm const &gen : rhs.generators()) {
    if (!contains_element(gen))
      return false;
  }

  return true;
}

bool PermGroup::operator!=(PermGroup const &rhs) const
{
  return !(*this == rhs);
}

PermGroup PermGroup::symmetric(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u);

  if (degree == 1u)
    return PermGroup(1u, {Perm(1u)});

  std::vector<unsigned> gen;
  for (unsigned i = 0u; i < degree; ++i)
    gen.push_back(i);

  return PermGroup(degree, {Perm(degree, {{0, 1}}), Perm(degree, {gen})});
}

PermGroup PermGroup::cyclic(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u);

  std::vector<unsigned> gen;
  for (unsigned i = 0u; i < degree; ++i)
    gen.push_back(i);

  return PermGroup(degree, {Perm(degree, {gen})});
}

PermGroup PermGroup::dihedral(unsigned degree)
{
  // TODO: explicit BSGS

  assert(degree > 0u && degree % 2 == 0);

  if (degree == 2u)
    return PermGroup(2, {Perm({1, 0})});

  if (degree == 4u)
    return PermGroup(4, {Perm({1, 0, 2, 3}), Perm({0, 1, 3, 2})});

  std::vector<unsigned> rotation(degree / 2u);

  // rotation
  for (unsigned i = 0u; i < degree / 2u - 1u; ++i)
    rotation[i] = i + 1u;

  rotation[degree / 2u - 1u] = 0u;

  // reflection
  std::vector<unsigned> reflection(degree / 2u);

  reflection[0] = 0u;

  for (unsigned i = 1u; i < (degree / 2u + 1u) / 2u; ++i) {
    reflection[i] = degree / 2u - i;
    reflection[degree / 2u - i] = i;
  }

  if ((degree / 2u) % 2 == 0)
    reflection[degree / 4u] = degree / 4u;

  return PermGroup(degree / 2u, {Perm(rotation), Perm(reflection)});
}

PermSet PermGroup::wreath_product_generators(PermGroup const &lhs,
                                             PermGroup const &rhs)
{
  unsigned wp_degree = lhs.degree() * rhs.degree();

  auto lhs_gens(lhs.generators());
  auto rhs_gens(rhs.generators());

  PermSet wp_generators;

  if (lhs.is_trivial() && rhs.is_trivial()) {
    return {};

  } else if (rhs.is_trivial()) {
    wp_generators.resize(lhs_gens.size(), Perm(wp_degree));

    for (unsigned i = 0u; i < rhs.degree(); ++i) {
      for (auto j = 0u; j < lhs_gens.size(); ++j)
        wp_generators[j] *= lhs_gens[j].shifted(lhs.degree() * i).extended(wp_degree);
    }

  } else {
    for (unsigned i = 0u; i < rhs.degree(); ++i) {
      for (Perm const &perm : lhs_gens)
        wp_generators.insert(perm.shifted(lhs.degree() * i).extended(wp_degree));
    }

    for (Perm const &gen : rhs_gens) {
      std::vector<std::vector<unsigned>> cycles {gen.cycles()};
      for (auto &cycle : cycles) {
        for (unsigned &x : cycle)
          x = x * lhs.degree();
      }

      std::vector<std::vector<unsigned>> shifted_cycles {cycles};

      for (unsigned i = 1u; i < lhs.degree(); ++i) {
        for (auto const &cycle : cycles) {
          std::vector<unsigned> shifted_cycle(cycle);

          for (unsigned &x : shifted_cycle)
            x += i;

          shifted_cycles.push_back(shifted_cycle);
        }
      }

      wp_generators.emplace(wp_degree, shifted_cycles);
    }
  }

  return wp_generators;
}

PermGroup PermGroup::wreath_product(PermGroup const &lhs,
                                    PermGroup const &rhs,
                                    BSGSOptions const *bsgs_options_,
                                    timeout::flag aborted)
{
  // degree of wreath product
  unsigned wp_degree = lhs.degree() * rhs.degree();

  // generators of wreath product
  auto wp_generators(wreath_product_generators(lhs, rhs));

  if (wp_generators.empty())
    return PermGroup(wp_degree);

  // order of wreath product
  auto wp_order(wreath_product_order(lhs, rhs));

  // construct wreath product
  auto bsgs_options(BSGSOptions::fill_defaults(bsgs_options_));
  bsgs_options.schreier_sims_random_known_order = wp_order;

  return PermGroup(BSGS(wp_degree, wp_generators, &bsgs_options, aborted));
}

BSGS::order_type PermGroup::wreath_product_order(PermGroup const &lhs,
                                                 PermGroup const &rhs)
{
  using boost::multiprecision::pow;

  auto lhs_order(lhs.order());
  auto rhs_order(rhs.order());

  if (lhs.is_trivial())
    return rhs_order;

  if (rhs.is_trivial())
    return lhs_order;

  return pow(lhs_order, rhs.degree()) * rhs_order;
}

bool PermGroup::is_symmetric() const
{
  if (_bsgs.is_symmetric() || degree() == 1u)
    return true;

  return _order == symmetric_order(degree());
}

bool PermGroup::is_shifted_symmetric() const
{
  unsigned degree_ = largest_moved_point() - smallest_moved_point() + 1u;

  return _order == symmetric_order(degree_);
}

bool PermGroup::is_transitive() const
{
  auto orbit(Orbit::generate(0u, generators().with_inverses()));

  return orbit.size() == degree();
}

bool PermGroup::contains_element(Perm const &perm) const
{
  assert(perm.degree() == degree() && "element has same degree as group");

  return _bsgs.strips_completely(perm);
}

Perm PermGroup::random_element() const
{
  static auto re(util::random_engine());

  Perm result(degree());
  for (unsigned i = 0u; i < _bsgs.base_size(); ++i) {
    auto orbit(_bsgs.orbit(i));

    std::uniform_int_distribution<> d(0u, orbit.size() - 1u);

    result *= _bsgs.transversal(i, *(orbit.begin() + d(re)));
  }

  return result;
}

PermGroup::const_iterator::const_iterator(PermGroup const &pg)
  : _trivial(pg.bsgs().base_empty()),
    _end(false)
{
  if (_trivial) {
    _current = Perm(pg.degree());

    _current_valid = true;

  } else {
    for (unsigned i = 0u; i < pg.bsgs().base_size(); ++i) {
      _state.push_back(0u);

      auto transv = pg.bsgs().transversals(i);

      _transversals.push_back(transv);
      _current_factors.insert(transv[0]);
    }

    _current_valid = false;
  }
}

bool PermGroup::const_iterator::operator==(PermGroup::const_iterator const &rhs) const
{
  if (_end != rhs._end)
    return false;

  if (_end && rhs._end)
    return true;

  for (unsigned i = 0u; i < _state.size(); ++i) {
    if (_state[i] != rhs._state[i])
      return false;
  }

  return true;
}

PermGroup::const_iterator::reference PermGroup::const_iterator::current()
{
  if (_current_valid)
    return _current;

  _current = _current_factors[0];
  for (unsigned j = 1u; j < _current_factors.size(); ++j)
    _current = _current_factors[j] * _current;

  _current_valid = true;

  return _current;
}

void PermGroup::const_iterator::next()
{
  if (_trivial) {
    _end = true;
    return;
  }

  for (unsigned i = 0u; i < _state.size(); ++i) {
    _state[i]++;
    if (_state[i] == _transversals[i].size())
      _state[i] = 0u;

    _current_factors[i] = _transversals[i][_state[i]];

    if (i == _state.size() - 1u && _state[i] == 0u) {
      _end = true;
      break;
    }

    if (_state[i] != 0u)
      break;
  }

  _current_valid = false;
}

std::ostream &operator<<(std::ostream &os, PermGroup const &pg)
{
  os << pg.bsgs() << "\n"
     << "ORDER: " << pg._order;

  return os;
}

} // namespace internal

} // namespace mpsym


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef KRAW_APPROX_HPP
#define KRAW_APPROX_HPP

// Krawczyk method using approximate solution
// Newton iteration can be applied in advance.

#include <limits>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/autodif.hpp>
#include <kv/matrix-inversion.hpp>
#include <kv/make-candidate.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
bool
krawczyk_approx(F f, const ub::vector<T>& c, ub::vector< interval<T> >& result, int newton_max = 2, int verbose = 1)
{
	int s = c.size();

	ub::vector< interval<T> > I, fc, fi, Rfc, C, K;
	ub::matrix< interval<T> > fdc, fdi, M;
	ub::vector<T> c2, minus;
	ub::matrix<T> R;
	int i, j;
	bool r;
	ub::vector<T> newton_step;
	T tmp, tmp2;

	c2 = c;

	// Newton iteration
	// use interval<T> for argument of f
	// preparing for the case that f can not accept T.

	for (i=0; i<newton_max; i++) {
		C = c2;
		try {
			autodif< interval<T> >::split(f(autodif< interval<T> >::init(C)), fc, fdc);
		}
		catch (std::domain_error& e) {
			return false;
		}
		r = invert(mid(fdc), R);
		if (!r) return false;

		minus = prod(R, mid(fc));

		tmp = 1.;
		tmp2 = 0.;
		for (j=0; j<s; j++) {
			using std::abs;
			tmp = std::max(tmp, abs(c2(j)));
			tmp2 = std::max(tmp2, abs(minus(j)));
		}

		c2 = c2 - minus;
		if (verbose >= 1) {
			std::cout << "newton" << i << ": " << c2 << "\n";
		}
		if (tmp2 <= tmp * std::numeric_limits<T>::epsilon()) break;
	}

	C = c2;
	try {
		autodif< interval<T> >::split(f(autodif< interval<T> >::init(C)), fc, fdc);
	}
	catch (std::domain_error& e) {
		return false;
	}
	r = invert(mid(fdc), R);
	if (!r) return false;
	Rfc = prod(R, fc);

	newton_step.resize(s);
	for (i=0; i<s; i++) {
		newton_step(i) = norm(Rfc(i));
	}

	make_candidate(newton_step);

	I = C;
	for (i=0; i<s; i++) {
		tmp = std::numeric_limits<T>::epsilon() * norm(I(i)) * (s+1) * 2;
		tmp2 = std::numeric_limits<T>::min() * (s+1) * 2;
		if (newton_step(i) < tmp) newton_step(i) = tmp;
		if (newton_step(i) < tmp2) newton_step(i) = tmp2;
		I(i) += newton_step(i) * interval<T>(-1., 1.);
	}

	if (verbose >= 1) {
		std::cout << "I: " << I << "\n";
	}

	try {
		autodif< interval<T> >::split(f(autodif< interval<T> >::init(I)), fi, fdi);
	}
	catch (std::domain_error& e) {
		return false;
	}

	// M = ub::identity_matrix< interval<T> >(s) - prod(R, fdi);
	M = ub::identity_matrix< interval<T> >(s);
	M -= prod(R, fdi);

	K = C - Rfc +  prod(M, I - C);

	if (verbose >= 1) {
		std::cout << "K: " << K << "\n";
	}

	if (proper_subset(K, I)) {
		result = K;
		return true;
	} else {
		return false;
	}
}

namespace krawczyk_approx_sub {

// generate 1-d vector function from scalar function
template <class F>
struct MakeVec {
	F f;
	MakeVec(F f): f(f) {}

	template <class T> ub::vector<T> operator()(const ub::vector<T>& x) {
		ub::vector<T> r(1);
		r(0) = f(x(0));
		return r;
	}
};

} // namespace krawczyk_approx_sub;

// 1 dimensional version

template <class T, class F>
bool
krawczyk_approx(F f, const T& c, interval<T>& result, int newton_max = 2, int verbose = 1)
{
	ub::vector<T> in(1);
	ub::vector< interval<T> > out;
	krawczyk_approx_sub::MakeVec<F> g(f);
	bool r;

	in(0) = c;
	r = krawczyk_approx(g, in, out, newton_max, verbose);
	if (!r) return r;
	result = out(0);
	return r;
}

} // namespace kv

#endif // KRAW_APPROX_HPP


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Polyhedron_3.h>

#include <CGAL/Surface_mesh_parameterization/IO/File_off.h>
#include <CGAL/Surface_mesh_parameterization/Square_border_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_conformal_map_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>

#include <CGAL/Polygon_mesh_processing/measure.h>
#include <CGAL/Unique_hash_map.h>

#include <boost/array.hpp>

#include <unordered_set>
#include <cstdlib>
#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>                           Kernel;
typedef Kernel::Point_2                                          Point_2;
typedef Kernel::Point_3                                          Point_3;
typedef CGAL::Polyhedron_3<Kernel>                               PolyMesh;

typedef boost::graph_traits<PolyMesh>::halfedge_descriptor       halfedge_descriptor;
typedef boost::graph_traits<PolyMesh>::vertex_descriptor         vertex_descriptor;
typedef boost::graph_traits<PolyMesh>::face_descriptor           face_descriptor;

typedef boost::graph_traits<PolyMesh>::vertex_iterator           vertex_iterator;

typedef boost::array<vertex_descriptor, 4>                       Vd_array;

typedef CGAL::Unique_hash_map<vertex_descriptor, Point_2>        UV_uhm;
typedef boost::associative_property_map<UV_uhm>                  UV_pmap;

namespace SMP = CGAL::Surface_mesh_parameterization;

bool read_vertices(const PolyMesh& mesh,
                   const char* filename,
                   Vd_array& fixed_vertices)
{
  std::string str = filename;
  if( (str.length()) < 14 || (str.substr(str.length() - 14) != ".selection.txt") ) {
    std::cerr << "Error: vertices must be given by a *.selection.txt file" << std::endl;
    return false;
  }

  std::ifstream in(filename);
  std::string line;
  if(!std::getline(in, line)) {
    std::cerr << "Error: could not read input file: " << filename << std::endl;
    return false;
  }

  // The selection file is a list of integers, so we must build a correspondence
  // between vertices and the integers.
  std::vector<vertex_descriptor> vds;
  vds.reserve(num_vertices(mesh));
  vertex_iterator vi = vertices(mesh).begin(), vi_end = vertices(mesh).end();
  CGAL_For_all(vi, vi_end) {
    vds.push_back(*vi);
  }

  // Get the first line and read the fixed vertex indices
  std::size_t counter = 0;
  std::istringstream point_line(line);
  std::size_t s;
  std::unordered_set<std::size_t> indices;
  while(point_line >> s) {
    if(s >= vds.size())
    {
      std::cerr << "Error: Vertex index too large" << std::endl;
      return false;
    }

    vertex_descriptor vd = vds[s];
    if(!is_border(vd, mesh)) { // must be on the border
      std::cerr << "Error: vertex is not on the border of the mesh" << std::endl;
      return false;
    }

    if(counter >= 4) { // too many border vertices
      std::cerr << "Error: Too many vertices are fixed" << std::endl;
      return false;
    }

    fixed_vertices[counter++] = vd;
    indices.insert(s);
  }

  if(indices.size() < 4) {
    std::cerr << "Error: at least four unique vertices must be provided" << std::endl;
    return false;
  }

  return true;
}

int main(int argc, char** argv)
{
  std::ifstream in((argc>1) ? argv[1] : CGAL::data_file_path("meshes/nefertiti.off"));
  if(!in){
    std::cerr << "Error: problem loading the input data" << std::endl;
    return 1;
  }

  PolyMesh sm;
  in >> sm;

  halfedge_descriptor bhd = CGAL::Polygon_mesh_processing::longest_border(sm).first;

  // The 2D points of the uv parametrisation will be written into this map
  UV_uhm uv_uhm;
  UV_pmap uv_map(uv_uhm);

  const char* filename = (argc > 2) ? argv[2] : "data/square_corners.selection.txt";
  Vd_array vda;
  if(!read_vertices(sm, filename, vda)) {
    std::cerr << "Error: problem loading the square corners" << std::endl;
    return EXIT_FAILURE;
  }

  typedef SMP::Square_border_uniform_parameterizer_3<PolyMesh> Border_parameterizer;
  typedef SMP::Discrete_conformal_map_parameterizer_3<PolyMesh, Border_parameterizer> Parameterizer;

  // Border parameterizers (pick one)
  Border_parameterizer border_param(vda[0], vda[1], vda[2], vda[3]);
//  Border_parameterizer border_param; // the border parameterizer will compute the corner vertices

  SMP::Error_code err = SMP::parameterize(sm, Parameterizer(border_param), bhd, uv_map);

  if(err != SMP::OK) {
    std::cerr << "Error: " << SMP::get_error_message(err) << std::endl;
    return 1;
  }

  std::ofstream out("result.off");
  SMP::IO::output_uvmap_to_off(sm, bhd, uv_map, out);

  return EXIT_SUCCESS;
}


#include <iostream>

#pragma GCC diagnostic ignored "-Wparentheses"
#pragma GCC optimize ("rtti")

#include <boost/units/systems/si.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/io.hpp>

const auto meter = 1.0 * boost::units::si::meter;
const auto s     = 1.0 * boost::units::si::second;

void print( decltype( meter / s ) v ){
    std::cout << "velocity = " << v << "\n";
}

int main(){

   auto gravity = 10.0 * meter / ( s * s );
   auto duration = 2.0 * s;

   print( gravity * duration );

}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef DESCRIPTORS__GIST_HELPER_HPP
#define DESCRIPTORS__GIST_HELPER_HPP

#include <complex>
#include <cstddef>
#include <cmath>
#include <algorithm>

#include <boost/static_assert.hpp>
#include <opencv2/core/core.hpp>

////////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
void fftshift_even(const cv::Mat_<T>& src, cv::Mat_<T>& dst)
{
    assert(src.isContinuous() && dst.isContinuous());

    //assert that size is even!!!

    dst.create(src.size());

    const int w = src.size().width;
    const int h = src.size().height;
    const int hw = w / 2;
    const int hh = h / 2;

    for (int y = 0; y < hh; y++)
    {
        // src[i] gives the ith row with the T datatype
        std::copy(src[y], src[y] + hw, dst[y + hh] + hw); // copy tl quadrant to br quadrant
        std::copy(src[y] + hw, src[y] + w, dst[y + hh]);      // copy tr quadrant to bl quadrant
    }

    for (int y = hh; y < h; y++)
    {
        std::copy(src[y], src[y] + hw, dst[y - hh] + hw); // copy bl quadrant to tr quadrant
        std::copy(src[y] + hw, src[y] + w, dst[y - hh]);      // copy br quadrant to tl quadrant
    }
}

template <class T>
void generate_gaussian_filter(cv::Mat_<T>& image, double sigma)
{
    const int w = image.size().width;
    const int h = image.size().height;
    const int wh = w / 2;
    const int hh = h / 2;

    const double s = 1.0 / (sigma*sigma);

    for (int y = -hh; y < hh; y++)
    {
        size_t yy = (y + h) % h;
        for (int x = -wh; x < wh; x++)
        {
            size_t xx = (x + w) % w;
            double fx = x;
            double fy = y;
            image(yy, xx) = std::exp(-(fx*fx + fy*fy) * s);
        }
    }
}

class torralba_prefilter
{
    typedef std::complex<float> complex_t;

    double      _sigma;
    cv::Size    _size;
    cv::Mat_<complex_t> _filter;

    public:

    torralba_prefilter(std::size_t width, std::size_t height, double cycles = 4.0)
     : _sigma(cycles / std::sqrt(std::log(2.0)))
     , _size(width, height)
     , _filter(_size)
    {
        generate_gaussian_filter(_filter, _sigma);
    }

    void operator() (cv::Mat& img)
    {
        assert(img.type() == CV_8UC1 && img.size().width == _size.width && img.size().height == _size.height);

        // "whitening"
        cv::Mat_<float> logimg;
        img.convertTo(logimg, CV_32FC1);
        cv::log(1.0 + logimg, logimg);

        cv::Mat_<complex_t> spbuf(_size);
        std::copy(logimg.begin(), logimg.end(), spbuf.begin());

        cv::Mat_<complex_t> frbuf;
        cv::dft(spbuf, frbuf);

        cv::mulSpectrums(frbuf, 1.0 - _filter, frbuf, 0);

        cv::Mat_<complex_t> white;
        cv::idft(frbuf, white, cv::DFT_SCALE);

        // "local contrast normalization"
        cv::MatIterator_<complex_t> dit = spbuf.begin();
        for (cv::MatConstIterator_<complex_t> it = white.begin(); it != white.end(); ++it, ++dit)
        {
            const complex_t& v = *it;
            *dit = v.real() * v.real();
        }

        cv::dft(spbuf, frbuf);
        cv::mulSpectrums(frbuf, _filter, frbuf, 0);
        cv::idft(frbuf, spbuf, cv::DFT_SCALE);

        cv::MatIterator_<unsigned char> dst = img.begin<unsigned char>();
        cv::MatConstIterator_<complex_t> wit = white.begin();
        for (cv::MatConstIterator_<complex_t> it = spbuf.begin(); it != spbuf.end(); ++it, ++wit, ++dst)
        {
            float d = std::sqrt(std::abs((*it).real())) + 0.2;
            float v = std::min(255 * std::max((*wit).real(), 0.0f) / d, 255.0f);
            *dst = v;
        }
    }
};

template <class T>
void generate_gabor_filter(cv::Mat_<T>& image, double peakFreq, double deltaFreq, double orientAngle, double deltaAngle)
{
    const double C = std::sqrt(log(2.0) / M_PI);

    const double Ka = (deltaFreq - 1.0) / (deltaFreq + 1.0);
    const double Kb = std::tan(0.5 * deltaAngle);
    //const double lambda = Ka / Kb;

    // scaling factors of the gaussian envelope
    const double a = peakFreq * (Ka / C);
    //const double b = a / lambda;
    const double b = Kb * peakFreq/C * std::sqrt(1.0 - Ka*Ka);

    // spatial frequency in cartesian coordinates
    const double u0 = peakFreq * std::cos(orientAngle);
    const double v0 = peakFreq * std::sin(orientAngle);

    // default: set orientation of gaussian envelope (theta) equal to orientation of filter
    const double theta = orientAngle;

    // generate filter
    const size_t w = image.size().width;
    const size_t h = image.size().height;
    const double stepx = 1.0 / static_cast<double>(w);
    const double stepy = 1.0 / static_cast<double>(h);
    const double cos_theta = std::cos(theta);
    const double sin_theta = std::sin(theta);
    double v = 0.5 - v0;

    for (size_t yy = 0; yy < h; yy++)
    {
        size_t y = (yy + (h / 2)) % h;
        double u = -0.5 - u0;
        for (size_t xx = 0; xx < w; xx++)
        {
            size_t x = (xx + (w / 2)) % w;

            double ur = u * cos_theta + v * sin_theta;
            double vr = -u * sin_theta + v * cos_theta;

            double U = ur / a;
            double V = vr / b;

            double value = std::exp(-M_PI * (U*U + V*V));

            image(y, x) = value;

            u += stepx;
        }

        v -= stepy;
    }
}

template <class T>
void generate_polargabor_filter(cv::Mat_<T>& image, double peakFreq, double deltaFreq, double orientAngle, double deltaAngle)
{
    // sigma_omega = 1 / (kappa * omega)
    double kappa = (deltaFreq - 1) / ((deltaFreq + 1) * std::sqrt(2*std::log(2.0)));

//    double sigma_theta = std::sqrt(2*PI)*4.0*numorients/32.0; // torralba
    double sigma_theta = std::sqrt(std::log(2.0)) * 2.0 / deltaAngle;

    // generate filter
    const size_t w = image.size().width;
    const size_t h = image.size().height;
    const double stepx = 1.0 / static_cast<double>(w);
    const double stepy = 1.0 / static_cast<double>(h);

    double v = -0.5;
    for (size_t yy = 0; yy < h; yy++)
    {
        size_t y = (yy + (h / 2)) % h;

        double u = -0.5;
        for (size_t xx = 0; xx < w; xx++)
        {
            size_t x = (xx + (w / 2)) % w;

            double omega = std::sqrt(u*u + v*v);
            double theta = std::atan2(v, u);

            double Omega = omega/peakFreq - 1;
            double Theta = theta + orientAngle;

            if (Theta < -M_PI) Theta += 2*M_PI;
            if (Theta >  M_PI) Theta -= 2*M_PI;

            double value = std::exp(-1/(2*kappa*kappa) * Omega*Omega - sigma_theta*sigma_theta * Theta*Theta);

            image(y, x) = value;

            u += stepx;
        }

        v += stepy;
    }
}

template <class T>
void symmetric_pad(const cv::Mat_<T>& src, cv::Mat_<T>& dst)
{
    cv::Mat_<T> tmp;

    if (src.cols < dst.cols)
    {
        int width = dst.cols;
        int height = std::min(src.rows, dst.rows);

        int pad = dst.cols - src.cols;
        int border = src.cols + pad/2;

        tmp.create(height, width);

        cv::Mat_<T> flipped;
        cv::flip(src, flipped, 1);

        for (int p = 0, k = 0; p < border; p += src.cols, k++)
        {
            int w = std::min(src.cols, border - p);
            int h = height;

            cv::Mat_<T> r = tmp(cv::Rect(p, 0, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, 0, w, h)).copyTo(r);
            }
            else
            {
                src(cv::Rect(0, 0, w, h)).copyTo(r);
            }
        }

        for (int p = width, k = 1; p >= border; p -= src.cols, k++)
        {
            int w = std::min(src.cols, p - border);
            int h = height;

            cv::Mat_<T> r = tmp(cv::Rect(p - w, 0, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(src.cols - w, 0, w, h)).copyTo(r);
            }
            else
            {
                src(cv::Rect(src.cols - w, 0, w, h)).copyTo(r);
            }
        }
    }
    else
    {
        tmp = src;
    }

    if (src.rows < dst.rows)
    {
        int width = dst.cols;
        int height = dst.rows;

        int pad = dst.rows - src.rows;
        int border = src.rows + pad/2;

        cv::Mat_<T> flipped;
        cv::flip(tmp, flipped, 0);

        for (int p = 0, k = 0; p < border; p += src.rows, k++)
        {
            int w = width;
            int h = std::min(src.rows, border - p);

            cv::Mat_<T> r = dst(cv::Rect(0, p, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, 0, w, h)).copyTo(r);
            }
            else
            {
                tmp(cv::Rect(0, 0, w, h)).copyTo(r);
            }
        }

        for (int p = height, k = 1; p >= border; p -= src.rows, k++)
        {
            int w = width;
            int h = std::min(src.rows, p - border);

            cv::Mat_<T> r = dst(cv::Rect(0, p - h, w, h));

            if (k % 2)
            {
                flipped(cv::Rect(0, src.rows - h, w, h)).copyTo(r);
            }
            else
            {
                tmp(cv::Rect(0, src.rows - h, w, h)).copyTo(r);
            }
        }
    }
    else
    {
        tmp.copyTo(dst);
    }
}

////////////////////////////////////////////////////////////////////////////////////////////////

#endif // DESCRIPTORS__GIST_HELPER_HPP


#include "incidencematrices.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <array>
#include <memory>

namespace IncidenceMatrices {

/** @brief Create the mesh consisting of a triangle and quadrilateral
 *         from the exercise sheet.
 * @return Shared pointer to the hybrid2d mesh.
 */
std::shared_ptr<lf::mesh::Mesh> createDemoMesh() {
  // builder for a hybrid mesh in a world of dimension 2
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // Add points
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 0});    // (0)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 0});    // (1)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{1, 1});    // (2)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0, 1});    // (3)
  mesh_factory_ptr->AddPoint(Eigen::Vector2d{0.5, 1});  // (4)

  // Add the triangle
  // First set the coordinates of its nodes:
  Eigen::MatrixXd nodesOfTria(2, 3);
  nodesOfTria << 1, 1, 0.5, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kTria(),  // we want a triangle
      std::array<lf::mesh::Mesh::size_type, 3>{
          {1, 2, 4}},  // indices of the nodes
      std::make_unique<lf::geometry::TriaO1>(nodesOfTria));  // node coords

  // Add the quadrilateral
  Eigen::MatrixXd nodesOfQuad(2, 4);
  nodesOfQuad << 0, 1, 0.5, 0, 0, 0, 1, 1;
  mesh_factory_ptr->AddEntity(
      lf::base::RefEl::kQuad(),
      std::array<lf::mesh::Mesh::size_type, 4>{{0, 1, 4, 3}},
      std::make_unique<lf::geometry::QuadO1>(nodesOfQuad));

  std::shared_ptr<lf::mesh::Mesh> demoMesh_p = mesh_factory_ptr->Build();

  return demoMesh_p;
}

/** @brief Compute the edge-vertex incidence matrix G for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The edge-vertex incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_1 */
Eigen::SparseMatrix<int> computeEdgeVertexIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store edge-vertex incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> G;
  using size_type = lf::mesh::Mesh::size_type;

  //====================
  // Your code goes here
  // index() method of lf:mesh:Mesh provides the numbering of entities which underlies
  // indexing of the entries of the incidence matrixes.
  // lf::mesh::Mesh::Index() provides a consecutive numbering of all mesh entities of a specific co-dimension
  // lf::mesh::Entity::SubEntities() returns an array of subentities and fix their ordering.
  const std::size_t nnz_row=2;
  const size_type num_edge = mesh.Numentities(1);
  const size_type num_node = mesh.Numentities(2);

  Eigen::SparseMatrix<int, Eigen::RowMajor> G(num_edge, num_node);
  G.reserve(Eigen::VectorXi::Constant(num_edge,nnz_row));

  // to compute G efficiently, we iterate over all edges and
  // check the index of the nodes at its end.
  // this is the efficient way to do the assembly, introduced as distribute scheme
  // in class
  for(lf::mesh::Entity *edge: mesh.Entities(1)){
    nonstd::span<const lf::mesh::Entity *const> node{edge.SubEntities(1)}; // the relative codimension is 1
    size_type edge_index = mesh.Index(*edge);

    size_type node_start_index = mesh.Index(*node[0]);
    size_type node_end_index = mesh.Index(*node[1]); //
    // subentities returned from SubEntities() can be accessed through [] operator using their local index
    G.coeffRef(edge_index, node_start_index) +=1;
    G.coeffRef(edge_index, node_end_index) -=1;
  }

  //====================

  return G;
}
/* SAM_LISTING_END_1 */

/** @brief Compute the cell-edge incidence matrix D for a given mesh
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *        such as lf::mesh::hybrid2d::Mesh)
 * @return The cell-edge incidence matrix as Eigen::SparseMatrix<int>
 */
/* SAM_LISTING_BEGIN_2 */
Eigen::SparseMatrix<int> computeCellEdgeIncidenceMatrix(
    const lf::mesh::Mesh &mesh) {
  // Store cell-edge incidence matrix here
  Eigen::SparseMatrix<int, Eigen::RowMajor> D;

  //====================
  // Your code goes here
  using size_type = lf::mesh::Mesh::size_type;
  const std::size_t nnz = 4;
  const size_type num_cell = mesh.NumEntities(0);
  const size_type num_edge = mesh.NumEntities(1);
  Eigen::SparseMatrix<int, Eigen::RowMajor> D(num_cell,num_edge);
  D.reserve(Eigen::VectorXi::Constant(num_cell,nnz));
  for(lf::mesh::Entity *cell: mesh.Entities(0)){
    // get edges and their orientations of a cell
    nonstd::span<const Entity* const> edges = cell->SubEntities(1);
    nonstd::span<const Entity* const> orientations = cell->RelativeOrientations();
    size_type cell_index = mesh.Index(*cell);
    auto edge_start = edges.begin();
    auto orientation_start = orientation.begin();
    // get the index of each edge and add their orientations to D
    for(; edge_start !=edges.end() && orientation_start != orientation.end(); edge_start++, orientation++){
      size_type edge_index=mesh.Index(*edge[edge_start]);
      D.coeffRef(cell_index,edge_index) += lf::mesh::to_sign(orientation_start);
    }
  }

  //====================

  return D;
}
/* SAM_LISTING_END_2 */

/** @brief For a given mesh test if the product of cell-edge and edge-vertex
 *        incidence matrix is zero: D*G == 0?
 * @param mesh The input mesh of type lf::mesh::Mesh (or of derived type,
 *             such as lf::mesh::hybrid2d::Mesh)
 * @return true, if the product is zero and false otherwise
 */
/* SAM_LISTING_BEGIN_3 */
bool testZeroIncidenceMatrixProduct(const lf::mesh::Mesh &mesh) {
  bool isZero = false;

  //====================
  // Your code goes here
  // returns true whenever the two incidence matrices of the 2D hybrid mesh satisfy
  // the relationship asserted in 2.6.6
  Eigen::SparseMatrix<int> G = computeEdgeVertexIncidenceMatrix(*mesh);
  Eigen::SparseMatrix<int> D = computeCellEdgeIncidenceMatrix(*mesh);
  auto O = G*D;

  isZero = O.norm()==0;
  }
  //====================
  return isZero;
}
/* SAM_LISTING_END_3 */

}  // namespace IncidenceMatrices


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_2_algorithms.h>
#include <CGAL/Straight_skeleton_builder_2.h>
#include <CGAL/Polygon_offset_builder_2.h>
#include <CGAL/compute_outer_frame_margin.h>
#include "print.h"

#include <boost/shared_ptr.hpp>

#include <vector>
#include <cassert>

//
// This example illustrates how to use the CGAL Straight Skeleton package
// to construct an offset contour on the outside of a polygon
//

// This is the recommended kernel
typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;

typedef Kernel::Point_2 Point_2;
typedef CGAL::Polygon_2<Kernel>    Contour;
typedef boost::shared_ptr<Contour> ContourPtr;
typedef std::vector<ContourPtr>    ContourSequence ;

typedef CGAL::Straight_skeleton_2<Kernel> Ss;

typedef Ss::Halfedge_iterator Halfedge_iterator;
typedef Ss::Halfedge_handle   Halfedge_handle;
typedef Ss::Vertex_handle     Vertex_handle;

typedef CGAL::Straight_skeleton_builder_traits_2<Kernel>      SsBuilderTraits;
typedef CGAL::Straight_skeleton_builder_2<SsBuilderTraits,Ss> SsBuilder;

typedef CGAL::Polygon_offset_builder_traits_2<Kernel>                  OffsetBuilderTraits;
typedef CGAL::Polygon_offset_builder_2<Ss,OffsetBuilderTraits,Contour> OffsetBuilder;

int main()
{
  // A start-shaped polygon, oriented counter-clockwise as required for outer contours.
  Point_2 pts[] = { Point_2(-1,-1)
                  , Point_2(0,-12)
                  , Point_2(1,-1)
                  , Point_2(12,0)
                  , Point_2(1,1)
                  , Point_2(0,12)
                  , Point_2(-1,1)
                  , Point_2(-12,0)
                  } ;

  std::vector<Point_2> star(pts,pts+8);

  assert(CGAL::orientation_2(pts,pts+8,Kernel()) == CGAL::COUNTERCLOCKWISE);

  // We want an offset contour in the outside.
  // Since the package doesn't support that operation directly, we use the following trick:
  // (1) Place the polygon as a hole of a big outer frame.
  // (2) Construct the skeleton on the interior of that frame (with the polygon as a hole)
  // (3) Construc the offset contours
  // (4) Identify the offset contour that corresponds to the frame and remove it from the result

  double offset = 3 ; // The offset distance

  // First we need to determine the proper separation between the polygon and the frame.
  // We use this helper function provided in the package.
  boost::optional<double> margin = CGAL::compute_outer_frame_margin(star.begin(),star.end(),offset);

  // Proceed only if the margin was computed (an extremely sharp corner might cause overflow)
  if ( margin )
  {
    // Get the bbox of the polygon
    CGAL::Bbox_2 bbox = CGAL::bbox_2(star.begin(),star.end());

    // Compute the boundaries of the frame
    double fxmin = bbox.xmin() - *margin ;
    double fxmax = bbox.xmax() + *margin ;
    double fymin = bbox.ymin() - *margin ;
    double fymax = bbox.ymax() + *margin ;

    // Create the rectangular frame
    Point_2 frame[4]= { Point_2(fxmin,fymin)
                      , Point_2(fxmax,fymin)
                      , Point_2(fxmax,fymax)
                      , Point_2(fxmin,fymax)
                      } ;

    // Instantiate the skeleton builder
    SsBuilder ssb ;

    // Enter the frame
    ssb.enter_contour(frame,frame+4);

    // Enter the polygon as a hole of the frame (NOTE: as it is a hole we insert it in the opposite orientation)
    ssb.enter_contour(star.rbegin(),star.rend());

    // Construct the skeleton
    boost::shared_ptr<Ss> ss = ssb.construct_skeleton();

    // Proceed only if the skeleton was correctly constructed.
    if ( ss )
    {
      print_straight_skeleton(*ss);

      // Instantiate the container of offset contours
      ContourSequence offset_contours ;

      // Instantiate the offset builder with the skeleton
      OffsetBuilder ob(*ss);

      // Obtain the offset contours
      ob.construct_offset_contours(offset, std::back_inserter(offset_contours));

      // Locate the offset contour that corresponds to the frame
      // That must be the outmost offset contour, which in turn must be the one
      // with the largetst unsigned area.
      ContourSequence::iterator f = offset_contours.end();
      double lLargestArea = 0.0 ;
      for (ContourSequence::iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i  )
      {
        double lArea = CGAL_NTS abs( (*i)->area() ) ; //Take abs() as  Polygon_2::area() is signed.
        if ( lArea > lLargestArea )
        {
          f = i ;
          lLargestArea = lArea ;
        }
      }

      // Remove the offset contour that corresponds to the frame.
      offset_contours.erase(f);

      print_polygons(offset_contours);
    }
  }

  return 0;
}


/*
 * gr_checker.cpp
 *
 *  Created on: 04.01.2015
 *      Author: schlund
 */

#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

#include "datastructs/matrix.h"
#include "datastructs/equations.h"

#include "polynomials/commutative_polynomial.h"
#include "polynomials/non_commutative_polynomial.h"
#include "polynomials/lossy_non_commutative_polynomial.h"

#include "semirings/pseudo_linear_set.h"
#include "semirings/semilinear_set.h"

#include "semirings/semilinSetNdd.h"
#include "semirings/semilinSetNdd.h"

#include "parser.h"

#include "solvers/newton_generic.h"
#include "solvers/solver_utils.h"

#include "utils/string_util.h"
#include "utils/timer.h"

// check whether a set of grammars generates the same language up to commuativity
// (and modulo additional overapproximations given by the semiring)
template <typename SR>
void check_all_equal_commutative(const std::string& startsymbol, const std::vector<std::string>& inputs) {

  Parser p;
  int num_grammars = inputs.size();

  auto nc_equations = p.free_parser(inputs[0]);

  std::cout << "Eq (non-comm) : " << std::endl;
  PrintEquations(nc_equations);

  // Use appropriate semiring (has to be commutative!)
  auto equations_fst = MakeCommEquationsAndMap(nc_equations, [](const FreeSemiring &c) -> SR {
    auto srconv = SRConverter<SR>();
    return c.Eval(srconv);
  });

  std::cout << "Eq (comm) : "  << std::endl;
  PrintEquations(equations_fst);

  Timer timer;
  timer.Start();

  ValuationMap<SR> sol_fst = apply_solver<NewtonCL, CommutativePolynomial>(equations_fst, true, false, 0, false);

  bool all_equal = true;
  for(int i=1; i<num_grammars; i++) {
    auto equations = MakeCommEquationsAndMap(p.free_parser(inputs[i]), [](const FreeSemiring &c) -> SR {
      auto srconv = SRConverter<SR>();
      return c.Eval(srconv);
    });

    ValuationMap<SR> sol = apply_solver<NewtonCL, CommutativePolynomial>(equations, true, false, 0, false);

    if(startsymbol.compare("") == 0) {
      if(sol[equations[0].first] != sol_fst[equations_fst[0].first]) {
        std::cout << "[DIFF] Difference found for startsymbols (" << equations_fst[0].first << "," << equations[0].first << ")" << std::endl;
        std::cout << "0:" << result_string(sol_fst) << std::endl << i << ":" << result_string(sol) << std::endl;
        all_equal = false;
        break;
      }
    }
    else {

      if(sol.find(Var::GetVarId(startsymbol)) == sol.end() || sol_fst.find(Var::GetVarId(startsymbol)) == sol_fst.end()) {
        std::cout << "[ERROR] startsymbol (" << startsymbol << ") does not occur!"<< std::endl;
        return;
      }
      else if(sol[Var::GetVarId(startsymbol)] != sol_fst[Var::GetVarId(startsymbol)]) {
        std::cout << "[DIFF] Difference found for startsymbol (" << startsymbol << ")" << std::endl << "0:" << result_string(sol_fst)
                                               << std::endl << i << ":" << result_string(sol) << std::endl;
        all_equal = false;
        break;
      }
    }

  }

  if(all_equal) {
    std::cout << "[EQUIV] All grammars equivalent modulo commutativity" << std::endl;
  }

  timer.Stop();
  std::cout
  << "Total checking time:\t" << timer.GetMilliseconds().count()
  << " ms" << " ("
  << timer.GetMicroseconds().count()
  << "us)" << std::endl;

}

void check_all_equal_lossy(const std::string& startsymbol, const std::vector<std::string>& inputs, int refinementDepth) {
  int num_grammars = inputs.size();
  Parser p;
  auto eq_tmp = MapEquations(p.free_parser(inputs[0]), [](const FreeSemiring &c) -> LossyFiniteAutomaton {
    auto srconv = SRConverter<LossyFiniteAutomaton>();
    return c.Eval(srconv);
  });

  auto equations_fst = NCEquationsBase<LossyFiniteAutomaton>(eq_tmp.begin(), eq_tmp.end());

  VarId S_1;
  if(startsymbol.compare("") == 0) {
    S_1 = equations_fst[0].first;
  } else {
    S_1 = Var::GetVarId(startsymbol);
  }

  Timer timer;
  timer.Start();

  bool all_equal = true;
  for(int i=1; i<num_grammars; i++) {

    auto eq_tmp2 = MapEquations(p.free_parser(inputs[i]), [](const FreeSemiring &c) -> LossyFiniteAutomaton {
      auto srconv = SRConverter<LossyFiniteAutomaton>();
      return c.Eval(srconv);
    });
    auto equations = NCEquationsBase<LossyFiniteAutomaton>(eq_tmp2.begin(), eq_tmp2.end());

    VarId S_2;
    if(startsymbol.compare("") == 0) {
      S_2 = equations[0].first;
    } else {
      S_2 = Var::GetVarId(startsymbol);
    }

    auto witness = NonCommutativePolynomial<LossyFiniteAutomaton>::refineCourcelle(equations_fst, S_1, equations, S_1, refinementDepth);

    if(witness != LossyFiniteAutomaton::null()) {
      if(startsymbol.compare("") == 0) {
        std::cout << "[DIFF] Difference found for startsymbols (" << equations_fst[0].first << "," << equations[0].first << ")" << std::endl;
      }
      else {
        std::cout << "[DIFF] Difference found for startsymbols (" << S_1 << "," << S_2 << ")" << std::endl;
      }
        std::cout << "Witness: " << witness.string() << std::endl;
        all_equal = false;
        break;
    }
  }

  if(all_equal) {
    std::cout << "[EQUIV] All grammars equivalent modulo subword-closure" << std::endl;
  }

  timer.Stop();
  std::cout
  << "Total checking time:\t" << timer.GetMilliseconds().count()
  << " ms" << " ("
  << timer.GetMicroseconds().count()
  << "us)" << std::endl;

}

/*
 * Tests whether two grammars generate the same language modulo commutativity.
 * We use semilinear sets in constant-period representation to represent Parikh images
 * and check their equivalence via NDDs.
 */
int main(int argc, char* argv[]) {
  namespace po = boost::program_options;

  po::options_description generic("Generic options");
  generic.add_options()
        ( "help,h", "print this help message" )
        ( "startsymbol,s", po::value<std::string>(), "start symbol of the grammars")
        ( "input", po::value<std::vector<std::string> >(), "input grammars (at least two): g1 g2 [g3] [...]" )
        ( "slset", "commutative abstraction via semilinear sets" )
        ( "lossy", "abstraction via subword closure" )
        ("refD", po::value<int>(), "refinement Depth for Lossy Approximation")
        ;

  po::positional_options_description pos;
  pos.add("input", -1);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(generic).positional(pos).run(), vm);
  po::notify(vm);

  SemilinSetNdd::genepi_init();

  if(vm.count("help")) {
    std::cout << generic << std::endl;
    return EXIT_SUCCESS;
  }

  std::vector<std::string> input_files = vm["input"].as< std::vector<std::string> >();
  int num_grammars = input_files.size();
  std::vector<std::string> inputs;

  std::string startsymbol = "";

  if(vm.count("startsymbol")) {
    startsymbol = vm["startsymbol"].as<std::string>();
    std::cout << "Comparing startsymbols (" << startsymbol << ")" << std::endl;
  }
  else {
    std::cout << "No startsymbol specified, using defaults." << std::endl;
  }

  if (vm.count("input") && num_grammars > 1) {
    // we are reading the input from the given files
    std::ifstream file;

    for(auto& filename : input_files) {
      file.open(filename, std::ifstream::in);
      if (file.fail()) {
        std::cerr << "Could not open input file: " << filename << std::endl;
      }
      std::string line;
      std::vector<std::string> input;
      while (std::getline(file, line)) {
        input.push_back(line);
      }
      // join the input into one string
      inputs.push_back( std::accumulate(input.begin(), input.end(), std::string("")) );
      file.close();
    }
  } else {
    std::cout << "Please provide at least two input files!" << std::endl;
    return EXIT_FAILURE;
  }

  if(!vm.count("slset") && !vm.count("lossy")) {
    std::cout << "Please specify the abstraction used for checking equivalence!" << std::endl;
    return EXIT_FAILURE;
  }

  if(vm.count("slset")) {
    std::cout << "Plain Semilinear Sets" << std::endl;
    // no overapproximation -- just plain semilinear sets with simplification
    // (the approximations are not sound for inequivalence-testing!)
    check_all_equal_commutative<SemilinearSetL>(startsymbol, inputs);
  }

  if(vm.count("lossy")) {
    std::cout << "Lossy Approximation" << std::endl;

    int refinementDepth = 0;
    if(vm.count("refD")) {
        refinementDepth = vm["refine"].as<int>();
    }

    check_all_equal_lossy(startsymbol, inputs, refinementDepth);
  }

  SemilinSetNdd::genepi_dealloc();

  return EXIT_SUCCESS;
}


#ifndef OPTIMIZERS_HPP
#define OPTIMIZERS_HPP

// Eigen includes --------------------
#include <Eigen/Dense>

// Own includes --------------------
#include "optimizers/base-optimizer.hpp"

namespace NNet { // begin NNet

	/**
	 *SGDOptimizer.
	 */
	template< typename NetworkType >
	class SGDOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		SGDOptimizer( ) = delete;
		explicit SGDOptimizer( NetworkType& network, NumericType learningRate = 0.001 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ) {
		}
		SGDOptimizer( SGDOptimizer const& other ) = delete;
		~SGDOptimizer( ) = default;

		//get/set member functions
		NumericType getLearningRate( ) { return mLearningRate; }

		// interface
		void applyWeightUpdate( std::size_t batchSize ) override {
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				auto coeff = 1.0 / static_cast< NumericType >( batchSize );
				// std::cout << "LearningRate, coeff: " << mLearningRate << ", " << coeff << std::endl;
				// std::cout << "WeightMat Before: " << weightMat.rows( ) << ", " << weightMat.cols( ) << std::endl
				// 		  << weightMat << std::endl;
				// std::cout << "WeightGradMat Before: " << std::endl
				// 		  << weightGradMat << std::endl;
				weightMat = weightMat - mLearningRate * coeff * weightGradMat;
			}
		}
	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate;

	}; // end of class SGDOptimizer

    /**
	 *MomentumOptimizer.
	 */
	template< typename NetworkType >
	class MomentumOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		MomentumOptimizer( ) = delete;
		explicit MomentumOptimizer( NetworkType& network, NumericType learningRate = 0.001, NumericType momentum = 0.9 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ), mMomentum( momentum ) {
			for ( auto const& layer : this -> getTrainableLayers( ) ) {
				MatrixXType const& weightGradMat = layer -> getWeightGradMat( );
				auto numRows = weightGradMat.rows( );
				auto numCols = weightGradMat.cols( );
				MatrixXType mat( numRows, numCols );
				mat.setZero( );
				mWeightGradMatSaves.emplace_back( mat );
			}
		}
		MomentumOptimizer( MomentumOptimizer const& other ) = delete;
		~MomentumOptimizer( ) = default;

		//get/set member functions
		NumericType getLearningRate( ) const { return mLearningRate; }
		void setLearningRate( NumericType learningRate ) { mLearningRate = learningRate; }
		NumericType getMomentum( ) const { return mMomentum; }
		void setMomentum( NumericType momentum ) { mMomentum = momentum; }

		// interface
		void applyWeightUpdate( std::size_t batchSize ) override {
			auto v_iter = mWeightGradMatSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				NumericType coeff = 1.0 / static_cast< NumericType >( batchSize );
				// compute the velocity v_{t+1} at time t+1
				// note that v_iter at t = 0 is zero
				MatrixXType v = mMomentum * (*v_iter) - mLearningRate * coeff * weightGradMat;
				*v_iter = v;
				weightMat = weightMat + v;
				++v_iter;
			}
		}
	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate, mMomentum;
		std::vector< MatrixXType > mWeightGradMatSaves = { };
	}; // end of class MomentumOptimizer

	/**
	 *NesterovMomentumOptimizer.
	 */
	template< typename NetworkType >
	class NesterovMomentumOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		NesterovMomentumOptimizer( ) = delete;
		explicit NesterovMomentumOptimizer( NetworkType& network, NumericType learningRate = 0.001, NumericType momentum = 0.9 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ), mMomentum( momentum ) {
			for ( auto const& layer : this -> getTrainableLayers( ) ) {
				MatrixXType const& weightGradMat = layer -> getWeightGradMat( );
				auto numRows = weightGradMat.rows( );
				auto numCols = weightGradMat.cols( );
				MatrixXType mat( numRows, numCols );
				mat.setZero( );
				mWeightGradMatSaves.emplace_back( mat );
			}
		}
		NesterovMomentumOptimizer( NesterovMomentumOptimizer const& other ) = delete;
		~NesterovMomentumOptimizer( ) = default;

		//get/set member functions
		NumericType getLearningRate( ) { return mLearningRate; }
		void setLearningRate( NumericType learningRate ) { mLearningRate = learningRate; }
		NumericType getMomentum( ) const { return mMomentum; }
		void setMomentum( NumericType momentum ) { mMomentum = momentum; }

		// interface
		void applyInterimUpdate( ) override {
			auto v_iter = mWeightGradMatSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				weightMat = weightMat + mMomentum * ( *v_iter );
				++v_iter;
			}
		}
		void applyWeightUpdate( std::size_t batchSize ) override {
			auto v_iter = mWeightGradMatSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				NumericType coeff = 1.0 / static_cast< NumericType >( batchSize );
				MatrixXType v = mMomentum * (*v_iter) - mLearningRate * coeff * weightGradMat;
				*v_iter = v;
				weightMat = weightMat - mLearningRate * coeff * weightGradMat;
				++v_iter;
			}
		}
	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate, mMomentum;
		std::vector< MatrixXType > mWeightGradMatSaves = { };
	}; // end of class NesterovMomentumOptimizer

	/**
	 *AdaGradOptimizer.
	 */
	template< typename NetworkType >
	class AdaGradOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		AdaGradOptimizer() = delete;
		explicit AdaGradOptimizer( NetworkType& network, NumericType learningRate = 0.01 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ) {
			for ( auto const& layer : this -> getTrainableLayers( ) ) {
				MatrixXType const& weightGradMat = layer -> getWeightGradMat( );
				auto numRows = weightGradMat.rows( );
				auto numCols = weightGradMat.cols( );
				MatrixXType mat( numRows, numCols );
				mat.setZero( );
				mGradMatAccumSaves.emplace_back( mat );
			}
		}
		AdaGradOptimizer(const AdaGradOptimizer &c) = delete;
		~AdaGradOptimizer() = default;

		//get/set member functions
		NumericType getLearningRate( ) const { return mLearningRate; }
		void setLearningRate( NumericType learningRate ) { mLearningRate = learningRate; }

		// interface
		void applyWeightUpdate( std::size_t batchSize ) override {
			auto r_iter = mGradMatAccumSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				NumericType coeff = 1.0 / static_cast< NumericType >( batchSize );
				// accumulate squared gradient
				MatrixXType grad_sq = coeff * coeff * weightGradMat.cwiseProduct( weightGradMat );
				*r_iter = (*r_iter) + grad_sq;
				MatrixXType r = *r_iter;
				r = r.unaryExpr( [this]( auto const& ele ) {
					return ( mLearningRate / ( 1.0e-7 + std::sqrt( ele ) ) );
				} );
				weightMat = weightMat - r.cwiseProduct( coeff * weightGradMat );
				++r_iter;
			}
		}

	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate;
		std::vector< MatrixXType > mGradMatAccumSaves = { };
	}; // end of class AdaGradOptimizer

	/**
	 *RMSPropOptimizer.
	 */
	template< typename NetworkType >
	class RMSPropOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		RMSPropOptimizer() = delete;
		explicit RMSPropOptimizer( NetworkType& network, NumericType learningRate = 0.001, NumericType decayRate = 0.9 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ), mDecayRate( decayRate ) {
			for ( auto const& layer : this -> getTrainableLayers( ) ) {
				MatrixXType const& weightGradMat = layer -> getWeightGradMat( );
				auto numRows = weightGradMat.rows( );
				auto numCols = weightGradMat.cols( );
				MatrixXType mat( numRows, numCols );
				mat.setZero( );
				mGradMatAccumSaves.emplace_back( mat );
			}
		}
		RMSPropOptimizer( RMSPropOptimizer const& other ) = delete;
		~RMSPropOptimizer() = default;

		//get/set member functions
		NumericType getLearningRate( ) const { return mLearningRate; }
		void setLearningRate( NumericType learningRate ) { mLearningRate = learningRate; }
		NumericType getDecayRate( ) const { return mDecayRate; }
		void setDecayRate( NumericType decayRate ) { mDecayRate = decayRate; }

		// interface
		void applyWeightUpdate( std::size_t batchSize ) override {
			auto r_iter = mGradMatAccumSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				NumericType coeff = 1.0 / static_cast< NumericType >( batchSize );
				// accumulate squared gradient
				MatrixXType grad_sq = coeff * coeff * weightGradMat.cwiseProduct( weightGradMat );
				*r_iter = mDecayRate * (*r_iter) + (1.0 - mDecayRate ) * grad_sq;
				MatrixXType r = *r_iter;
				r = r.unaryExpr( [this]( auto const& ele ) {
					return ( mLearningRate / ( 1.0e-6 + std::sqrt( ele ) ) );
				} );
				weightMat = weightMat - r.cwiseProduct( coeff * weightGradMat );
				++r_iter;
			}
		}

	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate, mDecayRate;
		std::vector< MatrixXType > mGradMatAccumSaves = { };
	}; // end of class RMSPropOptimizer

	/**
	 *RMSPropNestMomOptimizer.
	 */
	template< typename NetworkType >
	class RMSPropNestMomOptimizer
		: public BaseOptimizer< NetworkType > {
	public: 	// public typedefs
		using NumericType = typename NetworkType::NumericType;
		using VectorXType = typename NetworkType::VectorXType;
		using MatrixXType = typename NetworkType::MatrixXType;

	private: 	// private typedefs

	public: 	//public member functions
		RMSPropNestMomOptimizer() = delete;
		explicit RMSPropNestMomOptimizer( NetworkType& network, NumericType learningRate = 0.001, NumericType momentum = 0.9, NumericType decayRate = 0.9 )
			: BaseOptimizer< NetworkType >( network ), mLearningRate( learningRate ), mMomentum( momentum ), mDecayRate( decayRate ) {
			for ( auto const& layer : this -> getTrainableLayers( ) ) {
				MatrixXType const& weightGradMat = layer -> getWeightGradMat( );
				auto numRows = weightGradMat.rows( );
				auto numCols = weightGradMat.cols( );
				MatrixXType mat( numRows, numCols );
				mat.setZero( );
				mWeightGradMatSaves.emplace_back( mat );
				mGradMatAccumSaves.emplace_back( mat );
			}
		}
		RMSPropNestMomOptimizer( RMSPropNestMomOptimizer const& other ) = delete;
		~RMSPropNestMomOptimizer() = default;

		//get/set member functions
		NumericType getLearningRate( ) const { return mLearningRate; }
		void setLearningRate( NumericType learningRate ) { mLearningRate = learningRate; }
		NumericType getMomentum( ) const { return mMomentum; }
		void setMomentum( NumericType momentum ) { mMomentum = momentum; }
		NumericType getDecayRate( ) const { return mDecayRate; }
		void setDecayRate( NumericType decayRate ) { mDecayRate = decayRate; }

		// interface
		void applyInterimUpdate( ) override {
			auto v_iter = mWeightGradMatSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				weightMat = weightMat + mMomentum * ( *v_iter );
				++v_iter;
			}
		}
		void applyWeightUpdate( std::size_t batchSize ) override {
			auto v_iter = mWeightGradMatSaves.begin( );
			auto r_iter = mGradMatAccumSaves.begin( );
			for ( auto& layerPtr : this -> getTrainableLayers( ) ) {
				MatrixXType& weightMat = layerPtr -> getWeightMat( );
				MatrixXType const& weightGradMat = layerPtr -> getWeightGradMat( );
				NumericType coeff = 1.0 / static_cast< NumericType >( batchSize );
				// accumulate squared gradient
				MatrixXType grad_sq = coeff * coeff * weightGradMat.cwiseProduct( weightGradMat );
				*r_iter = mDecayRate * (*r_iter) + (1.0 - mDecayRate ) * grad_sq;
				MatrixXType r = *r_iter;
				r = r.unaryExpr( [this]( auto const& ele ) {
					return ( mLearningRate / ( 1.0e-7 + std::sqrt( ele ) ) );
				} );
				MatrixXType v = mMomentum * ( *v_iter ) - r.cwiseProduct( coeff * weightGradMat );
				*v_iter = v;
				weightMat = weightMat - r.cwiseProduct( coeff * weightGradMat );
				++v_iter;
				++r_iter;
			}
		}

	private: 	//private member functions

	public: 	//public data members

	private: 	//private data members
		NumericType mLearningRate, mMomentum, mDecayRate;
		std::vector< MatrixXType > mWeightGradMatSaves = { }, mGradMatAccumSaves = { };
	}; // end of class RMSPropNestMomOptimizer

} // end NNet

#endif // OPTIMIZERS_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2016 Sebastian Schlenkrich

*/

#ifndef quantlib_templateauxilliaries_regression_hpp
#define quantlib_templateauxilliaries_regression_hpp

//#include <ql/types.hpp>
//#include <boost/function.hpp>

//#include <ql/experimental/template/auxilliaries/templatesvd.hpp>
#include <ql/experimental/templatemodels/auxilliaries/qrfactorisationT.hpp>

namespace TemplateAuxilliaries {

    template <class Type>
    class Regression {

    protected:

        size_t                               maxDegree_;  // max polynomial degree
        std::vector< std::vector<size_t> >   multIdx_;    // list of all multi-indeces with degree <= maxDegree_
        std::vector<Type>                    beta_;       // linear coefficients

        inline void divide( std::vector<size_t> x, size_t idx, size_t degree) {
            if (idx==x.size()-1) {
                x[idx] = degree;
                multIdx_.push_back(x);
            } else {
                for (size_t k=0; k<=degree; ++k) {
                    x[idx] = k;
                    divide(x, idx+1, degree-k);
                }
            }
        }

        // initialise multi-index matrix via recursive call of divide()
        inline void setUpMultiIndex(const size_t dim, const size_t maxDegree) {
            multIdx_.clear();
            std::vector<size_t> x(dim,0);
            for (size_t k=0; k<=maxDegree; ++k) divide(x,0,k);
        }

        // perform actual regression calculation
        inline void calculateRegression( const std::vector< std::vector<Type> >& controls,
                                         const std::vector< Type >&              observations ) {
            std::vector<Type> b(observations);
            std::vector< std::vector<Type> >  M(controls.size());
            for (size_t i=0; i<M.size(); ++i) M[i] = monomials(controls[i]);
            qrsolveles(M,b);
            for (size_t i=0; i<beta_.size(); ++i) beta_[i] = b[i];

        }

    public:

        Regression ( const std::vector< std::vector<Type> >& controls,
                     const std::vector< Type >&              observations,
                     const size_t                            maxDegree ) : maxDegree_(maxDegree) {
            // check dimensions
            size_t nRows = 0;
            if (controls.size()==observations.size()) nRows = controls.size();

            if (nRows>0) setUpMultiIndex(controls[0].size(),maxDegree_);
            size_t nCols = multIdx_.size();

            // initialise beta
            beta_.resize(nCols,0.0);
            if ((nRows>0)&&(nRows>=nCols))
                calculateRegression(controls,observations);  // if nRows < nCols regression does not really make sense

        }

        const std::vector<Type> monomials( const std::vector<Type>& x ) const {
            std::vector<Type> y(multIdx_.size(),0.0);
            if ((multIdx_.size()==0)||(multIdx_[0].size()!=x.size())) return y;  // dimension mismatch
            for (size_t i=0; i<y.size(); ++i) {
                y[i] = 1.0;
                for (size_t j=0; j<x.size(); ++j) {  // don't want to use pow coz not clear how it's implemented
                    for (size_t k=0; k<multIdx_[i][j]; ++k) y[i] *= x[j];
                }
            }
            return y;
        }

        const Type value( const std::vector<Type>& x ) const {
            std::vector<Type> y = monomials(x);
            if (y.size()!=beta_.size()) return 0.0;  // dimension mismatch
            Type res = 0.0;
            for (size_t i=0; i<y.size(); ++i) res += beta_[i] * y[i];
            return res;
        }

        // inspectors

        const size_t                              maxDegree() const { return maxDegree_; }
        const std::vector< std::vector<size_t> >& multIdx()   const { return multIdx_;   }
        const std::vector<Type>&                  beta()      const { return beta_;      }

        const std::vector< std::vector<Type> >    multiIndex() const {  // workaround for Excel interface debugging
            std::vector< std::vector<Type> > M(multIdx_.size());
            for (size_t i=0; i<multIdx_.size(); ++i) {
                M[i].resize(multIdx_[i].size());
                for (size_t j=0; j<multIdx_[i].size(); ++j) M[i][j] = multIdx_[i][j];
            }
            return M;
        }

    };

}

#endif  /* ifndef quantlib_templateauxilliaries_regression_hpp */


#ifndef CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H
#define CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H

#include <cmath>
#include <utility>
#include <random>

#include <Eigen/Dense>

#include <ompl/control/spaces/RealVectorControlSpace.h>

#include <cannon/physics/systems/system.hpp>
#include <cannon/physics/euler_integrator.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

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

namespace cannon {
  namespace physics {
    namespace systems {

      struct PendSystem : System {
        PendSystem(double g = 10.0, double m = 1.0, double l = 1.0, double dt =
            0.05, double max_speed = 8.0) : g_(g), m_(m), l_(l), dt_(dt),
        max_speed_(max_speed) {}

        virtual void operator()(const VectorXd& x, VectorXd& dxdt, const double /*t*/) override {
          double th = x[0];
          double thdot = x[1];
          double u = x[2];

          double new_thdot = (-3 * g_ / (2 * l_)) * std::sin(th + M_PI) +
            (3.0 * u / (m_ * std::pow(l_, 2.0)));
          double new_th = thdot + (new_thdot * dt_);

          dxdt.resize(3);
          dxdt[0] = new_th;
          dxdt[1] = new_thdot;
          dxdt[2] = 0.0;
        }

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

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

          VectorXd s(3);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = u;
          VectorXd dsdt(3);

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

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

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          MatrixXd A = MatrixXd::Zero(2, 2);
          MatrixXd B = MatrixXd::Zero(2, 1);
          VectorXd c = VectorXd::Zero(2);

          // TODO

          return std::make_tuple(A, B, c);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A,
                                          Ref<MatrixXd> B) override {
          throw std::runtime_error("Not implemented yet");
        }

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

        // Parameters
        double g_;
        double m_;
        double l_;
        double dt_;
        double max_speed_;
      };

      class InvertedPendulum {
        public:
          InvertedPendulum(double max_torque = 2.0) : max_torque_(max_torque),
          e_(s_, 3, 0.05) {
            std::random_device rd;
            gen_ = std::mt19937(rd());

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

            state_ = Vector3d::Zero(3);
            reset();
          }

          std::pair<VectorXd, double> step(double u) {
            double th = state_[0];
            double thdot = state_[1];

            double reward = -(std::pow(normalize_(th), 2.0) + (0.1 * std::pow(thdot,
                    2.0)) + (0.001 * std::pow(u, 2.0)));

            double clipped_u = std::max(-max_torque_, std::min(max_torque_, u));
            state_[2] = clipped_u;

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

            state_[0] = std::atan2(std::sin(state_[0]),std::cos(state_[0]));
            state_[1] = std::max(-8.0, std::min(state_[1], 8.0));

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

          VectorXd reset() {
            double th = th_dis_(gen_);
            double thdot = thdot_dis_(gen_);

            state_[0] = th;
            state_[1] = thdot;
            state_[2] = 0.0;

            return state_.head(2);
          }

          PendSystem s_;

        private:
          inline double normalize_(double th) {
            return (std::fmod((th + M_PI), (2.0 * M_PI)) - M_PI);
          }

          double max_torque_;

          EulerIntegrator<PendSystem> e_;

          Vector3d state_;

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

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

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_INVERTED_PENDULUM_H */


//
// Created by liyubo on 12/15/17.
//

#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
#include"hw.h"
#define MATRIX_SIZE 50

void homework()
{

//作业
    Eigen::MatrixXd matrix_A;
    matrix_A = Eigen::MatrixXd::Random( 100, 100 );
    Eigen::MatrixXd matrix_b;
    matrix_b = Eigen::MatrixXd::Random( 100, 1 );
    Eigen::MatrixXd x;
// cout << matrix_A << endl;
// cout << matrix_b << endl;
    x = matrix_A.llt().solve(matrix_b);   //llt Cholesky来解方程

    /*******************时间比较*********************/

    clock_t time_stt = clock();
    x = matrix_A.colPivHouseholderQr().solve(matrix_b);  //利用QR分解求解方程
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    time_stt = clock();
    x = matrix_A.fullPivLu().solve(matrix_b);   //LU
    cout <<"time use  in fullPivLu  is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    time_stt = clock();
    x = matrix_A.inverse()*matrix_b;;  //利用求逆来解方程
    cout <<"time use  in normal inverse  is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    time_stt = clock();
    x = matrix_A.llt().solve(matrix_b);   //llt Cholesky来解方程
    cout <<"time use  in llt(Cholesky)  is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    time_stt = clock();
    x = matrix_A.ldlt().solve(matrix_b);   //ldlt
    cout <<"time use  in ldlt is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms"<< endl;
}


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

#ifndef BOOST_MATH_TOOLS_CENTERED_CONTINUED_FRACTION_HPP
#define BOOST_MATH_TOOLS_CENTERED_CONTINUED_FRACTION_HPP

#include <cmath>
#include <cstdint>
#include <vector>
#include <ostream>
#include <iomanip>
#include <limits>
#include <stdexcept>
#include <sstream>
#include <array>
#include <type_traits>
#include <boost/math/tools/is_standalone.hpp>

#ifndef BOOST_MATH_STANDALONE
#include <boost/core/demangle.hpp>
#endif

namespace boost::math::tools {

template<typename Real, typename Z = int64_t>
class centered_continued_fraction {
public:
    centered_continued_fraction(Real x) : x_{x} {
        static_assert(std::is_integral_v<Z> && std::is_signed_v<Z>,
                      "Centered continued fractions require signed integer types.");
        using std::round;
        using std::abs;
        using std::sqrt;
        using std::isfinite;
        if (!isfinite(x))
        {
            throw std::domain_error("Cannot convert non-finites into continued fractions.");
        }
        b_.reserve(50);
        Real bj = round(x);
        b_.push_back(static_cast<Z>(bj));
        if (bj == x)
        {
            b_.shrink_to_fit();
            return;
        }
        x = 1/(x-bj);
        Real f = bj;
        if (bj == 0)
        {
            f = 16*(std::numeric_limits<Real>::min)();
        }
        Real C = f;
        Real D = 0;
        int i = 0;
        while (abs(f - x_) >= (1 + i++)*std::numeric_limits<Real>::epsilon()*abs(x_))
        {
            bj = round(x);
            b_.push_back(static_cast<Z>(bj));
            x = 1/(x-bj);
            D += bj;
            if (D == 0) {
                D = 16*(std::numeric_limits<Real>::min)();
            }
            C = bj + 1/C;
            if (C==0)
            {
                C = 16*(std::numeric_limits<Real>::min)();
            }
            D = 1/D;
            f *= (C*D);
        }
        // Deal with non-uniqueness of continued fractions: [a0; a1, ..., an, 1] = a0; a1, ..., an + 1].
        if (b_.size() > 2 && b_.back() == 1)
        {
            b_[b_.size() - 2] += 1;
            b_.resize(b_.size() - 1);
        }
        b_.shrink_to_fit();

        for (size_t i = 1; i < b_.size(); ++i)
        {
            if (b_[i] == 0) {
                std::ostringstream oss;
                oss << "Found a zero partial denominator: b[" << i << "] = " << b_[i] << "."
                    #ifndef BOOST_MATH_STANDALONE
                    << " This means the integer type '" << boost::core::demangle(typeid(Z).name())
                    #else
                    << " This means the integer type '" << typeid(Z).name()
                    #endif
                    << "' has overflowed and you need to use a wider type,"
                    << " or there is a bug.";
                throw std::overflow_error(oss.str());
            }
        }
    }

    Real khinchin_geometric_mean() const {
        if (b_.size() == 1)
        {
            return std::numeric_limits<Real>::quiet_NaN();
        }
        using std::log;
        using std::exp;
        using std::abs;
        const std::array<Real, 7> logs{std::numeric_limits<Real>::quiet_NaN(), Real(0), log(static_cast<Real>(2)), log(static_cast<Real>(3)), log(static_cast<Real>(4)), log(static_cast<Real>(5)), log(static_cast<Real>(6))};
        Real log_prod = 0;
        for (size_t i = 1; i < b_.size(); ++i)
        {
            if (abs(b_[i]) < static_cast<Z>(logs.size()))
            {
                log_prod += logs[abs(b_[i])];
            }
            else
            {
                log_prod += log(static_cast<Real>(abs(b_[i])));
            }
        }
        log_prod /= (b_.size()-1);
        return exp(log_prod);
    }

    const std::vector<Z>& partial_denominators() const {
        return b_;
    }

    template<typename T, typename Z2>
    friend std::ostream& operator<<(std::ostream& out, centered_continued_fraction<T, Z2>& ccf);

private:
    const Real x_;
    std::vector<Z> b_;
};

template<typename Real, typename Z2>
std::ostream& operator<<(std::ostream& out, centered_continued_fraction<Real, Z2>& scf) {
    constexpr const int p = std::numeric_limits<Real>::max_digits10;
    if constexpr (p == 2147483647)
    {
        out << std::setprecision(scf.x_.backend().precision());
    }
    else
    {
        out << std::setprecision(p);
    }

    out << "[" << scf.b_.front();
    if (scf.b_.size() > 1)
    {
        out << "; ";
        for (size_t i = 1; i < scf.b_.size() -1; ++i)
        {
            out << scf.b_[i] << ", ";
        }
        out << scf.b_.back();
    }
    out << "]";
    return out;
}

}
#endif


#include "eigen_ext.hpp"
#include "function.hpp"
#include "parameters.hpp"
#include <cassert>
#include <iostream>

#include <Eigen/Core>
#include <Eigen/SparseCore>

namespace pear {
void fun_diff(Vec &Cu, Vec &Cv, Vec &Ru, Vec &Rv, Mat &RudCu, Mat &RudCv,
              Mat &RvdCu, Mat &RvdCv) {

  int np = Cu.rows();

  // FUNCTIONS
  Ru = pear::Vmu * Cu.array() /
       ((pear::Kmu + Cu.array()) * (1 + Cv.array() / pear::Kmv));

  Rv = pear::rq * Ru.array() + pear::Vmfv / (1 + Cu.array() / pear::Kmfu);

  // DERIVATIVES
  Vec RudCu_diag(np);
  Vec RudCVdiag(np);
  Vec RvdCu_diag(np);
  Vec RvdCVdiag(np);

  RudCu_diag = (pear::Kmu * pear::Kmv * pear::Vmu) /
               ((Cu.array() + pear::Kmu).pow(2) * (Cv.array() + pear::Kmv));

  RudCVdiag = -(Cu.array() * pear::Kmv * pear::Vmu) /
              ((Cu.array() + pear::Kmu) * (Cv.array() + pear::Kmv).pow(2));

  RvdCu_diag = (pear::Kmv * pear::Vmu * pear::rq) /
                   ((Cu.array() + pear::Kmu) * (Cv.array() + pear::Kmv)) -
               (pear::Kmfu * pear::Vmfv) / (Cu.array() + pear::Kmfu).pow(2) -
               (Cu.array() * pear::Kmv * pear::Vmu * pear::rq) /
                   ((Cu.array() + pear::Kmu).pow(2) * (Cv.array() + pear::Kmv));

  RvdCVdiag = -(Cu.array() * pear::Kmv * pear::Vmu * pear::rq) /
              ((Cu.array() + pear::Kmu) * (Cv.array() + pear::Kmv).pow(2));

  RudCu = RudCu_diag.asDiagonal();
  RvdCu = RudCu_diag.asDiagonal();
  RudCv = RudCu_diag.asDiagonal();
  RvdCv = RudCu_diag.asDiagonal();
}

} // namespace pear


#ifndef __IRLS_H__
#define __IRLS_H__

#include <armadillo>

#include <glm/glm_info.hpp>
#include <glm/models/glm_model.hpp>

/**
 * Maximum number of iterations in the IRLS algorithm.
 */
static const int IRLS_MAX_ITERS = 25;

/**
 * Smallest change in likelihood before terminating the IRLS algorithm.
 */
static double IRLS_TOLERANCE = 10e-8;

/**
* Sets the weights of missing observations to zero, so that
* the will not influence the regression.
*
* @param missing Missing individuals are indicated by 1.
* @param w Vector of weights, missing entries will be replaced by 0.
*/
void set_missing_to_zero(const arma::uvec &missing, arma::vec &w);

/**
 * Compute the chisquare cdf for a vector of chi square variables.
 *
 * @param x Vector of chi square values.
 * @param df Degrees of freedom.
 *
 * @return Vector of corresponding p-values.
 */
arma::vec chi_square_cdf(const arma::vec &x, unsigned int df);

/**
 * Solves the weighted least square problem:
 *
 *   W*X*W*b = X*W*y
 *
 * The algorithm uses the singular value decomposition, to
 * compute the solution b:
 *
 *   b = V * S^-1 * U^t sqrt( w ) * y
 *
 * where wX = U S V^t
 *
 * @param X The design matrix.
 * @param y The right hand side.
 * @param w The weight for each observation.
 * @param fast_inversion If true use less robust but faster inversion.
 *
 * @return The vector b that minimizes the weighted least squares problem.
 */
arma::vec weighted_least_squares(const arma::mat &X, const arma::vec &y, const arma::vec &w, bool fast_inversion = false);

/**
 * Compute the adjusted dependent variates in the Iteratively reweighted
 * least squares algorithm.
 *
 * @param eta The linearized parameter.
 * @param mu The mean value parameter.
 * @param mu_eta The derivative of mu with respect to eta.
 * @param y The observations.
 *
 * @return The adjusted dependent variates.
 */
arma::vec compute_z(const arma::vec &eta, const arma::vec &mu, const arma::vec &mu_eta, const arma::vec &y);

/**
 * Compute the weight vector that is used in one iteration
 * in the Iteratively reweighted least squares algorithm.
 *
 * @param var Variance of each observation.
 * @param mu_eta The derivative of mu with respect to eta.
 *
 * @return A weight vector.
 */
arma::vec compute_w(const arma::vec &var, const arma::vec& mu_eta);

/**
 * This function performs the iteratively reweighted
 * least squares algorithm to estimate beta coefficients
 * of a genearlized linear model.
 *
 * @param X The design matrix (caller is responsible for
 *          adding an intercept).
 * @param y The observations.
 * @param model The GLM model to estimate.
 * @param output Output statistics of the estimated betas.
 * @param fast_inversion If true use less robust but faster inversion.
 *
 * @return Estimated beta coefficients.
 */
arma::vec irls(const arma::mat &X, const arma::vec &y, const glm_model &model, glm_info &output, bool fast_inversion = false);

/**
 * This function performs the iteratively reweighted
 * least squares algorithm to estimate beta coefficients
 * of a genearlized linear model.
 *
 * @param X The design matrix (caller is responsible for
 *          adding an intercept).
 * @param y The observations.
 * @param missing Identifies missing sampels by 1 and non-missing by 0.
 * @param model The GLM model to estimate.
 * @param output Output statistics of the estimated betas.
 * @param fast_inversion If true use less robust but faster inversion.
 *
 * @return Estimated beta coefficients.
 */
arma::vec irls(const arma::mat &X, const arma::vec &y, const arma::uvec &missing, const glm_model &model, glm_info &output, bool fast_inversion = false);

#endif /* End of __IRLS_H__ */


#ifndef MLT_MODELS_IMPLEMENTATIONS_AUTOENCODER_HPP
#define MLT_MODELS_IMPLEMENTATIONS_AUTOENCODER_HPP

#include <limits>
#include <tuple>

#include <Eigen/Core>

#include "../../defs.hpp"

namespace mlt {
namespace models {
namespace implementations {
namespace autoencoder {
	template <class HiddenActivation, class ReconstructionActivation>
	auto loss(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();

		return (((reconstruction_activation.compute(reconstruction_z)) - target).array().pow(2).sum() / (2 * input.cols())) +
			regularization * hidden_weights.array().pow(2).sum() +
			regularization * reconstruction_weights.array().pow(2).sum();
	}

	template <class HiddenActivation, class ReconstructionActivation>
	tuple<MatrixXd, VectorXd, MatrixXd, VectorXd>
	gradient(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();
		auto recontstruction_error = ((reconstruction_activation.compute(reconstruction_z) - target)).eval();
		auto reconstruction_delta = (recontstruction_error.cwiseProduct(reconstruction_activation.gradient(reconstruction_z))).eval();

		auto hidden_delta = ((reconstruction_weights.transpose() * reconstruction_delta).cwiseProduct(hidden_activation.gradient(hidden_z))).eval();

		return{ (hidden_delta * input.transpose() / input.cols()) + regularization * 2 * hidden_weights,
			hidden_delta.rowwise().sum() / input.cols(),
			(reconstruction_delta * hidden_a.transpose() / input.cols()) + regularization * 2 * reconstruction_weights,
			reconstruction_delta.rowwise().sum() / input.cols() };
	}

	template <class HiddenActivation, class ReconstructionActivation>
	tuple<double, MatrixXd, VectorXd, MatrixXd, VectorXd>
	loss_and_gradient(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();
		auto recontstruction_error = ((reconstruction_activation.compute(reconstruction_z) - target)).eval();
		auto reconstruction_delta = (recontstruction_error.cwiseProduct(reconstruction_activation.gradient(reconstruction_z))).eval();

		auto loss = (recontstruction_error.array().pow(2).sum() / (2 * input.cols())) +
			regularization * hidden_weights.array().pow(2).sum() +
			regularization * reconstruction_weights.array().pow(2).sum();

		auto hidden_delta = ((reconstruction_weights.transpose() * reconstruction_delta).cwiseProduct(hidden_activation.gradient(hidden_z))).eval();

		return{ loss,
			(hidden_delta * input.transpose() / input.cols()) + regularization * 2 * hidden_weights,
			hidden_delta.rowwise().sum() / input.cols(),
			(reconstruction_delta * hidden_a.transpose() / input.cols()) + regularization * 2 * reconstruction_weights,
			reconstruction_delta.rowwise().sum() / input.cols() };
	}

	template <class HiddenActivation, class ReconstructionActivation>
	auto sparse_loss(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, double sparsity, double sparsity_weight,
	MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();

		auto rho_hat = ((hidden_a.rowwise().sum() / input.cols()).unaryExpr([](double x) { return abs(x - 1.0) < numeric_limits<double>::epsilon() ? (x + numeric_limits<double>::epsilon()) : x; })).eval();
		auto sparsity_penalty = ((sparsity * (sparsity / rho_hat.array()).log()) + ((1 - sparsity) * ((1 - sparsity) / (1 - rho_hat.array())).log())).sum();

		return (((reconstruction_activation.compute(reconstruction_z)) - target).array().pow(2).sum() / (2 * input.cols())) +
			regularization * hidden_weights.array().pow(2).sum() +
			regularization * reconstruction_weights.array().pow(2).sum() +
			sparsity_weight * sparsity_penalty;
	}

	template <class HiddenActivation, class ReconstructionActivation>
	tuple<MatrixXd, VectorXd, MatrixXd, VectorXd>
	sparse_gradient(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, double sparsity, double sparsity_weight,
	MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();
		auto recontstruction_error = ((reconstruction_activation.compute(reconstruction_z) - target)).eval();
		auto reconstruction_delta = (recontstruction_error.cwiseProduct(reconstruction_activation.gradient(reconstruction_z))).eval();

		auto rho_hat = ((hidden_a.rowwise().sum() / input.cols()).unaryExpr([](double x) { return abs(x - 1.0) < numeric_limits<double>::epsilon() ? (x + numeric_limits<double>::epsilon()) : x; })).eval();
		auto sparsity_delta = ((-sparsity / rho_hat.array()) + ((1 - sparsity) / (1 - rho_hat.array()))).matrix().eval();
		auto hidden_delta = (((reconstruction_weights.transpose() * reconstruction_delta).colwise() +
			(sparsity_weight * sparsity_delta)).cwiseProduct(hidden_activation.gradient(hidden_z))).eval();

		return{ (hidden_delta * input.transpose() / input.cols()) + regularization * 2 * hidden_weights,
			hidden_delta.rowwise().sum() / input.cols(),
			(reconstruction_delta * hidden_a.transpose() / input.cols()) + regularization * 2 * reconstruction_weights,
			reconstruction_delta.rowwise().sum() / input.cols() };
	}

	template <class HiddenActivation, class ReconstructionActivation>
	tuple<double, MatrixXd, VectorXd, MatrixXd, VectorXd>
	sparse_loss_and_gradient(const HiddenActivation& hidden_activation, const ReconstructionActivation& reconstruction_activation,
	MatrixXdRef hidden_weights, VectorXdRef hidden_intercepts,
	MatrixXdRef reconstruction_weights, VectorXdRef reconstruction_intercepts,
	double regularization, double sparsity, double sparsity_weight,
	MatrixXdRef input, MatrixXdRef target) {
		auto hidden_z = ((hidden_weights * input).colwise() + hidden_intercepts).eval();
		auto hidden_a = (hidden_activation.compute(hidden_z)).eval();
		auto reconstruction_z = ((reconstruction_weights * hidden_a).colwise() + reconstruction_intercepts).eval();
		auto recontstruction_error = ((reconstruction_activation.compute(reconstruction_z) - target)).eval();
		auto reconstruction_delta = (recontstruction_error.cwiseProduct(reconstruction_activation.gradient(reconstruction_z))).eval();

		auto rho_hat = ((hidden_a.rowwise().sum() / input.cols()).unaryExpr([](double x) { return abs(x - 1.0) < numeric_limits<double>::epsilon() ? (x + numeric_limits<double>::epsilon()) : x; })).eval();
		auto sparsity_delta = ((-sparsity / rho_hat.array()) + ((1 - sparsity) / (1 - rho_hat.array()))).eval().matrix();
		auto hidden_delta = (((reconstruction_weights.transpose() * reconstruction_delta).colwise() +
			(sparsity_weight * sparsity_delta)).cwiseProduct(hidden_activation.gradient(hidden_z))).eval();

		auto loss = (recontstruction_error.array().pow(2).sum() / (2 * input.cols())) +
			regularization * hidden_weights.array().pow(2).sum() +
			regularization * reconstruction_weights.array().pow(2).sum() +
			sparsity_weight * ((sparsity * (sparsity / rho_hat.array()).log()) + ((1 - sparsity) * ((1 - sparsity) / (1 - rho_hat.array())).log())).sum();

		return{ loss,
			(hidden_delta * input.transpose() / input.cols()) + regularization * 2 * hidden_weights,
			hidden_delta.rowwise().sum() / input.cols(),
			(reconstruction_delta * hidden_a.transpose() / input.cols()) + regularization * 2 * reconstruction_weights,
			reconstruction_delta.rowwise().sum() / input.cols() };
	}
}
}
}
}
#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

// The computeRoots function included in this is based on materials
// covered by the following copyright and license:
//
// Geometric Tools, LLC
// Copyright (c) 1998-2010
// Distributed under the Boost Software License, Version 1.0.
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>

using namespace Eigen;
using namespace std;

template<typename Matrix, typename Roots>
inline void computeRoots(const Matrix& m, Roots& roots)
{
  typedef typename Matrix::Scalar Scalar;
  const Scalar s_inv3 = 1.0/3.0;
  const Scalar s_sqrt3 = internal::sqrt(Scalar(3.0));

  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
  // eigenvalues are the roots to this equation, all guaranteed to be
  // real-valued, because the matrix is symmetric.
  Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(0,1)*m(0,2)*m(1,2) - m(0,0)*m(1,2)*m(1,2) - m(1,1)*m(0,2)*m(0,2) - m(2,2)*m(0,1)*m(0,1);
  Scalar c1 = m(0,0)*m(1,1) - m(0,1)*m(0,1) + m(0,0)*m(2,2) - m(0,2)*m(0,2) + m(1,1)*m(2,2) - m(1,2)*m(1,2);
  Scalar c2 = m(0,0) + m(1,1) + m(2,2);

  // Construct the parameters used in classifying the roots of the equation
  // and in solving the equation for the roots in closed form.
  Scalar c2_over_3 = c2*s_inv3;
  Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
  if (a_over_3 > Scalar(0))
    a_over_3 = Scalar(0);

  Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));

  Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
  if (q > Scalar(0))
    q = Scalar(0);

  // Compute the eigenvalues by solving for the roots of the polynomial.
  Scalar rho = internal::sqrt(-a_over_3);
  Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3;
  Scalar cos_theta = internal::cos(theta);
  Scalar sin_theta = internal::sin(theta);
  roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
  roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
  roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);

  // Sort in increasing order.
  if (roots(0) >= roots(1))
    std::swap(roots(0),roots(1));
  if (roots(1) >= roots(2))
  {
    std::swap(roots(1),roots(2));
    if (roots(0) >= roots(1))
      std::swap(roots(0),roots(1));
  }
}

template<typename Matrix, typename Vector>
void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
{
  typedef typename Matrix::Scalar Scalar;
  // Scale the matrix so its entries are in [-1,1].  The scaling is applied
  // only when at least one matrix entry has magnitude larger than 1.

  Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
  scale = std::max(scale,Scalar(1));
  Matrix scaledMat = mat / scale;

  // Compute the eigenvalues
//   scaledMat.setZero();
  computeRoots(scaledMat,evals);

  // compute the eigen vectors
  // **here we assume 3 differents eigenvalues**

  // "optimized version" which appears to be slower with gcc!
//     Vector base;
//     Scalar alpha, beta;
//     base <<   scaledMat(1,0) * scaledMat(2,1),
//               scaledMat(1,0) * scaledMat(2,0),
//              -scaledMat(1,0) * scaledMat(1,0);
//     for(int k=0; k<2; ++k)
//     {
//       alpha = scaledMat(0,0) - evals(k);
//       beta  = scaledMat(1,1) - evals(k);
//       evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
//     }
//     evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();

//   // naive version
//   Matrix tmp;
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(0);
//   evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
//
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(1);
//   evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
//
//   tmp = scaledMat;
//   tmp.diagonal().array() -= evals(2);
//   evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();

  // a more stable version:
  if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
  {
    evecs.setIdentity();
  }
  else
  {
    Matrix tmp;
    tmp = scaledMat;
    tmp.diagonal ().array () -= evals (2);
    evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();

    tmp = scaledMat;
    tmp.diagonal ().array () -= evals (1);
    evecs.col(1) = tmp.row (0).cross(tmp.row (1));
    Scalar n1 = evecs.col(1).norm();
    if(n1<=Eigen::NumTraits<Scalar>::epsilon())
      evecs.col(1) = evecs.col(2).unitOrthogonal();
    else
      evecs.col(1) /= n1;

    // make sure that evecs[1] is orthogonal to evecs[2]
    evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
    evecs.col(0) = evecs.col(2).cross(evecs.col(1));
  }

  // Rescale back to the original size.
  evals *= scale;
}

int main()
{
  BenchTimer t;
  int tries = 10;
  int rep = 400000;
  typedef Matrix3f Mat;
  typedef Vector3f Vec;
  Mat A = Mat::Random(3,3);
  A = A.adjoint() * A;

  SelfAdjointEigenSolver<Mat> eig(A);
  BENCH(t, tries, rep, eig.compute(A));
  std::cout << "Eigen:  " << t.best() << "s\n";

  Mat evecs;
  Vec evals;
  BENCH(t, tries, rep, eigen33(A,evecs,evals));
  std::cout << "Direct: " << t.best() << "s\n\n";

  std::cerr << "Eigenvalue/eigenvector diffs:\n";
  std::cerr << (evals - eig.eigenvalues()).transpose() << "\n";
  for(int k=0;k<3;++k)
    if(evecs.col(k).dot(eig.eigenvectors().col(k))<0)
      evecs.col(k) = -evecs.col(k);
  std::cerr << evecs - eig.eigenvectors() << "\n\n";
}