/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_MATRIXALGO__H_
#define _PIMATH_MATRIXALGO__H_

#include <boost/python.hpp>
#include <ImathMatrixAlgo.h>
#include "util.h"

/*
 * extractSHRT,
 * extractScaling,
 * extractScalingAndShear:
 * Return the value(s) rather than a bool, or None where Imath would return False.
 *
 * removeScaling,
 * removeScalingAndShear,
 * sansScaling,
 * sansScalingAndShear:
 * Not implemented. Use 'm = withoutScaling(AndShear)(m)' instead.
 *
 * withoutScaling(AndShear) replaces sansScaling(AndShear), but behaviour is slightly different.
 * withoutScaling will return None, where removeScaling would have returned False. Otherwise,
 * it will return the same value as sansScaling.
 *
 * extractAndRemoveScalingAndShear:
 * Not implemented, use a combination of extractScalingAndShear and withoutScalingAndShear instead.
 *
 * extractSHRT:
 * The form of this function which takes args (M44, Vec3, Vec3, Euler, Vec3) is available
 * as the function 'extractEulerSHRT'.
 */

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct MatrixAlgoBind
	{
		typedef Imath::Vec2<T> 		vec2_type;
		typedef Imath::Vec3<T> 		vec3_type;
		typedef Imath::Quat<T> 		quat_type;
		typedef Imath::Matrix33<T> 	mat33_type;
		typedef Imath::Matrix44<T> 	mat44_type;
		typedef Imath::Euler<T> 	euler_type;

		MatrixAlgoBind()
		{
			bp::def("extractQuat", &Imath::extractQuat<T>);
			bp::def("rotationMatrix", &Imath::rotationMatrix<T>);
			bp::def("rotationMatrixWithUpDir", &Imath::rotationMatrixWithUpDir<T>);
			bp::def("alignZAxisWithTargetDir", &Imath::alignZAxisWithTargetDir<T>);

			bp::def("extractScaling", extractScaling<mat33_type>);
			bp::def("extractScaling", extractScaling_<mat33_type>);
			bp::def("extractScaling", extractScaling<mat44_type>);
			bp::def("extractScaling", extractScaling_<mat44_type>);

			bp::def("withoutScaling", removeScaling<mat33_type>);
			bp::def("withoutScaling", removeScaling_<mat33_type>);
			bp::def("withoutScaling", removeScaling<mat44_type>);
			bp::def("withoutScaling", removeScaling_<mat44_type>);

			bp::def("withoutScalingAndShear", removeScalingAndShear<mat33_type>);
			bp::def("withoutScalingAndShear", removeScalingAndShear_<mat33_type>);
			bp::def("withoutScalingAndShear", removeScalingAndShear<mat44_type>);
			bp::def("withoutScalingAndShear", removeScalingAndShear_<mat44_type>);

			bp::def("extractScalingAndShear", extractScalingAndShear33);
			bp::def("extractScalingAndShear", extractScalingAndShear33_);
			bp::def("extractScalingAndShear", extractScalingAndShear44);
			bp::def("extractScalingAndShear", extractScalingAndShear44_);

			bp::def("extractSHRT", extractSHRT33);
			bp::def("extractSHRT", extractSHRT33_);

			bp::def("extractSHRT", extractSHRT44);
			bp::def("extractSHRT", extractSHRT44_);
			bp::def("extractSHRT", extractSHRT44__);

			bp::def("extractEulerSHRT", extractEulerSHRT);
			bp::def("extractEulerSHRT", extractEulerSHRT_);

			bp::def("extractEuler", extractEuler);
			bp::def("extractEulerXYZ", extractEulerXYZ);
			bp::def("extractEulerZYX", extractEulerZYX);
		}

		static bp::object extractEulerSHRT(const mat44_type& mat, bool exc)
		{
			vec3_type s, h, t;
			euler_type r;
			return (Imath::extractSHRT(mat, s, h, r, t, exc))?
				bp::make_tuple(s,h,r,t) : bp::object();
		}

		static bp::object extractEulerSHRT_(const mat44_type& mat) {
			return extractEulerSHRT(mat, true);
		}

		static bp::object extractSHRT44(const mat44_type& mat,
			bool exc, typename euler_type::Order order)
		{
			vec3_type s, h, r, t;
			return (Imath::extractSHRT(mat, s, h, r, t, exc, order))?
				bp::make_tuple(s,h,r,t) : bp::object();
		}

		static bp::object extractSHRT44_(const mat44_type& mat, bool exc) {
			return extractSHRT44(mat, exc, euler_type::XYZ);
		}

		static bp::object extractSHRT44__(const mat44_type& mat) {
			return extractSHRT44(mat, true, euler_type::XYZ);
		}

		static bp::object extractSHRT33(const mat33_type& mat, bool exc)
		{
			vec2_type s, t;
			T h, r;
			return (Imath::extractSHRT(mat, s, h, r, t, exc))?
				bp::make_tuple(s,h,r,t) : bp::object();
		}

		static bp::object extractSHRT33_(const mat33_type& mat) {
			return extractSHRT33(mat, true);
		}

		static bp::object extractScalingAndShear33(const mat33_type& mat, bool exc)
		{
			vec2_type scl;
			T h;
			return (Imath::extractScalingAndShear(mat, scl, h, exc))?
				bp::make_tuple(scl, h) : bp::object();
		}

		static bp::object extractScalingAndShear33_(const mat33_type& mat) {
			return extractScalingAndShear33(mat, true);
		}

		static bp::object extractScalingAndShear44(const mat44_type& mat, bool exc)
		{
			vec3_type scl, shr;
			return (Imath::extractScalingAndShear(mat, scl, shr, exc))?
				bp::make_tuple(scl, shr) : bp::object();
		}

		static bp::object extractScalingAndShear44_(const mat44_type& mat) {
			return extractScalingAndShear44(mat, true);
		}

		template<typename Matrix>
		static bp::object removeScalingAndShear(const Matrix& mat, bool exc)
		{
			Matrix m(mat);
			return (Imath::removeScalingAndShear(m, exc))?
				bp::object(m) : bp::object();
		}

		template<typename Matrix>
		static bp::object removeScalingAndShear_(const Matrix& mat, bool exc) {
			return removeScalingAndShear(mat, true);
		}

		template<typename Matrix>
		static bp::object removeScaling(const Matrix& mat, bool exc)
		{
			Matrix m(mat);
			return (Imath::removeScaling(m, exc))?
				bp::object(m) : bp::object();
		}

		template<typename Matrix>
		static bp::object removeScaling_(const Matrix& mat, bool exc) {
			return removeScaling(mat, true);
		}

		template<typename Matrix>
		static bp::object extractScaling(const Matrix& mat, bool exc)
		{
			typedef imath_traits<Matrix> 								mat_traits;
			typedef typename make_vec<mat_traits::_columns-1, T>::type 	vec_less1_type;

			vec_less1_type v;
			return (Imath::extractScaling(mat, v, exc))?
				bp::object(v) : bp::object();
		}

		template<typename Matrix>
		static bp::object extractScaling_(const Matrix& mat) {
			return extractScaling(mat, true);
		}

		static T extractEuler(const mat33_type &mat)
		{
			T rot;
			Imath::extractEuler(mat, rot);
			return rot;
		}

		static vec3_type extractEulerXYZ(const mat44_type &mat)
		{
			vec3_type rot;
			Imath::extractEulerXYZ(mat, rot);
			return rot;
		}

		static vec3_type extractEulerZYX(const mat44_type &mat)
		{
			vec3_type rot;
			Imath::extractEulerZYX(mat, rot);
			return rot;
		}
	};

}

#endif


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */

/* GTestextractDigits.cpp - extracting digits.
 *   For a plaintext space modulo a prime-power $p^e$, extracting
 *   the base-$p$ representation of an encrypted values.
 */
#include <NTL/ZZ.h>
#include <helib/EncryptedArray.h>
#include <helib/polyEval.h>

#include "gtest/gtest.h"
#include "test_common.h"

#include <helib/debugging.h>

namespace {

struct Parameters
{
  Parameters(long p, long r, long m) : p(p), r(r), m(m)
  {
    if (p < 2)
      throw std::invalid_argument("p must be at least 2");
  };
  long p; // Plaintext base
  long r; // Lifting
  long m; // The cyclotomic ring

  // Let googletest know how to print the Parameters
  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "p=" << params.p << ","
              << "r=" << params.r << ","
              << "m=" << params.m << "}";
  };
};

class GTestExtractDigits : public ::testing::TestWithParam<Parameters>
{
protected:
  long p;
  long r;
  long m;
  long p2r;
  long L;

  helib::Context context;
  helib::SecKey secretKey;
  const helib::PubKey& publicKey;

  // lifting value rOld requires some manipulation before being used.
  // Utility function for calculating this.
  static long correctLifting(long rOld, long p)
  {
    long r = rOld;
    double lBound = 30.0;
    long bound = floor(lBound / log2((double)p));
    if (r < 2 || r > bound)
      r = bound;
    return r;
  };

  // Calculate how many levels we need
  static long calculateLevels(long r, long p)
  {
    long ll = NTL::NextPowerOfTwo(p);
    return 30 * (r * ll * 3 + 2);
  };

  GTestExtractDigits() :
      p(GetParam().p),
      r(correctLifting(GetParam().r, p)),
      m(GetParam().m
            ? GetParam().m
            : p + 1), // FindM(/*secparam=*/80, L, /*c=*/4, p, /*d=*/1, 0, m);
      p2r(NTL::power_long(p, r)),
      L(calculateLevels(r, p)),
      context(m, p, r),
      secretKey((helib::buildModChain(context, L, /*c=*/4), context)),
      publicKey(secretKey)
  {}

  virtual void SetUp() override
  {
    helib::setDryRun(helib_test::dry);
    if (!helib_test::noPrint) {
      if (helib_test::dry)
        std::cout << "dry run: ";
      std::cout << "m=" << m << ", p=" << p << ", r=" << r << ", L=" << L
                << std::endl;
    }

    secretKey.GenSecKey(); // A +-1/0 secret key
    helib::addSome1DMatrices(
        secretKey); // compute key-switching matrices that we need
    // On legacy test is debug, but used verbose for consistency with other
    // tests
    if (helib_test::verbose) {
      helib::dbgKey = &secretKey; // debugging key and ea
      helib::dbgEa = context.ea;
    }

#ifdef DEBUG_PRINTOUT
    helib::dbgKey = &secretKey;
    helib::dbgEa = context.ea;
#endif // DEBUG_PRINTOUT
  };

  virtual void TearDown() override { helib::cleanupGlobals(); }
};

TEST_P(GTestExtractDigits, correctlyExtractsDigits)
{
  helib::EncryptedArray ea(context);
  std::vector<long> v;
  std::vector<long> pDigits;
  ea.random(v); // random values in the slots

  const helib::PubKey& publicKey = secretKey;

  helib::Ctxt c(publicKey);
  ea.encrypt(c, publicKey, v);
  ea.decrypt(c, secretKey, pDigits);
  if (ea.size() <= 20 && !helib_test::noPrint)
    std::cout << "plaintext=" << helib::vecToStr(pDigits) << std::endl;

  if (!helib_test::noPrint)
    std::cout << "extracting " << r << " digits..." << std::flush;
  std::vector<helib::Ctxt> digits;
  helib::extractDigits(digits, c);
  if (!helib_test::noPrint)
    std::cout << " done\n" << std::flush;

  std::vector<long> tmp = v;
  long pp = p2r;
  for (long i = 0; i < (long)digits.size(); i++) {
    if (!digits[i].isCorrect()) {
      helib::CheckCtxt(digits[i], "");
      FAIL() << " potential decryption error for " << i << "th digit ";
    }
    ea.decrypt(digits[i], secretKey, pDigits);
    if (ea.size() <= 20 && !helib_test::noPrint)
      std::cout << i << "th digit=" << helib::vecToStr(pDigits) << std::endl;

    // extract the next digit from the plaintext, compare to pDigits
    for (long j = 0; j < (long)v.size(); j++) {
      long digit = tmp[j] % p;
      if (digit > p / 2)
        digit -= p;
      else if (digit < -p / 2)
        digit += p;

      EXPECT_EQ((pDigits[j] - digit) % pp, 0)
          << " error: v[" << j << "]=" << v[j] << " but " << i
          << "th digit comes " << pDigits[j] << " rather than " << digit
          << std::endl
          << std::endl;
      tmp[j] -= digit;
      tmp[j] /= p;
    }
    pp /= p;
  }
}

INSTANTIATE_TEST_SUITE_P(variousPlaintextBases,
                         GTestExtractDigits,
                         ::testing::Values(
                             // SLOW
                             Parameters(5, 0, 2047)
                             // FAST
                             // Parameters(5, 0, 91)
                             ));

} // anonymous namespace


#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "entity.hpp"

namespace cuauv {
namespace fishbowl {

entity::entity(double m, double r, const inertia_tensor& I, const Eigen::Quaterniond& btom_rq)
    : m(m)
    , r(r)
    , I(I)
    , btom_rq(btom_rq)
    , btom_rm(btom_rq.matrix())
    , mtob_rm(btom_rq.conjugate().matrix())
{
    if (m <= 0) throw std::invalid_argument("expected m > 0.");
    if (r <= 0) throw std::invalid_argument("expected r > 0.");

    Eigen::Vector3d diag = I.diagonal();
    if (diag[0] == 0 || diag[1] == 0 || diag[2] == 0) throw std::invalid_argument("expected I fully non-zero");

    Eigen::Vector3d& diagr = Ir.diagonal();
    for (int i = 0; i < 3; i++)
        diagr[i] = 1.0 / diag[i];
}

double entity::get_m() const { return m; }
double entity::get_r() const { return r; }

inertia_tensor entity::get_I() const { return I; }
inertia_tensor entity::get_Ir() const { return Ir; }

Eigen::Matrix3d entity::get_btom_rm() const { return btom_rm; }
Eigen::Matrix3d entity::get_mtob_rm() const { return mtob_rm; }
Eigen::Quaterniond entity::get_model_q() const { return Eigen::Quaterniond(q * btom_rq.conjugate()).normalized(); }

} // namespace fishbowl
} // namespace cuauv


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

// Implementation File
// Look at polynomial_basis.h for documentation
#include <traj_opt_pro/polynomial_basis.h>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/pointer_cast.hpp>
#include <boost/range/irange.hpp>

#include <iostream>
#include <stdexcept>
#include <vector>

namespace traj_opt {

Poly PolyCalculusPro::bernstein_polynomial(typename Poly::size_type n,
                                           typename Poly::size_type i) {
  std::vector<decimal_t> t_dat({0, 1});
  Poly t(t_dat.data(), 1);  // t
  std::vector<decimal_t> omt_dat({1, -1});
  Poly omt(omt_dat.data(), 1);  // 1-t

  Poly t_rasied_i(omt_dat.data(), 0);  // t to the ith power
  Poly omt_rasied_ni(t_rasied_i);      // (1-t) to the (n-i)th power

  for (uint c = 0; c < i; c++) t_rasied_i *= t;
  for (uint c = 0; c < n - i; c++) omt_rasied_ni *= omt;

  Poly result(t_rasied_i);
  result *= omt_rasied_ni;
  result *= boost::math::binomial_coefficient<decimal_t>(n, i);
  return result;
}
Poly PolyCalculusPro::chebyshev_polynomial(typename Poly::size_type n) {
  std::vector<decimal_t> omt_dat({1, -1});
  Poly omt(omt_dat.data(), 1);  //  1 - t
  decimal_t one = 1;
  decimal_t zero = 0;

  Poly omt_raised_k(&one, 0);  // (1-t) to the kth power
  if (n == 0) return omt_raised_k;

  Poly result(&zero, 0);

  for (uint k = 0; k <= n; k++) {
    decimal_t coeff = std::pow(-2.0, k) * decimal_t(n) / decimal_t(n + k);
    coeff *= boost::math::binomial_coefficient<decimal_t>(n + k, 2 * k);
    // std::cout << "sub chevy " << coeff*omt_raised_k << std::endl;
    result += coeff * omt_raised_k;
    omt_raised_k *= omt;
  }

  return result;
}
Poly PolyCalculusPro::shifted_legendre(typename Poly::size_type n) {
  // shifted legensdre polynomial of order n
  typename std::vector<decimal_t> v;
  for (typename Poly::size_type k = 0; k <= n; k++) {
    v.push_back(boost::math::binomial_coefficient<decimal_t>(n, k) *
                boost::math::binomial_coefficient<decimal_t>(n + k, k) *
                std::pow(-1.0, k + n));
  }
  Poly result(v.data(), n);
  return result;
}

uint Basis::dim() { return n_p; }

// // switched these constructors to use new generic one
// BasisBundle::BasisBundle(uint n_p_, uint k_r_)
//     : BasisBundle(LEGENDRE, n_p_, k_r_) {}
// BasisBundle::BasisBundle(int n) : BasisBundle(BEZIER, n, 0) {}

LegendreBasis::LegendreBasis(uint n_p_, uint k_r_) : StandardBasis(0) {
  orthogonal_ = true;
  n_p = n_p_;
  k_r = k_r_;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i < k_r; i++) {
    Poly poly(simple.data(), simple.size() - 1);
    polys.push_back(poly);
    simple.back() = 0.0;
    simple.push_back(1.0);
  }
  // does this line break ooqp? yes it does
  if (n_p == k_r) return;
  for (uint p = 0; p <= n_p - k_r; p++) {
    Poly p_cur = PolyCalculusPro::shifted_legendre(p);
    // std::cout << "shifted p " << p_cur << std::endl;
    for (uint i = 0; i < k_r; i++) {
      p_cur = PolyCalculus::integrate(p_cur);
    }
    polys.push_back(p_cur);
  }
}

BezierBasis::BezierBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;  // this was commented out, why?
  type_ = PolyType::BEZIER;
  for (int i = 0; i <= static_cast<int>(n_p); i++)
    polys.push_back(PolyCalculusPro::bernstein_polynomial(n_p, i));
}
ChebyshevBasis::ChebyshevBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;
  type_ = PolyType::CHEBYSHEV;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i <= n_p; i++) {
    polys.push_back(PolyCalculusPro::chebyshev_polynomial(i));
  }

  // std::cout << "Chebyshev "  << *this << std::endl;
}
EndPointBasis::EndPointBasis(uint n_p_) : StandardBasis(0) {
  n_p = n_p_;  // this was commented out, why?
  type_ = PolyType::ENDPOINT;
  traj_opt::MatD coeffs = MatD::Zero(n_p_ + 1, n_p + 1);
  for (int i = 0; i <= static_cast<int>(n_p); i++) {
    if (i % 2 == 0) {
      coeffs(i, i / 2) = boost::math::factorial<decimal_t>(i / 2);

    } else {
      for (int j = 0; j <= static_cast<int>(n_p - i / 2); j++) {
        if (i < 2)
          coeffs(i, j) = 1;
        else
          coeffs(i, j + i / 2) = coeffs(i - 2, j + i / 2) * (j + 1);
      }
    }
  }
  traj_opt::MatD coeffsi = coeffs.inverse();

  for (int i = 0; i <= static_cast<int>(n_p); i++) {
    std::vector<decimal_t> data;
    for (int j = 0; j <= static_cast<int>(n_p); j++) {
      //      data.push_back(coeffsi(i, j));
      if (std::abs(coeffsi(j, i)) > 1e-12) data.push_back(coeffsi(j, i));
      //            data.push_back(coeffsi(i, j));
      else
        data.push_back(0.0);
    }
    polys.push_back(Poly(data.data(), n_p));
  }
  //  std::cout << "Endpoint M " << coeffs << std::endl;
  //  std::cout << "Endpoint Minv " << coeffsi << std::endl;
  //  std::cout << "Basis " << *this << std::endl;

  //  std::cout << "Endpoint np " << n_p << " poly size " << polys.size()
  //  <<std::endl;
}

decimal_t LegendreBasis::innerproduct(uint i, uint j) const {
  if (i != j)
    return 0;
  else
    return StandardBasis::innerproduct(i, j);
}
Poly StandardBasis::getPoly(uint i) const { return polys.at(i); }
StandardBasis::StandardBasis(uint n) : Basis(n) {
  type_ = PolyType::STANDARD;
  if (n == 0) return;
  std::vector<decimal_t> simple;
  simple.push_back(1.0);
  for (uint i = 0; i <= n_p; i++) {
    Poly poly(simple.data(), simple.size() - 1);
    polys.push_back(poly);
    simple.back() = 0.0;
    simple.push_back(1.0);
  }
}
BasisTransformer::BasisTransformer(boost::shared_ptr<StandardBasis> from,
                                   int derr) {
  int n = from->dim();
  boost::shared_ptr<StandardBasis> to = boost::make_shared<StandardBasis>(n);
  *this = BasisTransformer(from, to, derr);
}
BasisTransformer::BasisTransformer(boost::shared_ptr<StandardBasis> from,
                                   boost::shared_ptr<StandardBasis> to,
                                   int derr)
    : to_(to), from_(from) {
  //  assert(to_->dim() == from_->dim());
  n = static_cast<int>(from_->dim()) + 1;
  A = MatD::Zero(n - derr, n);
  //  std::cout << "Rows " << A.rows() << " , " << A.cols() << std::endl;
  for (int i = 0; i < n - derr; i++) {
    Poly pi = from_->getPoly(i + derr);
    for (int j = 0; j < static_cast<int>(pi.size()); j++) {
      //      std::cout << "i,j " << i << " , " << j << std::endl;
      A(i, j + derr) = pi[j];
    }
  }
  B = MatD::Zero(n - derr, n - derr);
  for (int i = 0; i < n - derr; i++) {
    Poly qi = to_->getPoly(i);
    for (int j = 0; j < static_cast<int>(qi.size()); j++) {
      //      std::cout << "i,j " << i << " , " << j << std::endl;
      B(i, j) = qi[j];
    }
  }
  //  Ainv = A.inverse();
  Binv = B.inverse();
  // draw your communitive diagram to see where these come from
  //  basisbasis_ = Binv * A;
  basisbasis_ = (A * Binv).transpose();

  //   std::cout << "Debug A: " << A << std::endl;
  //   std::cout << "Debug B: " << B << std::endl;
  //   std::cout << "Debug Ainv: " << Ainv << std::endl;
  //   std::cout << "Debug Binv: " << Binv << std::endl;
  //   std::cout << "BB " << basisbasis_ << std::endl;
}
const MatD &BasisTransformer::getBasisBasisTransform() {
  //  std::cout << "BB " << basisbasis_ << std::endl;
  return basisbasis_;
}
const MatD &BasisTransformer::getLinearTransform(decimal_t a, decimal_t b) {
  decimal_t data[2];
  data[0] = b;
  data[1] = a;  // remember boost's convention is backward from matlabs
  Poly fac(data, 1);
  decimal_t one = 1;
  Poly base(&one, 0);
  MatD mat = MatD::Zero(n, n);
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < static_cast<int>(base.size()); j++) {
      mat(i, j) = base[j];
    }
    base = base * fac;
  }

  //  std::cout << "Debug mat: " << mat << std::endl;
  scaledtranform_ = Ainv * mat * A;  // more communitive diagrams
  return scaledtranform_;
}
boost::shared_ptr<Basis> BasisBundle::getBasis(int i) {
  if (i >= 0)
    return derrivatives.at(i);
  else
    return integrals.at(-i - 1);
}
BasisBundlePro::BasisBundlePro(PolyType type, uint n_p_, uint k_r_) {
  n_p = n_p_;
  k_r = k_r_;
  // only computer the first 4 derrivatives
  derrivatives.reserve(10);
  // computer the integral too, preferable use i7 to do computering
  integrals.reserve(1);

  for (auto i : boost::irange(0, 11)) {
    boost::shared_ptr<Basis> base;
    //    std::cout << "np " << n_p << std::endl;
    //    std::cout << "np2 " << n_p_ << std::endl;

    if (type == LEGENDRE)
      base = boost::make_shared<LegendreBasis>(n_p, k_r);
    else if (type == STANDARD)
      base = boost::make_shared<StandardBasis>(n_p);
    else if (type == BEZIER)
      base = boost::make_shared<BezierBasis>(n_p);
    else if (type == ENDPOINT)
      base = boost::make_shared<EndPointBasis>(n_p);
    else if (type == CHEBYSHEV)
      base = boost::make_shared<ChebyshevBasis>(n_p);
    else
      throw std::runtime_error("Unknown basis type");

    if (i == 11) {
      base->integrate();
      integrals.push_back(base);
    } else {
      for (int j = 0; j < i; j++) base->differentiate();
      derrivatives.push_back(base);
    }
  }
}
}  // namespace traj_opt


// 出題元
// 麻布中学校 2008年 入試問題 算数 問4 解法
//
// Boost C++ Libraries 1.63.0 (boost_1_63_0.zip) に含まれる
// libs/spirit/example/qi/calc_utree_ast.cpp を改変したものを用いています。
// Boost Software License は、以下から参照できます
// http://www.boost.org/LICENSE_1_0.txt
//
// 構文解析結果をASTとしてではなく、直接値として返す方法は、以下を参考にしました。
// http://www.kmonos.net/alang/boost/classes/spirit.html

#include <algorithm>
#include <iostream>
#include <memory>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/optional.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_function.hpp>

#ifdef USE_BIG_INTETER
using BigNumber = boost::multiprecision::uint512_t;  // 計算結果
#else
using BigNumber = size_t; // Rust互換
#endif

// unordered_mapに格納できるようにする
namespace std {
#ifdef USE_BIG_INTETER
    template <> struct hash<BigNumber> {
        std::size_t operator()(const BigNumber& key) const {
            using KeyType = size_t;
            static_assert(std::is_unsigned<KeyType>::value, "Expect unsigned key");
            return static_cast<KeyType>(key);
        }
    };
#endif
}

class NumberSet {
    using Expression = std::string;
    struct Result final {
        Result(const Expression& argExpr, const boost::optional<BigNumber>& argValue) :
            expr(argExpr), value(argValue) {}
        Expression expr;
        boost::optional<BigNumber> value;
    };
    using ExpressionSet = std::vector<Expression>;
    using ResultSet = std::vector<std::shared_ptr<Result>>;
    using ResultMap = std::unordered_map<BigNumber, std::vector<std::shared_ptr<Result>>>;

public:
    enum class FILTER {
        BY_EXPRESSION,
        BY_VALUE,
    };

    enum class PRINT_VALUE {
        NO_PRINT,
        PRINT,
    };

    NumberSet(const BigNumber& minNumber, const BigNumber& maxNumber,
              FILTER filter, PRINT_VALUE printValue) :
        filter_(filter), printValue_(printValue) {
        addExpressions(minNumber, maxNumber);
        std::sort(results_.begin(), results_.end(),
                  [&](auto& l, auto& r) { return (*(l->value) < *(r->value)); });
    }

    virtual ~NumberSet(void) = default;
    NumberSet(const NumberSet&) = delete;
    NumberSet& operator=(const NumberSet&) = delete;

    void PrintSums(std::ostream& os) {
        for(auto& result : results_) {
            os << *(result->value) << " = " << result->expr << "\n";
        }
    }

    void PrintMatchedSums(const NumberSet& other, std::ostream& os) {
        for(auto& result : results_) {
            if (other.resultMap_.empty()) {
                for(auto& otherResult : other.results_) {
                    if (*(result->value) == *(otherResult->value)) {
                        printValue(result->value, os);
                        os << result->expr << " == " << otherResult->expr << "\n";
                    }
                }
            } else {
                auto iResults = other.resultMap_.find(*(result->value));
                if (iResults != other.resultMap_.end()) {
                    printValue(result->value, os);
                    os << result->expr;
                    for(auto& otherResult : iResults->second) {
                        os << " == " << otherResult->expr;
                    }
                    os << "\n";
                }
            }
        }
    }

private:
    void addExpressions(const BigNumber& number, const BigNumber& maxNumber) {
        ExpressionSet exprSet;
        exprSet.push_back(boost::lexical_cast<std::string>(number));
        addExpressions(exprSet, number + 1, maxNumber);
        return;
    }

    void addExpressions(const ExpressionSet& exprSet, const BigNumber& number, const BigNumber& maxNumber) {
        if (number > maxNumber) {
            for(auto& expr : exprSet) {
                registerExpressions(expr);
            }
        } else {
            ExpressionSet exprSetPlus;
            for(auto& expr : exprSet) {
                exprSetPlus.push_back(expr + "*" + boost::lexical_cast<std::string>(number));
                addExpressions(exprSetPlus, number + 1, maxNumber);
            }

            ExpressionSet exprSetMul;
            for(auto& expr : exprSet) {
                exprSetMul.push_back(expr + "+" + boost::lexical_cast<std::string>(number));
                addExpressions(exprSetMul, number + 1, maxNumber);
            }
        }
        return;
    }

    void registerExpressions(const Expression& expr) {
        auto value = calculate(expr);
        if (value) {
            std::shared_ptr<Result> pResult = std::make_shared<Result>(expr, value);
            results_.push_back(pResult);

            if ((filter_ == FILTER::BY_VALUE) && value) {
                resultMap_[*value].push_back(pResult);
            }
        }
    }

    template <typename Iterator>
    struct calculator : boost::spirit::qi::grammar<Iterator, boost::spirit::ascii::space_type, BigNumber()> {
        boost::spirit::qi::rule<Iterator, boost::spirit::ascii::space_type, BigNumber()> expression, term, factor;
        calculator() : calculator::base_type(expression) {
            expression = term[boost::spirit::qi::_val = boost::spirit::qi::_1]
                >> *('+' >> term[boost::spirit::qi::_val += boost::spirit::qi::_1]);
            term = factor[boost::spirit::qi::_val = boost::spirit::qi::_1]
                >> *('*' >> factor[boost::spirit::qi::_val *= boost::spirit::qi::_1]);
            factor = boost::spirit::qi::uint_[boost::spirit::qi::_val = boost::spirit::qi::_1];
        }
    };

    boost::optional<BigNumber> calculate(const std::string& str) {
        BigNumber number = 0;
        boost::optional<BigNumber> result;

        calculator<std::string::const_iterator> calc;
        if (phrase_parse(str.begin(), str.end(), calc, boost::spirit::ascii::space, number)) {
            result = number;
        }

        return result;
    }

    void printValue(const boost::optional<BigNumber>& value, std::ostream& os) {
        if (printValue_ == NumberSet::PRINT_VALUE::PRINT) {
            os << *value << " : ";
        }
        return;
    }

    ResultSet results_;
    ResultMap resultMap_;
    FILTER filter_;
    PRINT_VALUE printValue_;
};

int main(int argc, char* argv[]) {
    NumberSet::PRINT_VALUE printValue = (argc < 2) ?
        NumberSet::PRINT_VALUE::NO_PRINT : NumberSet::PRINT_VALUE::PRINT;

    if (argc < 6) {
        // 問4-1
        NumberSet s(1,4, NumberSet::FILTER::BY_EXPRESSION, printValue);
        s.PrintSums(std::cout);

        // 問4-2
        NumberSet l(1,5, NumberSet::FILTER::BY_EXPRESSION, printValue);
        NumberSet r(2,6, NumberSet::FILTER::BY_EXPRESSION, printValue);
        l.PrintMatchedSums(r, std::cout);
    } else {
        std::string mode(argv[1]);
        // 引数mapをつけると、連想配列を使って解く
        // 引数slowをつけると総当たりで解く、実はmap以外の文字列なら何でもよい
        NumberSet::FILTER filter = (mode == "map") ?
            NumberSet::FILTER::BY_VALUE : NumberSet::FILTER::BY_EXPRESSION;

        unsigned int minLeft  = boost::lexical_cast<decltype(minLeft)>(argv[2]);
        unsigned int maxLeft  = boost::lexical_cast<decltype(maxLeft)>(argv[3]);
        unsigned int minRight = boost::lexical_cast<decltype(minRight)>(argv[4]);
        unsigned int maxRight = boost::lexical_cast<decltype(maxRight)>(argv[5]);
        NumberSet l(minLeft,  maxLeft,  filter, printValue);
        NumberSet r(minRight, maxRight, filter, printValue);
        l.PrintMatchedSums(r, std::cout);
    }

    return 0;
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


#pragma once
#include "functions.hpp"
#include <armadillo>
#include <algorithm>
#include <random>
#include <cassert>
#include <vector>

template <typename activation = Logistic, typename error = Squared_Error>
struct FeedForward_Network {
  FeedForward_Network(std::vector<int> layer_sizes_in) : layer_sizes(layer_sizes_in) {

    //For the connections inbetween layers
    for (int i = 0; i < layer_sizes.size()-1; ++i) {
      weights.push_back(arma::Mat<float>(layer_sizes[i], layer_sizes[i+1], arma::fill::zeros));
      last_weights.push_back(arma::Mat<float>(layer_sizes[i], layer_sizes[i+1], arma::fill::zeros));
    }
  }

  inline void resize_activation(int num_trials) {
    activations.clear();
    for (int i = 0; i < layer_sizes.size(); ++i) {
      activations.push_back(arma::Mat<float>(num_trials, layer_sizes[i], arma::fill::zeros));
    }

    //No deltas for input layer
    deltas.clear();
    for (int i = 0; i < layer_sizes.size() - 1; ++i) {
      deltas.push_back(arma::Mat<float>(num_trials, layer_sizes[i+1]));
    }
  }

  //configuration of layers
  std::vector<int> layer_sizes;

  std::vector<arma::Mat<float>> weights;
  //used in backprop with momentum
  std::vector<arma::Mat<float>> last_weights;
  std::vector<arma::Mat<float>> activations;
  //used in backprop
  std::vector<arma::Mat<float>> deltas;
};


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <set>
#include <string>
#include <vector>

#include <boost/variant.hpp>

namespace pnmc { namespace properties {

/*------------------------------------------------------------------------------------------------*/

// Integer formulae :: Arithmetic operators
struct integer_constant;
struct integer_sum;
struct integer_product;
struct integer_difference;
struct integer_division;

// Integer formulae :: Petri net operators
struct place_bound
{
  std::vector<std::string> places;
};

struct tokens_count
{
  std::vector<std::string> places;
};

using integer_expression = boost::variant< integer_constant
                                         , boost::recursive_wrapper<integer_sum>
                                         , boost::recursive_wrapper<integer_product>
                                         , boost::recursive_wrapper<integer_difference>
                                         , boost::recursive_wrapper<integer_division>
                                         , place_bound
                                         , tokens_count>;

// Integer formulae :: Arithmetic operators
struct integer_constant
{
  int value;
};

struct integer_sum
{
  std::vector<integer_expression> expressions;
};

struct integer_product
{
  std::vector<integer_expression> expressions;
};

struct integer_difference
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_division
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

// Boolean formulae :: Reachability operators
struct invariant;
struct impossibility;
struct possibility;

// Boolean formulae :: Petri net operators
struct deadlock {};
struct is_fireable
{
  std::vector<std::string> transitions;
};

// Boolean formulae :: Boolean operators
struct true_ {};
struct false_ {};
struct negation;
struct conjunction;
struct disjunction;
struct exclusive_disjonction;
struct implication;
struct equivalence;

// Boolean formulae :: Comparison operators
struct integer_eq;
struct integer_ne;
struct integer_lt;
struct integer_le;
struct integer_gt;
struct integer_ge;

using boolean_expression = boost::variant< boost::recursive_wrapper<invariant>
                                         , boost::recursive_wrapper<impossibility>
                                         , boost::recursive_wrapper<possibility>
                                         , deadlock
                                         , is_fireable
                                         , true_
                                         , false_
                                         , boost::recursive_wrapper<negation>
                                         , boost::recursive_wrapper<conjunction>
                                         , boost::recursive_wrapper<disjunction>
                                         , boost::recursive_wrapper<exclusive_disjonction>
                                         , boost::recursive_wrapper<implication>
                                         , boost::recursive_wrapper<equivalence>
                                         , boost::recursive_wrapper<integer_eq>
                                         , boost::recursive_wrapper<integer_ne>
                                         , boost::recursive_wrapper<integer_lt>
                                         , boost::recursive_wrapper<integer_le>
                                         , boost::recursive_wrapper<integer_gt>
                                         , boost::recursive_wrapper<integer_ge>>;

//// Boolean formulae :: Reachability operators
struct invariant
{
  boolean_expression expression;
};

struct impossibility
{
  boolean_expression expression;
};

struct possibility
{
  boolean_expression expression;
};

// Boolean formulae :: Boolean operators
struct negation
{
  boolean_expression expression;
};

struct conjunction
{
  std::vector<boolean_expression> expressions;
};

struct disjunction
{
  std::vector<boolean_expression> expressions;
};

struct exclusive_disjonction
{
  std::vector<boolean_expression> expressions;
};

struct implication
{
  boolean_expression lhs_expression;
  boolean_expression rhs_expression;
};

struct equivalence
{
  boolean_expression lhs_expression;
  boolean_expression rhs_expression;
};

// Boolean formulae :: Comparison operators
struct integer_eq
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_ne
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_lt
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_le
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_gt
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

struct integer_ge
{
  integer_expression lhs_expression;
  integer_expression rhs_expression;
};

/*------------------------------------------------------------------------------------------------*/

using formula = boost::variant<boolean_expression, integer_expression>;

/*------------------------------------------------------------------------------------------------*/

struct formulae
{
  bool compute_deadlock;
  std::set<std::string> places_bounds;
  std::set<std::string> fireable_transitions;

  struct boolean
  {
    std::string id;
    std::set<std::string> targets;
    boolean_expression expression;
  };

  struct integer
  {
    std::string id;
    std::set<std::string> targets;
    integer_expression expression;
  };

  std::vector<boolean> booleans;
  std::vector<integer> integers;
};

/*------------------------------------------------------------------------------------------------*/

}} // namespace pnmc::properties


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_RSQRT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing rsqrt capabilities

    Returns the inverse of the square root of the input.

    @par semantic:

    For any given value @c x of floating type @c T:

    @code
    T r = rsqrt(x);
    @endcode

    For signed type is similar to:

    @code
    T r = T(1)/sqrt(x)
    @endcode

    @par Decorators

    - raw_ for floating entries: if full accuracy is not needed a sometimes rawer less accurate version of the function
    can be obtained using the the raw_ decorator : raw_(rsqrt)(x).

    @par Decorators

  **/
  Value rsqrt(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/rsqrt.hpp>
#include <boost/simd/function/scalar/rsqrt.hpp>
#include <boost/simd/function/simd/rsqrt.hpp>

#endif


#include <Eigen/Dense>
#include <fmt/core.h>
#include <fmt/ranges.h>

#include <algorithm>
#include <array>
#include <fstream>
#include <iostream>
#include <numeric>
#include <optional>
#include <sstream>
#include <stack>
#include <string>
#include <vector>

using Mat = Eigen::MatrixXi;

auto makeMat(std::vector<std::string> const &lines) {
  Mat m(10, 10);
  m.setZero();

  for (auto i = 0; i < lines.size(); ++i) {
    auto row = lines[i];
    for (auto j = 0; j < row.size(); ++j) {
      std::string val;
      val += row[j];
      m(i, j) = std::stoi(val);
    }
  }

  return m;
}

auto parseFile(char const *file_name) {
  auto file = std::ifstream(file_name);
  std::string line;
  std::vector<std::string> out;
  while (std::getline(file, line)) {
    out.push_back(line);
  }

  return makeMat(out);
}

std::size_t absorb(Mat &m, int i, int j) {
  if (i < 0 || i >= m.rows() || j < 0 || j >= m.cols() || m(i, j) == 0) {
    return 0; // Don't absorb if we don't exist or already flashed
  }

  m(i, j) += 1;
  std::size_t out = 0;
  if (m(i, j) > 9) { // Flash
    m(i, j) = 0;
    out += 1; // Count our flash
    out += absorb(m, i + 1, j);
    out += absorb(m, i - 1, j);
    out += absorb(m, i, j + 1);
    out += absorb(m, i, j - 1);
    out += absorb(m, i + 1, j + 1);
    out += absorb(m, i - 1, j + 1);
    out += absorb(m, i - 1, j - 1);
    out += absorb(m, i + 1, j - 1);
  }

  return out;
}

std::size_t step(Mat &m) {
  // Increase energy levels by one
  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      m(r, c) += 1;
    }
  }

  std::size_t out = 0;
  for (auto r = 0; r < m.rows(); ++r) {
    for (auto c = 0; c < m.cols(); ++c) {
      if (m(r, c) > 9) {
        m(r, c) = 0;
        out += 1;
        out += absorb(m, r + 1, c);
        out += absorb(m, r - 1, c);
        out += absorb(m, r, c + 1);
        out += absorb(m, r, c - 1);
        out += absorb(m, r + 1, c + 1);
        out += absorb(m, r - 1, c + 1);
        out += absorb(m, r - 1, c - 1);
        out += absorb(m, r + 1, c - 1);
      }
    }
  }

  return out;
}

int main(int _, char **argv) {
  auto M = parseFile(argv[1]);
  std::cout << "M:\n" << M << "\n\n";

  auto Mp1 = M;
  std::size_t sum = 0;
  for(auto i = 0; i < 100; ++i){
    sum += step(Mp1);
  }
  fmt::print("Part1 score: {}\n", sum);

  auto day = 0;
  while(M.sum() != 0){
    ++day;
    step(M);
  }
  fmt::print("Part2 day: {}\n", day);
}


#include <ndt_map/ndt_cell.h>
#include <ndt_map/lazy_grid.h>
#include <ndt_map/pointcloud_utils.h>
#include <ndt_registration/ndt_matcher_d2d_sc.h>
#include <ndt_generic/eigen_utils.h>

#include <Eigen/Eigen>
#include <fstream>
#include <omp.h>
#include <sys/time.h>

namespace perception_oru
{

// inline void convertAffine3dToMatrix61(const Eigen::Affine3d &T, Eigen::Matrix<double,6,1> &v) {
//     Eigen::Vector3d transl = T.translation();
//     v(0) = transl(0);
//     v(1) = transl(1);
//     v(2) = transl(2);

//     Eigen::Vector3d rot = T.rotation().eulerAngles(0,1,2);
//     v(3) = rot(0);
//     v(4) = rot(1);
//     v(5) = rot(2);

// }

Eigen::MatrixXd computeHessianMahalanobis(const Eigen::MatrixXd &Q) {
  // Compute the Hessian for the mahalanobis distance Q = the inverse of the covariance matrix(!)
  Eigen::MatrixXd H(6,6);
  H << Q(0,0)+Q(0,0), Q(1,0)+Q(0,1), Q(2,0)+Q(0,2), Q(3,0)+Q(0,3), Q(4,0)+Q(0,4), Q(5,0)+Q(0,5),
       Q(0,1)+Q(1,0), Q(1,1)+Q(1,1), Q(2,1)+Q(1,2), Q(3,1)+Q(1,3), Q(4,1)+Q(1,4), Q(5,1)+Q(1,5),
       Q(0,2)+Q(2,0), Q(1,2)+Q(2,1), Q(2,2)+Q(2,2), Q(3,2)+Q(2,3), Q(4,2)+Q(2,4), Q(5,2)+Q(2,5),
       Q(0,3)+Q(3,0), Q(1,3)+Q(3,1), Q(2,3)+Q(3,2), Q(3,3)+Q(3,3), Q(4,3)+Q(3,4), Q(5,3)+Q(3,5),
       Q(0,4)+Q(4,0), Q(1,4)+Q(4,1), Q(2,4)+Q(4,2), Q(3,4)+Q(4,3), Q(4,4)+Q(4,4), Q(5,4)+Q(4,5),
       Q(0,5)+Q(5,0), Q(1,5)+Q(5,1), Q(2,5)+Q(5,2), Q(3,5)+Q(5,3), Q(4,5)+Q(5,4), Q(5,5)+Q(5,5);

  // A 3DOF hessian...
  // H <<
  //   Q(0,0)+Q(0,0), Q(1,0)+Q(0,1), 0., 0., 0., Q(5,0)+Q(0,5),
  //   Q(0,1)+Q(1,0), Q(1,1)+Q(1,1), 0., 0., 0., Q(5,1)+Q(1,5),
  //   0., 0., 0., 0., 0., 0.,
  //   0., 0., 0., 0., 0., 0.,
  //   0., 0., 0., 0., 0., 0.,
  //   Q(0,5)+Q(5,0), Q(1,5)+Q(5,1), 0., 0., 0., Q(5,5)+Q(5,5);

  //  H.setZero();

  return H;
}

double computeScoreMahalanobis(const Eigen::Matrix<double,6,1> &x,
                               const Eigen::MatrixXd &Q) {
  return x.transpose()*Q*x;
  //  return 0.;
}

Eigen::Matrix<double,6,1> computeGradientMahalanobis(const Eigen::Matrix<double,6,1> &x,
                                                     const Eigen::MatrixXd &Q) {
  return computeHessianMahalanobis(Q)*x;
}

double NDTMatcherD2DSC::lineSearchMTSC(Eigen::Matrix<double,6,1> &increment,
                                       std::vector<NDTCell*> &sourceNDT,
                                       NDTMap &targetNDT,
                                       const Eigen::Matrix<double,6,1> &localpose,
                                       const Eigen::MatrixXd &Q,
                                       const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &T)
{
    // default params
    double stp = 1.0; //default step
    double recoverystep = 0.1;
    double dginit = 0.0;
    double ftol = 0.11111; //epsilon 1
    double gtol = 0.99999; //epsilon 2
    double stpmax = 4.0;
    double stpmin = 0.001;
    int maxfev = 40; //max function evaluations
    double xtol = 0.01; //window of uncertainty around the optimal step

    //my temporary variables
    std::vector<NDTCell*> sourceNDTHere;
    double score_init = 0.0;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> ps;
    ps.setIdentity();

    Eigen::Matrix<double,6,1> scg_here;
    Eigen::MatrixXd pincr(6,1), score_gradient_here(6,1);
    Eigen::MatrixXd pseudoH(6,6);
    Eigen::Vector3d eulerAngles;
    Eigen::Matrix<double,6,1> X = localpose;

    /////

    int info = 0;			// return code
    int infoc = 1;		// return code for subroutine cstep

    // Compute the initial gradient in the search direction and check
    // that s is a descent direction.
    score_gradient_here.setZero();
    score_init = derivativesNDT(sourceNDT,targetNDT,score_gradient_here,pseudoH,false);
    score_init += computeScoreMahalanobis(X,Q);
    score_gradient_here += computeGradientMahalanobis(X,Q);

    scg_here = score_gradient_here;
    dginit = increment.dot(scg_here);

    if (dginit >= 0.0)
    {
        std::cout << "MoreThuente::cvsrch - wrong direction (dginit = " << dginit << ")" << std::endl;
        //return recoverystep; //TODO TSV -1; //
        //return -1;

        increment = -increment;
        dginit = -dginit;

        if (dginit >= 0.0)
        {
            for(unsigned int i=0; i<sourceNDTHere.size(); i++)
            {
                if(sourceNDTHere[i]!=NULL)
                    delete sourceNDTHere[i];
            }
            return recoverystep;
        }
    }
    else
    {

    }

    // Initialize local variables.

    bool brackt = false;		// has the soln been bracketed?
    bool stage1 = true;		// are we in stage 1?
    int nfev = 0;			// number of function evaluations
    double dgtest = ftol * dginit; // f for curvature condition
    double width = stpmax - stpmin; // interval width
    double width1 = 2 * width;	// ???

    // initial function value
    double finit = 0.0;
    finit = score_init;

    // The variables stx, fx, dgx contain the values of the step,
    // function, and directional derivative at the best step.  The
    // variables sty, fy, dgy contain the value of the step, function,
    // and derivative at the other endpoint of the interval of
    // uncertainty.  The variables stp, f, dg contain the values of the
    // step, function, and derivative at the current step.

    double stx = 0.0;
    double fx = finit;
    double dgx = dginit;
    double sty = 0.0;
    double fy = finit;
    double dgy = dginit;

    // Get the linear solve tolerance for adjustable forcing term
    //double eta_original = -1.0;
    //double eta = 0.0;
    //eta = eta_original;

    // Start of iteration.

    double stmin, stmax;
    double fm, fxm, fym, dgm, dgxm, dgym;

    while (1)
    {
        // Set the minimum and maximum steps to correspond to the present
        // interval of uncertainty.
        if (brackt)
        {
            stmin = MoreThuente::min(stx, sty);
            stmax = MoreThuente::max(stx, sty);
        }
        else
        {
            stmin = stx;
            stmax = stp + 4 * (stp - stx);
        }

        // Force the step to be within the bounds stpmax and stpmin.
        stp = MoreThuente::max(stp, stpmin);
        stp = MoreThuente::min(stp, stpmax);

        // If an unusual termination is to occur then let stp be the
        // lowest point obtained so far.

        if ((brackt && ((stp <= stmin) || (stp >= stmax))) ||
                (nfev >= maxfev - 1) || (infoc == 0) ||
                (brackt && (stmax - stmin <= xtol * stmax)))
        {
            stp = stx;
        }

        // Evaluate the function and gradient at stp
        // and compute the directional derivative.
        ///////////////////////////////////////////////////////////////////////////

        pincr = stp*increment;

        ps = Eigen::Translation<double,3>(pincr(0),pincr(1),pincr(2))*
             Eigen::AngleAxisd(pincr(3),Eigen::Vector3d::UnitX())*
             Eigen::AngleAxisd(pincr(4),Eigen::Vector3d::UnitY())*
             Eigen::AngleAxisd(pincr(5),Eigen::Vector3d::UnitZ());

        for(unsigned int i=0; i<sourceNDTHere.size(); i++)
        {
            if(sourceNDTHere[i]!=NULL)
                delete sourceNDTHere[i];
        }
        sourceNDTHere.clear();
        for(unsigned int i=0; i<sourceNDT.size(); i++)
        {
            NDTCell *cell = sourceNDT[i];
            if(cell!=NULL)
            {
                Eigen::Vector3d mean = cell->getMean();
                Eigen::Matrix3d cov = cell->getCov();
                mean = ps*mean;
                cov = ps.rotation()*cov*ps.rotation().transpose();
                NDTCell* nd = (NDTCell*)cell->copy();
                nd->setMean(mean);
                nd->setCov(cov);
                sourceNDTHere.push_back(nd);
            }
        }

        Eigen::Affine3d T_previous = T;
        Eigen::Affine3d T_current = ps*T; // Note that ps is not an incremental part, it is redefined in each loop from frame T.

        Eigen::Vector3d Tposition_diff = T_current.translation() - T_previous.translation();

        //        std::cerr << "Tposition_diff : " << Tposition_diff.transpose() << std::endl;
        //        std::cerr << "pincr : " << pincr.transpose() << std::endl;

        double f = 0.0;
        score_gradient_here.setZero();

        f = derivativesNDT(sourceNDTHere,targetNDT,score_gradient_here,pseudoH,false);
        // X is defined in global coords...
        X(0) = localpose(0) + Tposition_diff(0);
        X(1) = localpose(1) + Tposition_diff(1);
        X(2) = localpose(2) + Tposition_diff(2);
        X(3) = localpose(3) + pincr(3);
        X(4) = localpose(3) + pincr(4);
        X(5) = localpose(3) + pincr(5);

        f += computeScoreMahalanobis(X,Q);
        score_gradient_here += computeGradientMahalanobis(X,Q);

        double dg = 0.0;
        scg_here = score_gradient_here;
        dg = increment.dot(scg_here);

        nfev ++;

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

        // Armijo-Goldstein sufficient decrease
        double ftest1 = finit + stp * dgtest;

        // Test for convergence.

        if ((brackt && ((stp <= stmin) || (stp >= stmax))) || (infoc == 0))
            info = 6;			// Rounding errors

        if ((stp == stpmax) && (f <= ftest1) && (dg <= dgtest))
            info = 5;			// stp=stpmax

        if ((stp == stpmin) && ((f > ftest1) || (dg >= dgtest)))
            info = 4;			// stp=stpmin

        if (nfev >= maxfev)
            info = 3;			// max'd out on fevals

        if (brackt && (stmax-stmin <= xtol*stmax))
            info = 2;			// bracketed soln

        // RPP sufficient decrease test can be different
        bool sufficientDecreaseTest = false;
        sufficientDecreaseTest = (f <= ftest1);  // Armijo-Golstein

        if ((sufficientDecreaseTest) && (fabs(dg) <= gtol*(-dginit)))
            info = 1;			// Success!!!!

        if (info != 0) 		// Line search is done
        {
            if (info != 1) 		// Line search failed
            {
                stp = recoverystep;
            }
            else 			// Line search succeeded
            {

            }
            // Returning the line search flag
            for(unsigned int i=0; i<sourceNDTHere.size(); i++)
            {
                if(sourceNDTHere[i]!=NULL)
                    delete sourceNDTHere[i];
            }
            return stp;

        } // info != 0

        // In the first stage we seek a step for which the modified
        // function has a nonpositive value and nonnegative derivative.

        if (stage1 && (f <= ftest1) && (dg >= MoreThuente::min(ftol, gtol) * dginit))
        {
            stage1 = false;
        }

        // A modified function is used to predict the step only if we have
        // not obtained a step for which the modified function has a
        // nonpositive function value and nonnegative derivative, and if a
        // lower function value has been obtained but the decrease is not
        // sufficient.

        if (stage1 && (f <= fx) && (f > ftest1))
        {

            // Define the modified function and derivative values.

            fm = f - stp * dgtest;
            fxm = fx - stx * dgtest;
            fym = fy - sty * dgtest;
            dgm = dg - dgtest;
            dgxm = dgx - dgtest;
            dgym = dgy - dgtest;

            // Call cstep to update the interval of uncertainty
            // and to compute the new step.

            infoc = MoreThuente::cstep(stx,fxm,dgxm,sty,fym,dgym,stp,fm,dgm,
                                       brackt,stmin,stmax);

            // Reset the function and gradient values for f.

            fx = fxm + stx*dgtest;
            fy = fym + sty*dgtest;
            dgx = dgxm + dgtest;
            dgy = dgym + dgtest;

        }

        else
        {

            // Call cstep to update the interval of uncertainty
            // and to compute the new step.

            infoc = MoreThuente::cstep(stx,fx,dgx,sty,fy,dgy,stp,f,dg,
                                       brackt,stmin,stmax);

        }

        // Force a sufficient decrease in the size of the
        // interval of uncertainty.

        if (brackt)
        {
            if (fabs(sty - stx) >= 0.66 * width1)
                stp = stx + 0.5 * (sty - stx);
            width1 = width;
            width = fabs(sty-stx);
        }

    } // while-loop
}

void NDTMatcherD2DSC::scoreComparision( NDTMap& targetNDT,
                                        NDTMap& sourceNDT,
                                        const Eigen::Affine3d &T,
                                        const Eigen::MatrixXd& Tcov,
                                        double &score_NDT,
                                        double &score_NDT_SC,
                                        const Eigen::Affine3d &offset,
                                        const Eigen::Affine3d &odom_offset,
                                        double alpha)
{
    Eigen::MatrixXd Hessian(6,6), score_gradient(6,1);
    std::vector<NDTCell*> nextNDT = sourceNDT.pseudoTransformNDT(T*offset);
    score_NDT = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,false);
    Eigen::VectorXd X = ndt_generic::affine3dToVector(odom_offset);
    ndt_generic::normalizeEulerAngles6dVec(X);
    Eigen::MatrixXd Q = Tcov.inverse();

    score_NDT_SC = score_NDT + alpha*computeScoreMahalanobis(X,Q);

    for(unsigned int i=0; i<nextNDT.size(); i++)
    {
        if(nextNDT[i]!=NULL)
            delete nextNDT[i];
    }
}

bool NDTMatcherD2DSC::match( NDTMap& targetNDT,
        NDTMap& sourceNDT,
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T ,
        const Eigen::MatrixXd& Tcov)
{

    //locals
    bool convergence = false;
    //double score=0;
    double score_best = INT_MAX;
    //double DELTA_SCORE = 0.0005;
    //double NORM_MAX = current_resolution, ROT_MAX = M_PI/10; //
    int itr_ctr = 0;
    //double alpha = 0.95;
    double step_size = 1;
    Eigen::Matrix<double,6,1>  pose_increment_v, scg, pose_local_v, pose_increment_v_Tframe;
    Eigen::MatrixXd Hessian(6,6), score_gradient(6,1); //column vectors, pose_increment_v(6,1)
    Eigen::MatrixXd Q = Tcov.inverse();

    //    std::cout << "Q1 : " << Q << std::endl;
    if (only_xy_motion) {
      Eigen::MatrixXd Q_tmp = Q;
      Q.setZero();
      Q.block(0,0,2,2) = Q_tmp.block(0,0,2,2);
    }
    if (lock_zrp_motion) {
      Q(2,2) = 0; // !0
      Q(3,3) = 1000000000;
      Q(4,4) = 1000000000;
    }
    //    std::cout << "Q2 : " << Q << std::endl;

    Eigen::Matrix<double,6,1> X;
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> TR, Tbest;
    Eigen::Vector3d transformed_vec, mean;
    bool ret = true;

    Eigen::Affine3d Tinit = T;

    Tbest = T;
    pose_local_v.setZero();

    Eigen::Array<double,6,1> weights;
    std::vector<NDTCell*> nextNDT = sourceNDT.pseudoTransformNDT(T);

    int iter = 0;
    int best_iter;
    this->nb_match_calls++;
    bool found_improvement = false;

    double alpha = 0.0; //

    while(!convergence)
    {
      iter++;
        TR.setIdentity();
        Hessian.setZero();
        score_gradient.setZero();

        double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,true);
        X = pose_local_v;

        if (lock_zrp_motion) {
          // Assign the X to be the offset between global z and global roll, pitch.
          X(2) = T.translation()[2];
          // No Euler angles, simply compute the roll and pitch as separate angles.
          double pitch = asin(T.rotation()(2,0)); // Z offset along the x axis...
          double roll = asin(T.rotation()(2,1));  // Z offset along the y axis...
          X(3) = roll;  //T.rotation().eulerAngles(0,1,2)[0];
          X(4) = pitch; //T.rotation().eulerAngles(0,1,2)[1];
          std::cout << "[" << X.transpose() << "]";
        }

        //        std::cout << "score_here : " << score_here << std::flush;
        score_here += alpha*computeScoreMahalanobis(X,Q);
        //        std::cout << " <-> score_here2 : " << computeScoreMahalanobis(X,Q) << std::endl;
        score_gradient += alpha*computeGradientMahalanobis(X,Q);
        //        std::cout << "Hessian : \n" << Hessian << std::endl;
        Hessian += alpha*computeHessianMahalanobis(Q);
        //        std::cout << "Hessian2 : \n" << computeHessianMahalanobis(Q) << std::endl;

        scg = score_gradient;
	if(score_here < score_best)
	{
	    Tbest = T;
	    score_best = score_here;
            if (iter > 1) {
              if (!found_improvement) {
                this->nb_success_reg++;
                found_improvement = true;
                alpha = 1.;
              }
              std::cerr << "[" << iter << "] X : " << X.transpose() << " -- " << score_here << std::endl;
            }
	}
        else {
          //            std::cerr << "X : " << X.transpose() << std::flush;
        }

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double,6,6> > Sol (Hessian);
        Eigen::Matrix<double,6,1> evals = Sol.eigenvalues().real();
        double minCoeff = evals.minCoeff();
        double maxCoeff = evals.maxCoeff();
        if(minCoeff < 0)  //|| evals.minCoeff()) // < 10e-5*evals.maxCoeff())
        {
	    if(regularize) {
		Eigen::Matrix<double,6,6> evecs = Sol.eigenvectors().real();
		double regularizer = score_gradient.norm();
		regularizer = regularizer + minCoeff > 0 ? regularizer : 0.001*maxCoeff - minCoeff;
		//double regularizer = 0.001*maxCoeff - minCoeff;
		Eigen::Matrix<double,6,1> reg;
		//ugly
		reg<<regularizer,regularizer,regularizer,regularizer,regularizer,regularizer;
		evals += reg;
		Eigen::Matrix<double,6,6> Lam;
		Lam = evals.asDiagonal();
		Hessian = evecs*Lam*(evecs.transpose());
	    } else {
		if(score_here > score_best)
		{
		    T = Tbest;
		}
		//de-alloc nextNDT
		for(unsigned int i=0; i<nextNDT.size(); i++)
		{
		    if(nextNDT[i]!=NULL)
			delete nextNDT[i];
		}
		return true;
	    }
        }
        if (score_gradient.norm()<= DELTA_SCORE)
        {
	    if(score_here > score_best)
	    {
		T = Tbest;
	    }
            //de-alloc nextNDT
            for(unsigned int i=0; i<nextNDT.size(); i++)
            {
                if(nextNDT[i]!=NULL)
                    delete nextNDT[i];
            }
            return true;
        }

        //        std::cerr << " score_here : " << score_here << " score_gradient : " << score_gradient.transpose() << std::endl;

        pose_increment_v = -Hessian.ldlt().solve(score_gradient);

        double dginit = pose_increment_v.dot(scg);
        if(dginit > 0)
        {
            //de-alloc nextNDT
	    if(score_here > score_best)
	    {
		T = Tbest;
	    }
            for(unsigned int i=0; i<nextNDT.size(); i++)
            {
                if(nextNDT[i]!=NULL)
                    delete nextNDT[i];
            }
            return true;
        }
        //check direction here:

	if(step_control) {
          step_size = lineSearchMTSC(pose_increment_v,nextNDT,targetNDT,pose_local_v, Q, T);
	} else {
	    step_size = 1;
	}
        pose_increment_v = step_size*pose_increment_v;

        // const double max_step = 1.0;
        // if (pose_increment_v.norm() > max_step) {
        //   pose_increment_v.normalize();
        //   pose_increment_v *= max_step;
        // }

        TR.setIdentity();
        TR =  Eigen::Translation<double,3>(pose_increment_v(0),pose_increment_v(1),pose_increment_v(2))*
              Eigen::AngleAxis<double>(pose_increment_v(3),Eigen::Vector3d::UnitX()) *
              Eigen::AngleAxis<double>(pose_increment_v(4),Eigen::Vector3d::UnitY()) *
              Eigen::AngleAxis<double>(pose_increment_v(5),Eigen::Vector3d::UnitZ()) ;

        // Make sure that the pose_local_v is updated with the right frame(!)
        Eigen::Affine3d T_previous = T;
        T = TR*T;
        Eigen::Affine3d T_current = T;
        Eigen::Vector3d Tposition_diff = T_current.translation() - T_previous.translation();

        pose_increment_v_Tframe = pose_increment_v;
        pose_increment_v_Tframe(0) = Tposition_diff(0);
        pose_increment_v_Tframe(1) = Tposition_diff(1);
        pose_increment_v_Tframe(2) = Tposition_diff(2);
        pose_local_v += pose_increment_v_Tframe;

        for(unsigned int i=0; i<nextNDT.size(); i++)
        {
	    //TRANSFORM
	    Eigen::Vector3d meanC = nextNDT[i]->getMean();
	    Eigen::Matrix3d covC = nextNDT[i]->getCov();
	    meanC = TR*meanC;
	    covC = TR.rotation()*covC*TR.rotation().transpose();
	    nextNDT[i]->setMean(meanC);
	    nextNDT[i]->setCov(covC);
        }

        if(itr_ctr>0)
        {
            convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
        }
        if(itr_ctr>ITR_MAX)
        {
            convergence = true;
            ret = false;
        }
        itr_ctr++;
    }

    score_gradient.setZero();
    double score_here = derivativesNDT(nextNDT,targetNDT,score_gradient,Hessian,false);
    X = pose_local_v;
    if (lock_zrp_motion) {
      // Assign the X to be the offset between global z and global roll, pitch.
      X(2) = T.translation()[2];
      // No Euler angles, simply compute the roll and pitch as separate angles.
      double pitch = asin(T.rotation()(2,0)); // Z offset along the x axis...
      double roll = asin(T.rotation()(2,1));  // Z offset along the y axis...
      X(3) = roll;  //T.rotation().eulerAngles(0,1,2)[0];
      X(4) = pitch; //T.rotation().eulerAngles(0,1,2)[1];
      std::cout << "[" << X.transpose() << "]";
    }

    score_here += alpha*computeScoreMahalanobis(X,Q);
    if(score_here > score_best)
    {
	T = Tbest;
    }
    for(unsigned int i=0; i<nextNDT.size(); i++)
    {
	if(nextNDT[i]!=NULL)
	    delete nextNDT[i];
    }

    return ret;
}
} // namespace


#include <igl/cat.h>
#include <igl/edge_lengths.h>
#include <igl/parula.h>
#include <igl/per_edge_normals.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/point_mesh_squared_distance.h>
#include <igl/readMESH.h>
#include <igl/signed_distance.h>
#include <igl/slice_mask.h>
#include <igl/slice_tets.h>
#include <igl/upsample.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/writeOBJ.h>
#include <Eigen/Sparse>
#include <iostream>

#include "tutorial_shared_path.h"

Eigen::MatrixXd V;
Eigen::MatrixXi T,F;
igl::AABB<Eigen::MatrixXd,3> tree;
Eigen::MatrixXd FN,VN,EN;
Eigen::MatrixXi E;
Eigen::VectorXi EMAP;
double max_distance = 1;

double slice_z = 0.5;
bool overlay = false;

void update_visualization(igl::opengl::glfw::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  Eigen::Vector4d plane(
    0,0,1,-((1-slice_z)*V.col(2).minCoeff()+slice_z*V.col(2).maxCoeff()));
  MatrixXd V_vis;
  MatrixXi F_vis;
  // Extract triangle mesh slice through volume mesh and subdivide nasty
  // triangles
  {
    VectorXi J;
    SparseMatrix<double> bary;
    {
      // Value of plane's implicit function at all vertices
      const VectorXd IV =
        (V.col(0)*plane(0) +
         V.col(1)*plane(1) +
         V.col(2)*plane(2)).array()
        + plane(3);
      igl::slice_tets(V,T,IV,V_vis,F_vis,J,bary);
      igl::writeOBJ("vis.obj",V_vis,F_vis);
    }
    while(true)
    {
      MatrixXd l;
      igl::edge_lengths(V_vis,F_vis,l);
      l /= (V_vis.colwise().maxCoeff() - V_vis.colwise().minCoeff()).norm();
      const double max_l = 0.03;
      if(l.maxCoeff()<max_l)
      {
        break;
      }
      Array<bool,Dynamic,1> bad = l.array().rowwise().maxCoeff() > max_l;
      MatrixXi F_vis_bad, F_vis_good;
      igl::slice_mask(F_vis,bad,1,F_vis_bad);
      igl::slice_mask(F_vis,(bad!=true).eval(),1,F_vis_good);
      igl::upsample(V_vis,F_vis_bad);
      F_vis = igl::cat(1,F_vis_bad,F_vis_good);
    }
  }

  // Compute signed distance
  VectorXd S_vis;
  {
    VectorXi I;
    MatrixXd N,C;
    // Bunny is a watertight mesh so use pseudonormal for signing
    signed_distance_pseudonormal(V_vis,V,F,tree,FN,VN,EN,EMAP,S_vis,I,C,N);
  }
  // push to [0,1] range
  S_vis.array() = 0.5*(S_vis.array()/max_distance)+0.5;
  MatrixXd C_vis;
  // color without normalizing
  igl::parula(S_vis,false,C_vis);

  const auto & append_mesh = [&C_vis,&F_vis,&V_vis](
    const Eigen::MatrixXd & V,
    const Eigen::MatrixXi & F,
    const RowVector3d & color)
  {
    F_vis.conservativeResize(F_vis.rows()+F.rows(),3);
    F_vis.bottomRows(F.rows()) = F.array()+V_vis.rows();
    V_vis.conservativeResize(V_vis.rows()+V.rows(),3);
    V_vis.bottomRows(V.rows()) = V;
    C_vis.conservativeResize(C_vis.rows()+V.rows(),3);
    C_vis.bottomRows(V.rows()).rowwise() = color;
  };
  if(overlay)
  {
    append_mesh(V,F,RowVector3d(0.8,0.8,0.8));
  }
  viewer.data().clear();
  viewer.data().set_mesh(V_vis,F_vis);
  viewer.data().set_colors(C_vis);
  viewer.core.lighting_factor = overlay;
}

bool key_down(igl::opengl::glfw::Viewer& viewer, unsigned char key, int mod)
{
  switch(key)
  {
    default:
      return false;
    case ' ':
      overlay ^= true;
      break;
    case '.':
      slice_z = std::min(slice_z+0.01,0.99);
      break;
    case ',':
      slice_z = std::max(slice_z-0.01,0.01);
      break;
  }
  update_visualization(viewer);
  return true;
}

int main(int argc, char *argv[])
{
  using namespace Eigen;
  using namespace std;

  cout<<"Usage:"<<endl;
  cout<<"[space]  toggle showing surface."<<endl;
  cout<<"'.'/','  push back/pull forward slicing plane."<<endl;
  cout<<endl;

  // Load mesh: (V,T) tet-mesh of convex hull, F contains original surface
  // triangles
  igl::readMESH(TUTORIAL_SHARED_PATH "/bunny.mesh",V,T,F);

  // Encapsulated call to point_mesh_squared_distance to determine bounds
  {
    VectorXd sqrD;
    VectorXi I;
    MatrixXd C;
    igl::point_mesh_squared_distance(V,V,F,sqrD,I,C);
    max_distance = sqrt(sqrD.maxCoeff());
  }

  // Precompute signed distance AABB tree
  tree.init(V,F);
  // Precompute vertex,edge and face normals
  igl::per_face_normals(V,F,FN);
  igl::per_vertex_normals(
    V,F,igl::PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
  igl::per_edge_normals(
    V,F,igl::PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);

  // Plot the generated mesh
  igl::opengl::glfw::Viewer viewer;
  update_visualization(viewer);
  viewer.callback_key_down = &key_down;
  viewer.data().show_lines = false;
  viewer.launch();
}


#define BOOST_TEST_MODULE "Testing Manhattan Functions"

#include <boost/test/unit_test.hpp>

#include "distance/Manhattan.hpp"

using namespace genex;
#define TOLERANCE 1e-9

BOOST_AUTO_TEST_CASE( mandist, *boost::unit_test::tolerance(TOLERANCE) )
{
   Manhattan d;

   TimeSeries ts_1 {NULL, 0, 0, 2};
   TimeSeries ts_2 {NULL, 0, 0, 2};

   data_t a = d.dist(100.0, 110.0);
   data_t prev = d.init(); //0
   data_t first  = d.reduce(prev, prev, 60.0, 10.0); //50
   data_t second = d.reduce(first, first, 15.0, 5.0);
   data_t c = d.norm(second, ts_1, ts_2);

   d.clean(second);

   BOOST_TEST( a == 10 );
   BOOST_TEST( c == 30 );
}


/// @file
/// Interpolation for Lie groups.

#pragma once

#include <Eigen/Eigenvalues>

#include "interpolate_details.hpp"

namespace Sophus {

/// This function interpolates between two Lie group elements ``foo_T_bar``
/// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1].
///
/// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar``
/// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns
/// ``foo_T_baz``.
///
/// (Since interpolation on Lie groups is inverse-invariant, we can equivalently
/// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the
/// return value being ``quiz_T_foo``.)
///
/// Precondition: ``p`` must be in [0, 1].
///
template <class G, class Scalar2 = typename G::Scalar>
enable_if_t<interp_details::Traits<G>::supported, G> interpolate(
    G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) {
  using Scalar = typename G::Scalar;
  Scalar inter_p(p);
  SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1),
                "p ({}) must in [0, 1].", inter_p);
  return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log());
}

}  // namespace Sophus


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE snowlib

#include <boost/test/unit_test_suite.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/test_tools.hpp>
#include <ostream>

namespace tt = boost::test_tools;

#include "../lib/conjugate_residual_solver.h"
#include "../lib/SnowSolver.h"
#include "../lib/LavaSolver.h"

// A[3x3]
void A(std::vector<double> &Ax, std::vector<double> const &x) {
    Ax[0] = 2 * x[0] + x[1] + x[2];
    Ax[1] = x[0] + 2 * x[1] + x[2];
    Ax[2] = x[0] + x[1] + 2 * x[2];
}

// B[3x3]
void B(std::vector<glm::dvec3> &Ax, std::vector<glm::dvec3> const &x) {
    Ax[0] = glm::dmat3(2, 1, 1, 1, 2, 1, 1, 1, 2) * x[0];
}

template<typename T>
std::ostream &operator<<(std::ostream &os, std::vector<T> const &vector) {
    for (auto const &vec3 : vector) {
        os << vec3 << ", ";
    }
    return os;
}

BOOST_AUTO_TEST_SUITE(test_conjugate_gradient_method)

    BOOST_AUTO_TEST_CASE(test1) {

        // b
        std::vector<double> b = {1, 1, 1};

        // x - initial guess
        std::vector<double> x = {0, 0, 0};

        // Solve Ax = b
        conjugateResidualSolver(A, x, b, 2000);

        BOOST_TEST(x[0] == 0.25);
        BOOST_TEST(x[1] == 0.25);
        BOOST_TEST(x[2] == 0.25);

    }

    BOOST_AUTO_TEST_CASE(testb) {

        // b
        std::vector<glm::dvec3> b = {
                {1, 1, 1}
        };

        // x - initial guess
        std::vector<glm::dvec3> x = {
                {0, 0, 0}
        };

        // Solve Ax = b
        conjugateResidualSolver(B, x, b, 2000);

        BOOST_TEST(x[0][0] == 0.25);
        BOOST_TEST(x[0][1] == 0.25);
        BOOST_TEST(x[0][2] == 0.25);

    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_n)

    BOOST_AUTO_TEST_CASE(n) {

        auto n0 = SnowSolver::n(0);
        auto n0p5 = SnowSolver::n(0.5);
        auto n1 = SnowSolver::n(1);
        auto n1p5 = SnowSolver::n(1.5);
        auto n2 = SnowSolver::n(2);
        auto n2p5 = SnowSolver::n(2.5);

        std::cout << "n0=" << n0 << std::endl;
        std::cout << "n0p5=" << n0p5 << std::endl;
        std::cout << "n1=" << n1 << std::endl;
        std::cout << "n1p5=" << n1p5 << std::endl;
        std::cout << "n2=" << n2 << std::endl;
        std::cout << "n2p5=" << n2p5 << std::endl;

        BOOST_TEST(SnowSolver::n(-0.5) == n0p5);
        BOOST_TEST(SnowSolver::n(-1) == n1);
        BOOST_TEST(SnowSolver::n(-1.5) == n1p5);
        BOOST_TEST(SnowSolver::n(-2) == n2);
        BOOST_TEST(SnowSolver::n(-2.5) == n2p5);

    }

    BOOST_AUTO_TEST_CASE(del_n) {

        auto n0 = SnowSolver::del_n(0);
        auto n0p5 = SnowSolver::del_n(0.5);
        auto n1 = SnowSolver::del_n(1);
        auto n1p5 = SnowSolver::del_n(1.5);
        auto n2 = SnowSolver::del_n(2);
        auto n2p5 = SnowSolver::del_n(2.5);

        std::cout << "n0=" << n0 << std::endl;
        std::cout << "n0p5=" << n0p5 << std::endl;
        std::cout << "n1=" << n1 << std::endl;
        std::cout << "n1p5=" << n1p5 << std::endl;
        std::cout << "n2=" << n2 << std::endl;
        std::cout << "n2p5=" << n2p5 << std::endl;

        BOOST_TEST(SnowSolver::del_n(-2) == 0);
        BOOST_TEST(SnowSolver::del_n(-1.5) > 0);
        BOOST_TEST(SnowSolver::del_n(-1) > 0);
        BOOST_TEST(SnowSolver::del_n(-0.5) > 0);
        BOOST_TEST(SnowSolver::del_n(0) == 0);
        BOOST_TEST(SnowSolver::del_n(0.5) < 0);
        BOOST_TEST(SnowSolver::del_n(1) < 0);
        BOOST_TEST(SnowSolver::del_n(1.5) < 0);
        BOOST_TEST(SnowSolver::del_n(2) == 0);

        BOOST_TEST(SnowSolver::del_n(-0.5) == -n0p5);
        BOOST_TEST(SnowSolver::del_n(-1) == -n1);
        BOOST_TEST(SnowSolver::del_n(-1.5) == -n1p5);
        BOOST_TEST(SnowSolver::del_n(-2) == -n2);
        BOOST_TEST(SnowSolver::del_n(-2.5) == -n2p5);

    }

    BOOST_AUTO_TEST_CASE(tight_n) {

        auto n0 = LavaSolver::tight_n(0);
        auto n0p5 = LavaSolver::tight_n(0.5);
        auto n1 = LavaSolver::tight_n(1);
        auto n1p5 = LavaSolver::tight_n(1.5);
        auto n2 = LavaSolver::tight_n(2);
        auto n2p5 = LavaSolver::tight_n(2.5);

        std::cout << "n0=" << n0 << std::endl;
        std::cout << "n0p5=" << n0p5 << std::endl;
        std::cout << "n1=" << n1 << std::endl;
        std::cout << "n1p5=" << n1p5 << std::endl;
        std::cout << "n2=" << n2 << std::endl;
        std::cout << "n2p5=" << n2p5 << std::endl;

        BOOST_TEST(LavaSolver::tight_n(-0.5) == n0p5);
        BOOST_TEST(LavaSolver::tight_n(-1) == n1);
        BOOST_TEST(LavaSolver::tight_n(-1.5) == n1p5);
        BOOST_TEST(LavaSolver::tight_n(-2) == n2);
        BOOST_TEST(LavaSolver::tight_n(-2.5) == n2p5);

    }

    BOOST_AUTO_TEST_CASE(tight_del_n) {

        auto n0 = LavaSolver::tight_del_n(0);
        auto n0p5 = LavaSolver::tight_del_n(0.5);
        auto n1 = LavaSolver::tight_del_n(1);
        auto n1p5 = LavaSolver::tight_del_n(1.5);
        auto n2 = LavaSolver::tight_del_n(2);
        auto n2p5 = LavaSolver::tight_del_n(2.5);

        std::cout << "n0=" << n0 << std::endl;
        std::cout << "n0p5=" << n0p5 << std::endl;
        std::cout << "n1=" << n1 << std::endl;
        std::cout << "n1p5=" << n1p5 << std::endl;
        std::cout << "n2=" << n2 << std::endl;
        std::cout << "n2p5=" << n2p5 << std::endl;

        BOOST_TEST(LavaSolver::tight_del_n(-0.5) == -n0p5);
        BOOST_TEST(LavaSolver::tight_del_n(-1) == -n1);
        BOOST_TEST(LavaSolver::tight_del_n(-1.5) == -n1p5);
        BOOST_TEST(LavaSolver::tight_del_n(-2) == -n2);
        BOOST_TEST(LavaSolver::tight_del_n(-2.5) == -n2p5);

    }

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE(test_temperature)

    BOOST_AUTO_TEST_CASE(test_small_increments) {

        LavaParticleNode node({}, 1);
        node.temperature = 20;

        while (node.temperature < 50) {
            LavaSolver::applyTemperatureDifference(node, 1);
            std::cout << node.temperature << " " << node.latentHeat << std::endl;
        }

    }

    BOOST_AUTO_TEST_CASE(test_large_increments) {

        LavaParticleNode node({}, 1);
        node.temperature = 20;

        while (node.temperature < 50) {
            LavaSolver::applyTemperatureDifference(node, 50);
            std::cout << node.temperature << " " << node.latentHeat << std::endl;
        }

    }

    BOOST_AUTO_TEST_CASE(test_small_decrements) {

        LavaParticleNode node({}, 1);
        node.temperature = 50;

        while (node.temperature > 20) {
            LavaSolver::applyTemperatureDifference(node, -1);
            std::cout << node.temperature << " " << node.latentHeat << std::endl;
        }

    }

    BOOST_AUTO_TEST_CASE(test_large_decrements) {

        LavaParticleNode node({}, 1);
        node.temperature = 50;

        while (node.temperature > 20) {
            LavaSolver::applyTemperatureDifference(node, -50);
            std::cout << node.temperature << " " << node.latentHeat << std::endl;
        }

    }

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2018 Thanaphon Chavengsaksongkram <as12production@gmail.com>, He Sun <he.sun@ed.ac.uk>
// This file is subject to the license terms in the LICENSE file
// found in the top-level directory of this distribution.

/**
 * Example-5.cpp
 *
 * This example perform Spectral Sparsification, and use Spectra to calcualte Eigenvalue of Graph Laplacians
 *
 * */

#include <gSparse/gSparse.hpp>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/GenEigsSolver.h>
#include <Spectra/MatOp/SparseGenMatProd.h>

Eigen::VectorXcd GetTopEigenValue(const gSparse::SparsePrecisionMatrix & M)
{
    std::cout << "Calculating Eigen Value"<<std::endl;
    using namespace Spectra;
    SparseGenMatProd<double> op(M);

    // Construct eigen solver object, requesting the largest three eigenvalues
    GenEigsSolver< double, LARGEST_MAGN, SparseGenMatProd<double> > eigs(&op, 3, 6);

    // Initialize and compute
    eigs.init();
    int nconv = eigs.compute();

    // Retrieve results
    Eigen::VectorXcd evalues;
    if(eigs.info() == SUCCESSFUL)
        evalues = eigs.eigenvalues();

    std::cout << "Eigenvalues found:\n" << evalues << std::endl;
    return evalues;
}
int main()
{
    // Generate 100x100 Complete Graph
    auto graph = gSparse::Builder::buildUnitCompleteGraph(100);
    // Creating Sparsifier Object
    gSparse::SpectralSparsifier::ERSampling sparsifier(graph);
    // Set Hyper-parameters
    // Approximate the Effective Weight Resistance (Faster)
    // C = 100 and Epsilon = 0.5
    sparsifier.SetERPolicy(gSparse::SpectralSparsifier::APPROXIMATE_ER);
    sparsifier.SetC(100.0);
    sparsifier.SetEpsilon(0.5);
    // Compute Effective Weight Resistance using ApproxER
    sparsifier.Compute();
    // Get a sparsified graph
    auto sparseGraph1 = sparsifier.GetSparsifiedGraph();
    // Set to EXACT ER
    sparsifier.SetERPolicy(gSparse::SpectralSparsifier::EXACT_ER);
    // Re-calcuate ER using ExactER
    sparsifier.Compute();
    // Get a sparsified graph
    auto sparseGraph2 = sparsifier.GetSparsifiedGraph();

    // Use Spectra to calculate top Eigen values.
    std::cout<<"Original Eigen Value " <<std::endl;
    GetTopEigenValue(graph->GetLaplacianMatrix());
    std::cout<<"---------------------------"<<std::endl;
    std::cout<<"Top Eigen Value for ApproxER "<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix());
    std::cout<<"Top Eigen Value for ApproxER - Original"<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix() - graph->GetLaplacianMatrix());
    std::cout<<"---------------------------"<<std::endl;
    std::cout<<"Top Eigen Value for ExactER "<<std::endl;
    GetTopEigenValue(sparseGraph2->GetLaplacianMatrix());
    std::cout<<"Top Eigen Value for ApproxER - ExactER"<<std::endl;
    GetTopEigenValue(sparseGraph1->GetLaplacianMatrix() - graph->GetLaplacianMatrix());
    return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Triangulation_2.h>

#include <CGAL/boost/graph/dijkstra_shortest_paths.h>
#include <boost/graph/filtered_graph.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point;

typedef CGAL::Triangulation_2<K> Triangulation;

// As we want to run Dijskra's shortest path algorithm we only
// consider finite vertices and edges.

template <typename T>
struct Is_finite {

  const T* t_;

  Is_finite()
    : t_(NULL)
  {}

  Is_finite(const T& t)
    : t_(&t)
  { }

  template <typename VertexOrEdge>
  bool operator()(const VertexOrEdge& voe) const {
    return ! t_->is_infinite(voe);
  }
};

typedef Is_finite<Triangulation> Filter;
typedef boost::filtered_graph<Triangulation,Filter,Filter> Finite_triangulation;
typedef boost::graph_traits<Finite_triangulation>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Finite_triangulation>::vertex_iterator vertex_iterator;

typedef std::map<vertex_descriptor,int> VertexIndexMap;
VertexIndexMap vertex_id_map;

typedef boost::associative_property_map<VertexIndexMap> VertexIdPropertyMap;
VertexIdPropertyMap vertex_index_pmap(vertex_id_map);

int
main(int,char*[])
{
  Triangulation t;
  Filter is_finite(t);
  Finite_triangulation ft(t, is_finite, is_finite);

  t.insert(Point(0,0));
  t.insert(Point(1,0));
  t.insert(Point(0.2,0.2));
  t.insert(Point(0,1));
  t.insert(Point(0,2));

  vertex_iterator vit, ve;
  // Associate indices to the vertices
  int index = 0;
  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vit and ve
  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor  vd = *vit;
    vertex_id_map[vd]= index++;
    }

  // Dijkstra's shortest path needs property maps for the predecessor and distance
  // We first declare a vector
  std::vector<vertex_descriptor> predecessor(boost::num_vertices(ft));
  // and then turn it into a property map
  boost::iterator_property_map<std::vector<vertex_descriptor>::iterator,
                               VertexIdPropertyMap>
    predecessor_pmap(predecessor.begin(), vertex_index_pmap);

  std::vector<double> distance(boost::num_vertices(ft));
  boost::iterator_property_map<std::vector<double>::iterator,
                               VertexIdPropertyMap>
    distance_pmap(distance.begin(), vertex_index_pmap);

  // start at an arbitrary vertex
  vertex_descriptor source = *boost::vertices(ft).first;
  std::cout << "\nStart dijkstra_shortest_paths at " << source->point() <<"\n";

  boost::dijkstra_shortest_paths(ft, source,
				 distance_map(distance_pmap)
				 .predecessor_map(predecessor_pmap)
				 .vertex_index_map(vertex_index_pmap));

  for(boost::tie(vit,ve)=boost::vertices(ft); vit!=ve; ++vit ){
    vertex_descriptor vd = *vit;
    std::cout << vd->point() << " [" <<  vertex_id_map[vd] << "] ";
    std::cout << " has distance = "  << boost::get(distance_pmap,vd)
	      << " and predecessor ";
    vd =  boost::get(predecessor_pmap,vd);
    std::cout << vd->point() << " [" <<  vertex_id_map[vd] << "]\n ";
  }

  return 0;
}