#ifndef HOPS_LINEARMODEL_HPP
#define HOPS_LINEARMODEL_HPP

#define _USE_MATH_DEFINES

#include <math.h>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/LU>
#include <utility>

namespace hops {
    template<typename Matrix, typename Vector>
    class LinearModel {
    public:
        using MatrixType = Matrix;
        using VectorType = Vector;

        LinearModel(VectorType measuredData, MatrixType dataCovariance, MatrixType linearModel);

        /**
         * @brief Evaluates the negative log likelihood for input x.
         * @param x
         * @return
         */
        typename MatrixType::Scalar computeNegativeLogLikelihood(const VectorType &x) const;

        MatrixType computeExpectedFisherInformation(const VectorType &) const;

        VectorType computeLogLikelihoodGradient(const VectorType &x) const;

    private:
        VectorType measuredData;
        MatrixType dataCovariance;
        MatrixType inverseCovariance;
        MatrixType linearModel;
        typename MatrixType::Scalar logNormalizationConstant;
    };

    template<typename MatrixType, typename VectorType>
    LinearModel<MatrixType, VectorType>::LinearModel(
            VectorType measuredData,
            MatrixType dataCovariance,
            MatrixType linearModel) :
            measuredData(std::move(measuredData)),
            dataCovariance(std::move(dataCovariance)),
            linearModel(std::move(linearModel)) {
        Eigen::LLT<MatrixType, Eigen::Upper> solver(this->dataCovariance);
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixL = solver.matrixL();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> matrixU = solver.matrixU();
        Eigen::Matrix<typename MatrixType::Scalar, Eigen::Dynamic, Eigen::Dynamic> inverseMatrixL = matrixL.inverse();
        inverseCovariance = inverseMatrixL * inverseMatrixL.transpose();

        logNormalizationConstant = -static_cast<typename MatrixType::Scalar>(this->measuredData.rows()) / 2 *
                                   std::log(2 * M_PI)
                                   - matrixL.diagonal().array().log().sum();
    }

    template<typename MatrixType, typename VectorType>
    typename MatrixType::Scalar
    LinearModel<MatrixType, VectorType>::computeNegativeLogLikelihood(const VectorType &x) const {
        return -logNormalizationConstant +
               0.5 * static_cast<typename MatrixType::Scalar>((linearModel * x - measuredData).transpose() *
                                                              inverseCovariance * (linearModel * x - measuredData));
    }

    template<typename MatrixType, typename VectorType>
    MatrixType
    LinearModel<MatrixType, VectorType>::computeExpectedFisherInformation(const VectorType &) const {
        return inverseCovariance;
    }

    template<typename MatrixType, typename VectorType>
    VectorType
    LinearModel<MatrixType, VectorType>::computeLogLikelihoodGradient(const VectorType &x) const {
        return -inverseCovariance * (linearModel*x - measuredData);
    }
}

#endif //HOPS_LINEARMODEL_HPP


#include "statistics.hpp"

#include <armadillo>

void moment_function_4th(const arma::vec &seq,
                         const int width,
                         arma::vec &momfunc4)
{
  int num_seq = seq.n_rows;
  int idx = 0;
  for (int k = 0; k < width; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;
        double sum = 0;
        for (int n = 0; n < num_seq - k; ++n) {
          sum += seq(n) * seq(n + i) * seq(n + j) * seq(n + k);
        }
        momfunc4(idx) += sum;
        idx++;
      }
    }
  }
}

void cumulants_2nd(const arma::vec &seq,
                   const int width,
                   arma::vec &cum2)
{
  for (int i = 0; i < width; ++i) {
    double sum = 0.;
    for (int j = 0; j <= seq.n_elem - width; ++j) {
      sum += seq(j) * seq(j + i);
    }
    cum2(i) += sum;
  }
}

void cumulants_4th(const arma::vec &seq,
                   const int n2,
                   const int n4,
                   arma::vec &cum4)
{
  int num_seq = seq.n_rows;
  arma::vec cum2(n2, arma::fill::zeros);
  cumulants_2nd(seq, n2, cum2);
  double pwr = cum2(0) / num_seq;
  double scale2 = 1.0 / cum2(0);
  cum2 *= scale2;

  arma::vec mfunc4(n4, arma::fill::zeros);
  moment_function_4th(seq, n2, mfunc4);
  double scale4 = 1.0 / (num_seq * std::pow(pwr, 2));

  int idx = 0;
  for (int k = 0; k < n2; ++k) {
    for (int j = 0; j <= k; ++j) {
      for (int i = 0; i <= j; ++i) {
        if (i == 0 && k > 0 && k == j) continue;
        double m4g = cum2(i) * cum2(k - j) + cum2(j) * cum2(k - i)
          + cum2(k) * cum2(j - i);
        cum4(idx) += (scale4 * mfunc4(idx) - m4g) * (n2 - k) / n2;

        idx++;
      }
    }
  }
}


#include "fhd_math.h"
#include <math.h>
#include <float.h>
#include <assert.h>
#include <Eigen/Dense>

fhd_vec2 fhd_vec2_sub(fhd_vec2 a, fhd_vec2 b) {
  fhd_vec2 r;
  r.x = a.x - b.x;
  r.y = a.y - b.y;
  return r;
}

fhd_vec2 fhd_vec2_normalize(fhd_vec2 v) {
  const float a = sqrtf(v.x * v.x + v.y * v.y);
  return fhd_vec2{v.x / a, v.y / a};
}

fhd_vec2 fhd_vec2_mul(fhd_vec2 a, float s) { return {a.x * s, a.y * s}; }

fhd_vec2 fhd_vec2_mul_pcw(fhd_vec2 a, fhd_vec2 b) {
  return {a.x * b.x, a.y * b.y};
}

float fhd_vec2_distance(fhd_vec2 a, fhd_vec2 b) {
  return hypotf(b.x - a.x, b.y - a.y);
}

float fhd_vec2_length(fhd_vec2 v) { return sqrtf(v.x * v.x + v.y * v.y); }

fhd_vec3 fhd_vec3_normalize(fhd_vec3 v) {
  const float a = sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
  return fhd_vec3{v.x / a, v.y / a, v.z / a};
}

float fhd_vec3_dot(fhd_vec3 a, fhd_vec3 b) {
  return a.x * b.x + a.y * b.y + a.z * b.z;
}

fhd_vec3 fhd_vec3_sub(fhd_vec3 a, fhd_vec3 b) {
  fhd_vec3 r;
  r.x = a.x - b.x;
  r.y = a.y - b.y;
  r.z = a.z - b.z;
  return r;
}

fhd_vec3 fhd_vec3_cross(fhd_vec3 u, fhd_vec3 v) {
  const float t1 = u.x - u.y;
  const float t2 = v.y + v.z;
  const float t3 = u.x * v.z;
  const float t4 = t1 * t2 - t3;

  fhd_vec3 r;
  r.x = v.y * (t1 - u.z) - t4;
  r.y = u.z * v.x - t3;
  r.z = t4 - u.y * (v.x - t2);

  return r;
}

void fhd_aabb_expand(fhd_aabb* bbox, fhd_vec2 point) {
  if (point.x < bbox->top_left.x)
    bbox->top_left.x = point.x;
  else if (point.x > bbox->bot_right.x)
    bbox->bot_right.x = point.x;

  if (point.y < bbox->top_left.y)
    bbox->top_left.y = point.y;
  else if (point.y > bbox->bot_right.y)
    bbox->bot_right.y = point.y;
}

fhd_vec2 fhd_aabb_center(const fhd_aabb* bbox) {
  return {(bbox->bot_right.x + bbox->top_left.x) * 0.5f,
          (bbox->bot_right.y + bbox->top_left.y) * 0.5f};
}

bool fhd_aabb_overlap(const fhd_aabb* a, const fhd_aabb* b) {
  if (a->bot_right.x < b->top_left.x || a->top_left.x > b->bot_right.x)
    return false;
  if (a->bot_right.y < b->top_left.y || a->top_left.y > b->bot_right.y)
    return false;

  return true;
}

fhd_vec2 fhd_aabb_size(const fhd_aabb* a) {
  fhd_vec2 r;
  r.x = a->bot_right.x - a->top_left.x;
  r.y = a->bot_right.y - a->top_left.y;
  return r;
}

fhd_aabb fhd_aabb_from_points(const fhd_vec2* points, int len) {
  assert(len >= 2);

  fhd_aabb bbox = {{FLT_MAX, FLT_MAX}, {-FLT_MAX, -FLT_MAX}};

  for (int i = 0; i < len; i++) {
    fhd_aabb_expand(&bbox, points[i]);
  }

  return bbox;
}

float fhd_plane_point_dist(fhd_plane p, fhd_vec3 q) {
  return (fhd_vec3_dot(p.n, q) - p.d) / fhd_vec3_dot(p.n, p.n);
}

fhd_plane fhd_make_plane(fhd_vec3 a, fhd_vec3 b, fhd_vec3 c) {
  fhd_plane p;
  p.n = fhd_vec3_normalize(
      fhd_vec3_cross(fhd_vec3_sub(b, a), fhd_vec3_sub(c, a)));
  p.d = fhd_vec3_dot(p.n, a);
  return p;
}

float fhd_fast_atan2(float y, float x) {
  const float THRQTR_PI = 3.f * F_PI_4;
  float r, angle;
  float abs_y = fabs(y) + 1e-10f;
  if (x < 0.0f) {
    r = (x + abs_y) / (abs_y - x);
    angle = THRQTR_PI;
  } else {
    r = (x - abs_y) / (x + abs_y);
    angle = F_PI_4;
  }
  angle += (0.1963f * r * r - 0.9817f) * r;
  if (y < 0.0f)
    return -angle;
  else
    return angle;
}

fhd_vec3 fhd_pcl_normal(const fhd_vec3* points, int len) {
  Eigen::Matrix3f A = Eigen::Matrix3f::Zero();
  Eigen::Vector3f b = Eigen::Vector3f::Zero();
  for (int i = 0; i < len; i++) {
    const fhd_vec3 v = points[i];
    A(0, 0) += v.x * v.x;
    A(0, 1) += v.x * v.y;
    A(0, 2) += v.x;
    A(1, 0) += v.x * v.y;
    A(1, 1) += v.y * v.y;
    A(1, 2) += v.y;
    A(2, 0) += v.x;
    A(2, 1) += v.y;

    b(0) += v.x * v.z;
    b(1) += v.y * v.z;
    b(2) += v.z;
  }

  A(2, 2) += float(len);
  Eigen::Vector3f solution = A.llt().solve(b);
  return fhd_vec3_normalize(fhd_vec3{solution(0), solution(1), solution(2)});
}


//! [dot-main]
#include <boost/simd/function/load.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>

template <typename Value>
Value scaldot(Value* first1, Value* last1, Value* first2)
{
  Value v(0);

  for (; first1 < last1; ++first1, ++first2) {
    v += (*first1) * (*first2);
  }
  return v;
}

template <typename Value>
Value simddot(Value* first1, Value* last1, Value* first2)
{
  namespace bs = boost::simd;
  using pack_t = bs::pack<Value>;

  pack_t tmp{0};
  int card = pack_t::static_size;

  for (; first1 + card <= last1; first1 += card, first2 += card) {
    // Load current values from the datasets
    pack_t x1 = bs::load<pack_t>(first1);
    pack_t x2 = bs::load<pack_t>(first2);
    // Computation
    tmp = tmp + x1 * x2;
  }

  Value dot_product = bs::sum(tmp);
  for (; first1 < last1; ++first1, ++first2) {
    dot_product += (*first1) * (*first2);
  }

  return dot_product;
}

int main()
{
  namespace bs = boost::simd;
  using pack_t = bs::pack<float>;

  const size_t size = 113;
  float card_float  = bs::cardinal_of<pack_t>();

  std::vector<float> v1(size), v2(size);
  std::iota(v1.begin(), v1.end(), 0);
  std::iota(v2.begin(), v2.end(), 1);
  std::cout << "scalar dot product output " << scaldot(v1.data(), v1.data() + size, v2.data())
            << std::endl;
  std::cout << "simd   dot product output " << simddot(v1.data(), v1.data() + size, v2.data())
            << std::endl;
};
// This code can be compiled using (for instance for gcc)
// g++ dotmain.cpp -msse4.2 -std=c++11 -O3 -DNDEBUG -o dotmain
// -I/path_to/boost_simd/ -I/path_to/boost/
//! [dot-main]


// Copyright Paul A. Bristow 2018

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

// Example of most basic call of both lambert W functions.
// Only requires C++03
// (and optionally a call of max_digits10 to show precision).

#include <boost/math/special_functions/lambert_w.hpp> // For lambert_w0 and wm1 functions.

#include <iostream>
#include <iomanip>

int main()
{
  double z = 2.0;
  double w0 = boost::math::lambert_w0(z);
  std::cout.setf(std::ios_base::showpoint); // Include any trailing zeros.
  std::cout.precision(std::numeric_limits<double>::max_digits10); // Show all possibly significant digits.
  // Avoid using max_digfigs10 so as many old compilers can run the most basic lambert_w0 test?
  // Require to get max_digits10
  //   [ run lambert_w_basic_example.cpp  : : : [ requires cxx11_numeric_limits ] ]
  std::cout << " lambert_w0(" << z << ") = " << w0 << std::endl; // lambert_w0(2.00000) = 0.852606
  z = -0.2;
  double wm1 = boost::math::lambert_wm1(z);
  std::cout << " lambert_wm1(" << z << ") = " << wm1 << std::endl; // lambert_wm1(-0.200000) = -2.54264
  return 0;
} // int main()


#ifndef Utils_LinearRegression_hpp
#define Utils_LinearRegression_hpp

/** @file LinearRegression.hpp
  * @brief
  * @author C.D. Clark III
  * @date 07/07/17
  */

#include <Eigen/Dense>
using namespace Eigen;

namespace RUC {

/*
 * @breif Perform least-squared linear regression on a set of x-y data points.
 *
 * @param x vector containing x coordinates of point to fit.
 * @param y vector containing y coordinates of point to fit.
 * @return A 2x1 matrix containing the y-intercept (element 0) and slope (element 1) of the fit.
 */
template<typename T>
Matrix<T,2,1> LinearRegression( const Matrix<T,Dynamic,1> &x, const Matrix<T,Dynamic,1> &y )
{
  // given vectors x = | x0 | and y = | y0 | of x,y pairs,
  //                   | x1 | and     | y1 |
  //                   | .. | and     | .. |
  //                   | xn | and     | yn |
  //
  // construct matrix X = | 1 x0 ] see https://en.wikipedia.org/wiki/Linear_least_squares_(mathematics)
  //                      | 1 x1 ]
  //                      | .... ]
  //                      | 1 xn ]
  //
  // then coefficients matrix B = | b | that minimize sum of residuals is
  //                              | m |
  //
  // is found by solveing X^T X B = X^T y,
  // so
  //       B = (X^T X)^-1 X^T y;

  // matrix X
  assert( x.size() == y.size() );

  auto N = x.size();

  Matrix<T,Dynamic,2> X(N,2);
  // fill first column with 1's
  X.block(0,0,N,1) = Matrix<T,Dynamic,1>::Constant(N,1,1);
  // fill second column with x values
  X.block(0,1,N,1) = x;

  // need the transpose of X
  auto Xtrans = X.transpose();

  auto fit = (Xtrans*X).inverse()*Xtrans*y;

  Matrix<T,2,1> ret;
  ret[0] = fit[0];
  ret[1] = fit[1];

  return ret;
}

}

#endif // include protector


#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/debug.hpp>
#include <dcs/math/random/mersenne_twister.hpp>
#include <dcs/math/stats/distribution/mmpp.hpp>
#include <dcs/test.hpp>

static const double tol(1.0e-5);

DCS_TEST_DEF( rand )
{
	DCS_DEBUG_TRACE("Test Case: Random Variate Generation");

	typedef double real_type;
	typedef ::boost::numeric::ublas::matrix<real_type> matrix_type;
	typedef ::boost::numeric::ublas::vector<real_type> vector_type;
	typedef ::std::size_t size_type;

	const size_type n(2);
	const unsigned long seed(5489UL);

	vector_type lambda(n);
	lambda(0) = 20;
	lambda(1) = 2;

	matrix_type Q(n,n);
	Q(0,0) = -2; Q(0,1) =  2;
	Q(1,0) =  1; Q(1,1) = -1;

	::dcs::math::stats::mmpp_distribution<real_type> mmpp(lambda, Q);
	DCS_DEBUG_TRACE("Q: " << mmpp.Q());
	DCS_DEBUG_TRACE("lambda: " << mmpp.lambda());

	::dcs::math::random::mt19937 rng(seed);

	real_type res;
	real_type expect;

	res = ::dcs::math::stats::rand(mmpp, rng);
	expect = 0.107375;
	DCS_DEBUG_TRACE("x[1] " << ::std::fixed << res);
	DCS_TEST_CHECK_CLOSE( res, expect, tol );
}

int main()
{
	DCS_TEST_BEGIN();

	DCS_TEST_DO( rand );

	DCS_TEST_END();
}


// Unit test for the cons2prim function in fvm_1d_functions.cpp
// This function calculates the conservative variables based on the primitive variables for the 1D Euler equations
// COMPLETE. This function works as intended. 2021/11/21

#include <iostream>
#include <Eigen/Dense>
#include "../main/fvm_1D_functions.h"
#include "../main/testing_functions.h"

using namespace std;
using namespace Eigen;

int main() {

    // Print what we are doing
    cout << "------------------------------------- " << \
    "This is a test of the cons2prim function." << \
    " -------------------------------------" << endl;

    // Set the input parameters to the cons2prim function
    double gasGamma = 5./3.;   // Use standard gasGamma of 5/3

    // Make an array of primitive variables such that there are two elements in each 1D array
    Array<ArrayXd, 3, 1> Q;
    Q(0) = ArrayXd::Zero(2) + 1.5;
    Q(1) = ArrayXd::Zero(2) + 3.15;
    Q(2) = ArrayXd::Zero(2) + 7.8075;

    // Set the exact values for the primitive variables
    Array<ArrayXd, 3, 1> V_exact;
    V_exact(0) = ArrayXd::Zero(2) + 1.5;
    V_exact(1) = ArrayXd::Zero(2) + 2.1;
    V_exact(2) = ArrayXd::Zero(2) + 3.0;

    // Initialize an array of conservative variables
    Array<ArrayXd, 3, 1> V;
    for (int var = 0; var < 3; var++ ) {
        V(var) = ArrayXd::Zero(2);
    }

    // Calculate what the conserved variables are
    cons2prim(gasGamma, Q, V);

    // Set the acceptable tolerance. (Somewhat arbitrarily chosen)
    double TOL = 1e-14;

    // Determine if the two arrays are equal
    Array<Array<bool,Dynamic,1>, 3, 1> boolArray = isEqualArray1D_of_Array1D(V, V_exact, TOL);

    // Get the minimum value of the boolean array.
    // If it is 0, then at least one value in this array is incorrect
    for (int var = 0; var < 3; var++) {
        if (boolArray(var).minCoeff() == 0) {
            cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl \
        << "Test of cons2prim is a failure." << endl \
        << "!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
        break;
        }
        else if (var == 2) {
            cout << "Test of cons2prim is a success." << endl;
        }
    }

    cout << endl << endl;
}


#ifdef HOPS_GUROBI_FOUND

#define BOOST_TEST_MODULE LinearProgramGurobiImplTestSuite
#define BOOST_TEST_DYN_LINK

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <hops/hops.hpp>

BOOST_AUTO_TEST_SUITE(LinearProgrammingGurobiImpl)

    BOOST_AUTO_TEST_CASE(solveSmallTestProblem) {
        Eigen::VectorXd expectedParameters(2);
        expectedParameters << 8.0 / 3, 2.0 / 3;

        hops::LinearProgramSolution expectedSolution(10. / 3, expectedParameters, hops::LinearProgramStatus::OPTIMAL);

        Eigen::MatrixXd A(3, 2);
        A << 1, 2, 4, 2, -1, 1;
        Eigen::VectorXd b(3);
        b << 4, 12, 1;
        Eigen::VectorXd obj(2);
        obj << 1, 1;
        hops::LinearProgramGurobiImpl linearProgram(A, b);
        hops::LinearProgramSolution actualSolution = linearProgram.solve(obj);

        BOOST_CHECK_CLOSE(actualSolution.objectiveValue, expectedSolution.objectiveValue, 0.0001);
        BOOST_CHECK(actualSolution.optimalParameters.isApprox(expectedSolution.optimalParameters));
        BOOST_CHECK(actualSolution.status == expectedSolution.status);
    }

    BOOST_AUTO_TEST_CASE(computeChebyshevCenter) {
        Eigen::VectorXd expectedChebyshevCenter(2);
        expectedChebyshevCenter << 0.29289321881345, 0.29289321881345;

        Eigen::MatrixXd A(3, 2);
        Eigen::VectorXd b(3);
        A << 1, 1,
                -1, 0,
                0, -1;
        b << 1, 0, 0;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto actualChebyshevCenter = linearProgram.computeChebyshevCenter();

        BOOST_CHECK(actualChebyshevCenter.optimalParameters.isApprox(expectedChebyshevCenter, 1e-12));
    }

    BOOST_AUTO_TEST_CASE(computeChebyshevCenterIsStableUnderRepeatedCalculations) {
        Eigen::MatrixXd A(3, 2);
        Eigen::VectorXd b(3);
        A << 1, 1,
                -1, 0,
                0, -1;
        b << 1, 0, 0;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto actualChebyshevCenter1 = linearProgram.computeChebyshevCenter();
        auto actualChebyshevCenter2 = linearProgram.computeChebyshevCenter();
        auto actualChebyshevCenter3 = linearProgram.computeChebyshevCenter();
        auto actualChebyshevCenter4 = linearProgram.computeChebyshevCenter();

        BOOST_CHECK(actualChebyshevCenter1 == actualChebyshevCenter2);
        BOOST_CHECK(actualChebyshevCenter1 == actualChebyshevCenter3);
        BOOST_CHECK(actualChebyshevCenter1 == actualChebyshevCenter4);
        BOOST_CHECK(actualChebyshevCenter2 == actualChebyshevCenter3);
        BOOST_CHECK(actualChebyshevCenter2 == actualChebyshevCenter4);
        BOOST_CHECK(actualChebyshevCenter3 == actualChebyshevCenter4);
    }

    BOOST_AUTO_TEST_CASE(removeSingleRedundantConstraintTest) {
        Eigen::MatrixXd expectedA(4, 2);
        expectedA << 1, 0, 0, 1, -1, 0, 0, -1;
        Eigen::VectorXd expectedb(4);
        expectedb << 1, 1, 1, 1;

        Eigen::MatrixXd A(5, 2);
        A << 1, 0, 0, 1, -1, 0, 0, -1, 1, 0;
        Eigen::VectorXd b(5);
        b << 1, 1, 1, 1, 2;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto[actualA, actualb] = linearProgram.removeRedundantConstraints(1e-15);

        BOOST_CHECK(actualA.isApprox(expectedA));
        BOOST_CHECK(actualb.isApprox(expectedb));
    }

    BOOST_AUTO_TEST_CASE(removeSeveralRedundantConstraintsTest) {
        Eigen::MatrixXd expectedA(4, 2);
        expectedA << 1, 0, 0, 1, -1, 0, 0, -1;
        Eigen::VectorXd expectedB(4);
        expectedB << 1, 1, 1, 1;

        Eigen::MatrixXd A(10, 2);
        A << 1, 0, 0, 1, -1, 0, 0, -1, 1, 0,
                1, 0, 1, 0, 1, 0, 1, 0, 1, 0;
        Eigen::VectorXd b(10);
        b << 1, 1, 1, 1, 2, 7, 4, 2, 100, 5;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto[actualA, actualB] = linearProgram.removeRedundantConstraints(1e-15);
        BOOST_CHECK(actualA.isApprox(expectedA));
        BOOST_CHECK(actualB.isApprox(expectedB));
    }

    BOOST_AUTO_TEST_CASE(computeUnconstrainedDimensions) {
        std::vector<long> expectedUnboundDirections{1, -1};

        Eigen::MatrixXd A(2, 2);
        A << 0, 1, 0, -1;
        Eigen::VectorXd b(2);
        b << 1, 1;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto actualUnboundDirections = linearProgram.computeUnconstrainedDimensions();

        BOOST_CHECK(actualUnboundDirections == expectedUnboundDirections);
    }

    BOOST_AUTO_TEST_CASE(addConstraintsToUnconstrainedDimensions) {
        Eigen::MatrixXd expectedA(4, 2);
        expectedA << 0, 1, 0, -1, 1, 0, -1, 0;
        Eigen::VectorXd expectedB(4);
        expectedB << 1, 1, 3, 2;

        Eigen::MatrixXd A(2, 2);
        A << 0, 1, 0, -1;
        Eigen::VectorXd b(2);
        b << 1, 1;

        auto linearProgram = hops::LinearProgramGurobiImpl(A, b);
        auto[actualA, actualB] = linearProgram.addBoxConstraintsToUnconstrainedDimensions(2, 3);

        BOOST_CHECK(actualA == expectedA);
        BOOST_CHECK(actualB == expectedB);
    }

BOOST_AUTO_TEST_SUITE_END()

#endif //HOPS_GUROBI_FOUND


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

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::MatrixXf;
using Eigen::VectorXd;
using Eigen::VectorXf;
using namespace std;

class ANN
{
private:
    MatrixXd Wh, W0;
    VectorXd bh, b0;
    int input_layer = 5 * 5, hidden_layer = 16, output_layer;
    double lr = 1.;
    double eps = 1e-6;

public:
    ANN(vector<MatrixXd *> &train_symbols)
    {
        double HI = 2., LO = -2.;
        double range = HI - LO;

        output_layer = train_symbols.size();

        Wh = MatrixXd::Random(hidden_layer, input_layer);
        Wh = (Wh + MatrixXd::Constant(hidden_layer, input_layer, 1.)) * range / 2.;
        Wh = (Wh + MatrixXd::Constant(hidden_layer, input_layer, LO));

        bh = VectorXd::Zero(hidden_layer);

        W0 = MatrixXd::Random(output_layer, hidden_layer);
        W0 = (W0 + MatrixXd::Constant(output_layer, hidden_layer, 1.)) * range / 2.;
        W0 = (W0 + MatrixXd::Constant(output_layer, hidden_layer, LO));

        b0 = VectorXd::Zero(output_layer);
        train(train_symbols);
    }

    double sigmoid(double x)
    {
        return 1.0 / (1.0 + exp(-x));
    }

    void train(vector<MatrixXd *> train_symbols)
    {
        MatrixXd EW0, EWh, EI0, EIh;
        do
        {
            int i = 0;
            for (std::vector<MatrixXd *>::iterator it = train_symbols.begin(); it != train_symbols.end(); ++it)
            {
                MatrixXd yd = MatrixXd::Zero(output_layer, 1);
                yd(i) = 1.;
                // Forward pass
                MatrixXd X = **it;
                Map<VectorXd> X_flat(X.data(), X.size());

                MatrixXd Vh = Wh * X_flat + bh;
                MatrixXd Zh = Vh.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
                MatrixXd U0 = W0 * Zh + b0;
                MatrixXd Y0 = U0.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });

                // Backward propagation
                MatrixXd EA0 = Y0 - yd;
                EI0 = EA0.cwiseProduct(Y0.cwiseProduct((MatrixXd)(1 - Y0.array())));
                EW0 = EI0 * Zh.transpose();
                MatrixXd EAh = W0.transpose() * EI0;
                EIh = EAh.cwiseProduct(Zh.cwiseProduct((MatrixXd)(1 - Zh.array())));
                EWh = EIh * X_flat.transpose();

                //Update params
                W0 = (W0 - lr * EW0);
                Wh = (Wh - lr * EWh);
                b0 = (b0 + lr * EI0);
                bh = (bh + lr * EIh);

                i += 1;
            }
        } while (!(EW0.isZero(eps) && EWh.isZero(eps) && EI0.isZero(eps) && EIh.isZero(eps)));
    }

    VectorXd *test(MatrixXd &symbol)
    {
        MatrixXd X = symbol;
        Map<VectorXd> X_flat(X.data(), X.size());

        MatrixXd Vh = Wh * X_flat + bh;
        MatrixXd Zh = Vh.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
        MatrixXd U0 = W0 * Zh + b0;
        MatrixXd *Y0 = new MatrixXd(U0.rows(), U0.cols());
        *Y0 << U0.unaryExpr([](double x) { return 1.0 / (1.0 + exp(-x)); });
        return (VectorXd *)Y0;
    }
};

int main()
{
    srand((unsigned int)time(0));
    vector<MatrixXd *> train_symbols;
    MatrixXd m1, m2;
    m1 = MatrixXd::Random(5, 5);
    train_symbols.push_back(&m1);
    m2 = MatrixXd::Random(5, 5);
    train_symbols.push_back(&m2);
    ANN a(train_symbols);
    VectorXd *y2 = a.test(m2);
    cout << *y2 << '\n';
    VectorXd *y1 = a.test(m1);
    cout << *y1 << '\n';
}


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Random.h>

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <CGAL/Surface_mesh_shortest_path.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/iterator.h>

#include <boost/variant.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron_3;
typedef CGAL::Surface_mesh_shortest_path_traits<Kernel, Polyhedron_3> Traits;
typedef CGAL::Surface_mesh_shortest_path<Traits> Surface_mesh_shortest_path;
typedef Traits::Barycentric_coordinate Barycentric_coordinate;
typedef boost::graph_traits<Polyhedron_3> Graph_traits;
typedef Graph_traits::vertex_iterator vertex_iterator;
typedef Graph_traits::face_iterator face_iterator;
typedef Graph_traits::vertex_descriptor vertex_descriptor;
typedef Graph_traits::face_descriptor face_descriptor;
typedef Graph_traits::halfedge_descriptor halfedge_descriptor;

// A model of SurfaceMeshShortestPathVisitor storing simplicies
// using boost::variant
struct Sequence_collector
{
  typedef boost::variant< vertex_descriptor,
                         std::pair<halfedge_descriptor,double>,
                         std::pair<face_descriptor, Barycentric_coordinate> > Simplex;
  std::vector< Simplex > sequence;

  void operator()(halfedge_descriptor he, double alpha)
  {

    sequence.push_back( std::make_pair(he, alpha) );
  }

  void operator()(vertex_descriptor v)
  {
    sequence.push_back( v );
  }

  void operator()(face_descriptor f, Barycentric_coordinate alpha)
  {
    sequence.push_back( std::make_pair(f, alpha) );
  }
};

// A visitor to print what a variant contains using boost::apply_visitor
struct Print_visitor : public boost::static_visitor<> {
  int i;
  Polyhedron_3& g;

  Print_visitor(Polyhedron_3& g) :i(-1), g(g) {}

  void operator()(vertex_descriptor v)
  {
    std::cout << "#" << ++i << " : Vertex : " << get(boost::vertex_index, g)[v] << "\n";
  }

  void operator()(const std::pair<halfedge_descriptor,double>& h_a)
  {
    std::cout << "#" << ++i << " : Edge : " << get(CGAL::halfedge_index, g)[h_a.first] << " , ("
                                            << 1.0 - h_a.second << " , "
                                            << h_a.second << ")\n";
  }

  void operator()(const std::pair<face_descriptor, Barycentric_coordinate>& f_bc)
  {
    std::cout << "#" << ++i << " : Face : " << get(CGAL::face_index, g)[f_bc.first] << " , ("
                                            << f_bc.second[0] << " , "
                                            << f_bc.second[1] << " , "
                                            << f_bc.second[2] << ")\n";
  }
};

int main(int argc, char** argv)
{
  // read input polyhedron
  Polyhedron_3 polyhedron;
  std::ifstream input((argc>1)?argv[1]:"data/elephant.off");
  input >> polyhedron;
  input.close();

  // initialize indices of vertices, halfedges and facets
  CGAL::set_halfedgeds_items_id(polyhedron);

  // pick up a random face
  const size_t randSeed = argc > 2 ? std::atoi(argv[2]) : 7915421;
  CGAL::Random rand(randSeed);
  const int target_face_index = rand.get_int(0, num_faces(polyhedron));
  face_iterator face_it = faces(polyhedron).first;
  std::advance(face_it,target_face_index);
  // ... and define a barycentric coordinate inside the face
  Barycentric_coordinate face_location = {{0.25, 0.5, 0.25}};

  // construct a shortest path query object and add a source point
  Surface_mesh_shortest_path shortest_paths(polyhedron);
  shortest_paths.add_source_point(*face_it, face_location);

  // pick a random target point inside a face
  face_it = faces(polyhedron).first;
  std::advance(face_it, rand.get_int(0, num_faces(polyhedron)));

  // collect the sequence of simplicies crossed by the shortest path
  Sequence_collector sequence_collector;
  shortest_paths.shortest_path_sequence_to_source_points(*face_it, face_location, sequence_collector);

  // print the sequence using the visitor pattern
  Print_visitor print_visitor(polyhedron);
  for (size_t i = 0; i < sequence_collector.sequence.size(); ++i)
    boost::apply_visitor(print_visitor, sequence_collector.sequence[i]);

  return 0;
}


#pragma once

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
#include "grid3d.hpp"
#include <ros/ros.h>

// std
#include <iostream>

using namespace Eigen;
using namespace std;
// Generic functor for Eigen Levenberg-Marquardt minimizer
template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
struct Functor {
    typedef _Scalar Scalar;
    enum {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

    const int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }

    int values() const { return m_values; }
};

struct PoseEstimator : Functor<double> {
    /**
     * Default amount of sensors needed for Eigen templated structure
     * @param numberOfSensors you can however choose any number of sensors here
     */
    PoseEstimator(int numberOfSensors = 4);
    ~PoseEstimator(){
      delete grid;
    }

    /**
     * This is the function that is called in each iteration
     * @param x the pose vector (3 rotational parameters)
     * @param fvec the error function (the difference between the sensor positions)
     * @return
     */
    int operator()(const VectorXd &x, VectorXd &fvec) const;

    VectorXd pose;
    vector<Vector3d> sensor_pos, sensor_angle, sensor_target;
    int numberOfSensors = 4;
    Grid<float> *grid;
    float theta_range, theta_min;
};


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
/**
* @file
* @author Christian Winne
* @ingroup openOR_core_math
*/

#ifndef openOR_core_Math_detail_determinant_impl_hpp
#define openOR_core_Math_detail_determinant_impl_hpp

#include <boost/mpl/assert.hpp>
#include <boost/mpl/void.hpp>
#include <boost/type_traits/is_same.hpp>

#include <openOR/Math/matrix.hpp>

namespace openOR {
	namespace Math {
		namespace Impl {

			template<class Type, int Dim>
			struct Determinant {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					// function has to be implemented

					BOOST_MPL_ASSERT((boost::is_same<Type, boost::mpl::void_>));
				}
			};

			//determinant of 2x2 matrix
			template<class Type>
			struct Determinant<Type, 2> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return Math::get<0, 0>(mat) * Math::get<1, 1>(mat) - Math::get<0, 1>(mat) * Math::get<1, 0>(mat);
				}
			};

			//determinant of 3x3 matrix
			template<class Type>
			struct Determinant<Type, 3> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) -
						Math::get<2, 0>(mat) * Math::get<1, 1>(mat) * Math::get<0, 2>(mat) -
						Math::get<2, 1>(mat) * Math::get<1, 2>(mat) * Math::get<0, 0>(mat) -
						Math::get<2, 2>(mat) * Math::get<1, 0>(mat) * Math::get<0, 1>(mat);
				}
			};
			//determinant of 4x4 matrix
			template<class Type>
			struct Determinant<Type, 4> {
				typename MatrixTraits<Type>::ValueType operator()(const Type& mat) const {
					return   Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 0>(mat) * Math::get<1, 2>(mat) * Math::get<2, 3>(mat) * Math::get<3, 1>(mat) +
						Math::get<0, 0>(mat) * Math::get<1, 3>(mat) * Math::get<2, 1>(mat) * Math::get<3, 2>(mat) +

						Math::get<0, 1>(mat) * Math::get<1, 0>(mat) * Math::get<2, 3>(mat) * Math::get<3, 2>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 1>(mat) * Math::get<1, 3>(mat) * Math::get<2, 2>(mat) * Math::get<3, 0>(mat) +

						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) * Math::get<3, 3>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 1>(mat) * Math::get<2, 3>(mat) * Math::get<3, 0>(mat) +
						Math::get<0, 2>(mat) * Math::get<1, 3>(mat) * Math::get<2, 0>(mat) * Math::get<3, 1>(mat) +

						Math::get<0, 3>(mat) * Math::get<1, 0>(mat) * Math::get<2, 2>(mat) * Math::get<3, 1>(mat) +
						Math::get<0, 3>(mat) * Math::get<1, 1>(mat) * Math::get<2, 0>(mat) * Math::get<3, 2>(mat) +
						Math::get<0, 3>(mat) * Math::get<1, 2>(mat) * Math::get<2, 1>(mat) * Math::get<3, 0>(mat) -

						Math::get<0, 0>(mat) * Math::get<1, 1>(mat) * Math::get<2, 3>(mat) * Math::get<3, 2>(mat) -
						Math::get<0, 0>(mat) * Math::get<1, 2>(mat) * Math::get<2, 1>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 0>(mat) * Math::get<1, 3>(mat) * Math::get<2, 2>(mat) * Math::get<3, 1>(mat) -

						Math::get<0, 1>(mat) * Math::get<1, 0>(mat) * Math::get<2, 2>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 1>(mat) * Math::get<1, 2>(mat) * Math::get<2, 3>(mat) * Math::get<3, 0>(mat) -
						Math::get<0, 1>(mat) * Math::get<1, 3>(mat) * Math::get<2, 0>(mat) * Math::get<3, 2>(mat) -

						Math::get<0, 2>(mat) * Math::get<1, 0>(mat) * Math::get<2, 3>(mat) * Math::get<3, 1>(mat) -
						Math::get<0, 2>(mat) * Math::get<1, 1>(mat) * Math::get<2, 0>(mat) * Math::get<3, 3>(mat) -
						Math::get<0, 2>(mat) * Math::get<1, 3>(mat) * Math::get<2, 1>(mat) * Math::get<3, 0>(mat) -

						Math::get<0, 3>(mat) * Math::get<1, 0>(mat) * Math::get<2, 1>(mat) * Math::get<3, 2>(mat) -
						Math::get<0, 3>(mat) * Math::get<1, 1>(mat) * Math::get<2, 2>(mat) * Math::get<3, 0>(mat) -
						Math::get<0, 3>(mat) * Math::get<1, 2>(mat) * Math::get<2, 0>(mat) * Math::get<3, 1>(mat);
				}
			};

		}
	}
}

#endif


// #include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <numeric>
#include <vector>
#include "kernel.h"
#include "distance.h"
#include "meanshift.h"

using namespace ModelFitting;

// #define EPSILON 0.00000001
// #define CLUSTER_EPSILON 0.5

MeanShift::MeanShift():
        max_iterations(100000),
		max_inner_iterations(10000)
{

}

MeanShift::MeanShift(
    double (*_distance_metirc)(cv::Mat &, cv::Mat &, const int &) ,
    double (*_kernel)(const double &, const double &)):
    MeanShift()
{
    this->calculate_distance = _distance_metirc;
    this->calculate_kernel = _kernel;
}

MeanShift::~MeanShift()
{
}

void MeanShift::cluster(
    cv::InputArray _data_pts,
    cv::OutputArray _clusters_centers,
    std::vector<std::vector<int>> &_cluster_pts,
    double kernel_bandwidth)
{
    // initialization
    const cv::Mat data_pts = _data_pts.getMat();
    const int dim_num = data_pts.cols;
    const int pts_num = data_pts.rows;

    std::cout << dim_num << ", " << pts_num << std::endl;
    // std::cout << data_pts << std::endl;
    int cluster_num = 0;
    const double bandwidth_sq = kernel_bandwidth * kernel_bandwidth;
    const double hald_bandwith = static_cast<double>(kernel_bandwidth/2);
    const double stop_threshold = static_cast<double>(1e-3*kernel_bandwidth);

    std::vector<int> init_pts_inds(pts_num); // initial points indices
    std::iota(init_pts_inds.begin(), init_pts_inds.end(), 0);

    // double stop_threshold = 1e-3 * kernel_bandwidth; // need casting

    std::vector<int> been_visited(pts_num,0);
    std::vector<cv::Mat> cluster_votes;//(pts_num,std::vector<int>(pts_num, 0));
    cv::Mat current_cluster_votes;
    std::vector<cv::Mat> clusters_centers;

    cv::RNG gen;
    int tmp_idx, pt_idx;
    cv::Mat current_mean, old_mean;
    std::vector<int> current_cluster_members(0);

    // Iterate over all data points
    int iters = 0;
    while (init_pts_inds.size() && iters++ < max_iterations)// && num pf iterations)
    {
        tmp_idx = gen.uniform(0,init_pts_inds.size());
        pt_idx = init_pts_inds[tmp_idx];
        //
        current_mean = data_pts.row(pt_idx);
        current_cluster_members.resize(0);
        current_cluster_votes = cv::Mat_<int>(1,pts_num, 0);//.resize(pts_num, 0);
        //
        int inner_iters = 0;
        while(inner_iters++ < max_inner_iterations)
        {
            // std::cout << max_inner_iterations << std::endl;
            // mean shift procedure
            // 1. Compute the mean shift vector m
            // 2. translate the old mean
            // 1.1 iterate over all points and find indices of points
            //     that its distances from the current mean is less than the bandwidth
            // 1.2 vote for those points as clusters
            // 1.3 pass those points ||(current_mean - pts_i)/bandwidth|| to the kernel
            // 1.4 sum the return values of the kernel
            old_mean = current_mean.clone();
            current_mean = cv::Mat::zeros(1, data_pts.cols, data_pts.type());

            meanshift(
                data_pts,
                pts_num,
                dim_num,
                old_mean,
                current_mean,
                current_cluster_votes,
                current_cluster_members,
                been_visited,
                kernel_bandwidth);

            // checking stopping condition for current cluster
            if(this->calculate_distance(current_mean, old_mean, dim_num) < stop_threshold)
            {
                // std::cout << this->calculate_distance(current_mean, old_mean, dim_num) << std::endl;
                // check for merge posibilities
                int merge_with = -1;
                for(size_t c = 0; c < cluster_num; c++)
                {
                    // distance from posible new cluster max to old cluster max
                    double dist_to_other = this->calculate_distance(current_mean, clusters_centers.at(c), dim_num);
                    if(dist_to_other < hald_bandwith)
                    {
                        merge_with = c;
                        break;
                    }
                }

                if (merge_with > -1)
                {
                    clusters_centers.at(merge_with) = 0.5 * (current_mean + clusters_centers.at(merge_with));
                    cluster_votes.at(merge_with) += current_cluster_votes;
                }
                else
                {
                    // increment cluster numbers
                    ++cluster_num;
                    // record the current mean
                    clusters_centers.push_back(current_mean);
                    cluster_votes.push_back(current_cluster_votes);
                }
                break;
            }
        }

        // remove visited points
        init_pts_inds.erase(
            std::remove_if(
                init_pts_inds.begin(),
                init_pts_inds.end(),
                [been_visited](int pts_idx){ return been_visited.at(pts_idx) == 1;}
            ),
            init_pts_inds.end()
        );
    }

    // assigning points to clusters
    std::vector<int> pts_cluster_votes(pts_num, 0);
    std::vector<int> pts_cluster_idx(pts_num, -1);

    for (size_t pt_idx = 0; pt_idx < pts_num; pt_idx++)
    {
        for (size_t cluster_idx = 0; cluster_idx < cluster_num; cluster_idx++)
        {
            if(pts_cluster_votes.at(pt_idx) < cluster_votes.at(cluster_idx).at<int>(pt_idx))
            {
                pts_cluster_votes.at(pt_idx) = cluster_votes.at(cluster_idx).at<int>(pt_idx);
                pts_cluster_idx.at(pt_idx) = cluster_idx;
            }
        }

    }

    // refactor centers as Mat(center_num, dim_num)
    _clusters_centers.create(static_cast<int>(cluster_votes.size()), dim_num, data_pts.type());
	cv::Mat const &clusters_ref = _clusters_centers.getMatRef();
	for (auto i = 0; i < clusters_centers.size(); ++i)
		clusters_centers[i].copyTo(clusters_ref.row(i));

    // refactor cluster points as a vector of vectors of points indices
    _cluster_pts.resize(cluster_votes.size());
	for (size_t i = 0; i < pts_cluster_idx.size(); ++i)
	{
		if (pts_cluster_idx[i] == -1)
			continue;
		_cluster_pts[pts_cluster_idx[i]].push_back(i);
	}

    return;
}

void MeanShift::meanshift(
    const cv::Mat &_data_pts,
    const int _pts_num,
    const int _dim_num,
    cv::Mat &_old_mean,
    cv::Mat &_new_mean,
    cv::Mat &_current_cluster_votes,
    std::vector<int> &_current_culster_members,
    std::vector<int> &_been_visited,
    const double _kernel_bandwidth)
{
    // mean shift procedure
    // 1. Compute the mean shift vector m
    // 2. translate the old mean
    // 1.1 iterate over all points and find indices of points
    //     that its distances from the current mean is less than the bandwidth
    double distance_val = 0;
    double kernel_val = 0;
    double normalizer = 0;
    int window_members_num = 0;
    cv::Mat current_pt;
    for (size_t idx = 0; idx < _pts_num; idx++)
    {
        // calculate the distance
        current_pt = _data_pts.row(idx);
        distance_val = this->calculate_distance(_old_mean, current_pt, _dim_num);

        if(distance_val <= _kernel_bandwidth)
        {
        // 1.2 vote for those points as clusters
            ++_current_cluster_votes.at<int>(idx);
        // 1.3 pass those points ||(current_mean - pts_i)/bandwidth|| to the kernel
            kernel_val = this->calculate_kernel(distance_val, _kernel_bandwidth);
            ++window_members_num;
        // 1.4 sum the return values of the kernel
            _new_mean += _data_pts.row(idx) * kernel_val;
            normalizer += kernel_val;
            _been_visited.at(idx) = 1;
            _current_culster_members.push_back(idx);

        }
        idx++;

    }
    if(window_members_num == 0)
    {
        _new_mean = _old_mean;
        return;
    }
    _new_mean /= normalizer;
}


#include "registration.h"
#include <Eigen/Cholesky>
#include <vector>

struct SPoint2D
{
	double x[2];
};

double ComputeAngle(const Eigen::Vector2d& input_one, const Eigen::Vector2d& input_two)
{
	//compute theta
	//when e_im.dot(e_i) = 1.0, theta = pi;
	const double r_sinT = input_one(0)*input_two(1) - input_one(1)*input_two(0);
	const double r_cosT = -input_one.dot(input_two);
	return atan2(r_sinT, r_cosT);
}

double sgn(const double& x)
{
	return (x >= 0) ? 1.0 : -1.0;
}

double ComputeOmegaFromAngle(const double& theta)
{
	const double sinT = sin(theta);
	const double cosT = cos(theta);
	const double tan_phi_over_2 = sgn(sinT) * sqrt((1.0+cosT)/(max(0.0, 1.0-cosT) + 1.0e-10));
	return 2.0 * tan_phi_over_2;
}

double computeOmega(const Eigen::Vector2d& input_one, const Eigen::Vector2d& input_two)
{
	//compute 2.0 * tan(phi / 2.0)
	//phi = pi - theta
	return ComputeOmegaFromAngle(ComputeAngle(input_one, input_two));
}

double sampleDistanceField(const SImage<double, 1>& in_df, const SRegion& in_region, const Eigen::Vector2d& x)
{
	assert(in_df.width == in_df.height);
	assert(in_region.right-in_region.left == in_region.top-in_region.bottom);

	Eigen::Vector2d proj_x = x;
	proj_x(0) = std::max(in_region.left, std::min(in_region.right, x(0)));
	proj_x(1) = std::max(in_region.bottom, std::min(in_region.top, x(1)));

	const double dist_scale = (in_region.right-in_region.left) / in_df.width;

	double fi = (proj_x(0) - in_region.left) * in_df.width / (in_region.right - in_region.left);
	double fj = (proj_x(1) - in_region.bottom) * in_df.height / (in_region.top - in_region.bottom);
	double _i = floor(fi);
	double _j = floor(fj);

	int i = std::max(0, std::min(in_df.width-1, int(_i)));
	int j = std::max(0, std::min(in_df.height-1, int(_j)));
	int ip = std::min(in_df.width-1, i+1);
	int jp = std::min(in_df.height-1, j+1);

	double s = fi - _i;
	double t = fj - _j;

	double dist_proj = (1.0 - t) * (s * in_df.ptr[j*in_df.width+ip] + (1.0-s) * in_df.ptr[j*in_df.width+i])
		+ t * (s * in_df.ptr[jp*in_df.width+ip] + (1.0-s) * in_df.ptr[jp*in_df.width+i]);

	return dist_proj * dist_scale + (proj_x - x).norm();
}

double integrateDF2OverSegment(const SParameters* in_params, const Eigen::Vector2d& x1, const Eigen::Vector2d& x2, const double rest_length)
{
	double tot = 0.0;
	const double len = rest_length / in_params->substep_fit;

	//printf("integ_DF: ");
	for(int i=0; i<in_params->substep_fit; i++)
	{
		const Eigen::Vector2d& p1 = x1 + (x2-x1) * double(i)/double(in_params->substep_fit);
		const Eigen::Vector2d& p2 = x1 + (x2-x1) * double(i+1)/double(in_params->substep_fit);

		const double d1 = sampleDistanceField(in_params->df, in_params->region, p1);
		const double d2 = sampleDistanceField(in_params->df, in_params->region, p2);

		//printf("[%f, %f, %f], ", d1, d2, len);

		const double e1 = 0.5 * (exp(d1) + exp(-d1)) - 1.0;
		const double e2 = 0.5 * (exp(d2) + exp(-d2)) - 1.0;

		//tot += 0.5 * (d1*d1+d2*d2) * len;
		tot += 0.5 * (e1*e1+e2*e2) * len;
	}
	//printf("\n");

	return tot;
}

inline double computeSegmentElasticEnergy(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2, const double& YA, const double& rest_length)
{
	const Eigen::Vector2d diff = p1 - p2;
	return 0.5 * YA * rest_length
		* (diff.norm() / rest_length - 1)
		* (diff.norm() / rest_length - 1);
}

inline double computeSegmentBendingEnergy(const Eigen::Vector2d& pp, const Eigen::Vector2d& pc, const Eigen::Vector2d& pn,
	const double& alpha, const double& rest_length_p, const double& rest_length_n, const double& theta)
{
	const Eigen::Vector2d e_im = pc - pp;
	const Eigen::Vector2d e_i = pn - pc;

	/*
	double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
	double omega_bar = 2.0 * fabs(tan(phi*0.5));
	//*/
	double omega = computeOmega(e_im, e_i);
	double omega_bar = ComputeOmegaFromAngle(theta);
	return alpha * (omega - omega_bar) * (omega - omega_bar) / (rest_length_p + rest_length_n);
}

double computeEnergy_elastic(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_curve->nVertices;
		energy += computeSegmentElasticEnergy(in_position.col(ip), in_position.col(i), in_params->YA, in_curve->restLengths(i));
	}

	return energy;
}

double computeEnergy_bending(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position)
{
	int nAngles = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 2;

	double energy = 0.0;

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_curve->closed ?
			(i + in_curve->nVertices - 1) % in_curve->nVertices : i;
		int ii = in_curve->closed ? i : i + 1;
		int iip = in_curve->closed ? (i + 1) % in_curve->nVertices : i + 2;

		double restL_m = in_curve->closed ? in_curve->restLengths((i + in_curve->nVertices - 1) % in_curve->nVertices) :
			in_curve->restLengths(i);
		double restL = in_curve->closed ? in_curve->restLengths(i) :
			in_curve->restLengths(i+1);

		energy += computeSegmentBendingEnergy(in_position.col(iim), in_position.col(ii), in_position.col(iip),
			in_params->alpha, restL_m, restL, in_curve->restAngles(i));
	}

	return energy;
}

double computeEnergy_fit(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	double energy = 0.0;

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_params, in_position.col(i), in_position.col(ip), in_curve->restLengths(i));
		energy += 0.5 * in_params->fit * int_df_seg;
	}

	return energy;
}

double ComputeEnergy(const SParameters* in_params, const SCurve* in_curve, const SVar& in_vars)
{
	const double E_elastic = computeEnergy_elastic(in_params, in_curve, in_vars.pos);
	const double E_bending = computeEnergy_bending(in_params, in_curve, in_vars.pos);
	const double E_fit = computeEnergy_fit(in_params, in_curve, in_vars.pos);
	return E_elastic + E_bending + E_fit;
}

void compute_f_elastic(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_curve->nVertices == in_position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_curve->nVertices;
		const Eigen::Vector2d diff = in_position.col(ip) - in_position.col(i);
		io_f(i+offset_row) = sqrt(0.5 * in_params->YA * in_curve->restLengths(i)) * (diff.norm() / in_curve->restLengths(i) - 1);
	}
}

void compute_f_bending(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, Eigen::VectorXd& io_f, int offset_row)
{
	int nAngles = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 2;

	assert(io_f.rows() >= nAngles + offset_row);
	assert(in_curve->nVertices == in_position.cols());

	for(int i=0; i<nAngles; i++)
	{
		int iim = in_curve->closed ?
			(i + in_curve->nVertices - 1) % in_curve->nVertices : i;
		int ii = in_curve->closed ? i : i + 1;
		int iip = in_curve->closed ? (i + 1) % in_curve->nVertices : i + 2;

		double restL_m = in_curve->closed ? in_curve->restLengths((i + in_curve->nVertices - 1) % in_curve->nVertices) :
			in_curve->restLengths(i);
		double restL = in_curve->closed ? in_curve->restLengths(i) :
			in_curve->restLengths(i+1);

		const Eigen::Vector2d e_im = in_position.col(ii) - in_position.col(iim);
		const Eigen::Vector2d e_i = in_position.col(iip) - in_position.col(ii);

		/*
		double omega = 2.0 * fabs(e_im(0)*e_i(1)-e_im(1)*e_i(0)) / (e_im.norm()*e_i.norm() + e_im.dot(e_i));
		double omega_bar = 2.0 * fabs(tan(in_curve->restAngles(i)*0.5));
		//*/
		double omega = computeOmega(e_im, e_i);
		double omega_bar = ComputeOmegaFromAngle(in_curve->restAngles(i));
		io_f(i+offset_row) = sqrt(in_params->alpha/(restL_m + restL)) * (omega - omega_bar);
	}
}

void compute_f_fit(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, Eigen::VectorXd& io_f, int offset_row)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	assert(io_f.rows() >= nSegs + offset_row);
	assert(in_curve->nVertices == in_position.cols());

	//printf("int_seg: ");
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % in_curve->nVertices;
		double int_df_seg = integrateDF2OverSegment(in_params, in_position.col(i), in_position.col(ip), in_curve->restLengths(i));
		//printf("%f, ", int_df_seg);
		io_f(i+offset_row) = sqrt(in_params->fit * 0.5) * sqrt(int_df_seg);
	}
	//printf("\n");
}

void Compute_f(const SParameters* in_params, const SCurve* in_curve, const SVar& in_vars, Eigen::VectorXd& io_f)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;
	int nAngles = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 2;

	int nElems = nSegs + nAngles + nSegs;

	assert(io_f.rows() == nElems);
	assert(in_curve->nVertices == in_vars.pos.cols());
	assert(in_curve->nVertices == in_vars.pos.cols());

	io_f.setZero();

	compute_f_elastic(in_params, in_curve, in_vars.pos, io_f, 0);
	compute_f_bending(in_params, in_curve, in_vars.pos, io_f, nSegs);
	compute_f_fit(in_params, in_curve, in_vars.pos, io_f, nSegs+nAngles);

	/*
	printf("f: [");
	for(int i=0; i<nElems; i++)
	{
		if(i<nElems-1) printf("%f, ", io_f(i));
		else printf("%f]\n", io_f(i));
	}
	//*/
}

void computeNumericalDerivative_elastic(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, double epsilon, Eigen::MatrixXd& io_jacobian, int offset_row)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	assert(io_jacobian.rows() >= nSegs + offset_row);
	assert(io_jacobian.cols() == in_curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dJ/dxi, dJ/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_curve->nVertices;

		const Eigen::Vector2d xi = in_position.col(i);
		const Eigen::Vector2d xip = in_position.col(ip);

		const Eigen::Vector2d diff0 = xip - xi;
		const double fi = sqrt(in_params->YA * in_curve->restLengths(i) * 0.5) * (diff0.norm() / in_curve->restLengths(i) - 1);

		//dJ/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const Eigen::Vector2d diff1 = xip - xi_dx;
		const double fi_i_dx = sqrt(in_params->YA * in_curve->restLengths(i) * 0.5) * (diff1.norm() / in_curve->restLengths(i) - 1);
		io_jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d diff2 = xip - xi_dy;
		const double fi_i_dy = sqrt(in_params->YA * in_curve->restLengths(i) * 0.5) * (diff2.norm() / in_curve->restLengths(i) - 1);
		io_jacobian(i+offset_row, i+in_curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dJ/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const Eigen::Vector2d diff3 = xip_dx - xi;
		const double fi_ip_dx = sqrt(in_params->YA * in_curve->restLengths(i) * 0.5) * (diff3.norm() / in_curve->restLengths(i) - 1);
		io_jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dJ/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const Eigen::Vector2d diff4 = xip_dy - xi;
		const double fi_ip_dy = sqrt(in_params->YA * in_curve->restLengths(i) * 0.5) * (diff4.norm() / in_curve->restLengths(i) - 1);
		io_jacobian(i+offset_row, ip+in_curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_bending(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, double epsilon, Eigen::MatrixXd& io_jacobian, int offset_row)
{
	int nAngles = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 2;

	assert(io_jacobian.rows() >= nAngles + offset_row);
	assert(io_jacobian.cols() == in_curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dB/dxi, dB/dyi
	for(int i=0; i<nAngles; i++)
	{
		int iim = in_curve->closed ?
			(i + in_curve->nVertices - 1) % in_curve->nVertices : i;
		int ii = in_curve->closed ? i : i + 1;
		int iip = in_curve->closed ? (i + 1) % in_curve->nVertices : i + 2;

		double restL_m = in_curve->restLengths(iim);
		double restL = in_curve->restLengths(ii);

		const Eigen::Vector2d xim = in_position.col(iim);
		const Eigen::Vector2d xi = in_position.col(ii);
		const Eigen::Vector2d xip = in_position.col(iip);

		const Eigen::Vector2d xim_dx = xim + dx; const Eigen::Vector2d xim_dy = xim + dy;
		const Eigen::Vector2d xi_dx = xi + dx; const Eigen::Vector2d xi_dy = xi + dy;
		const Eigen::Vector2d xip_dx = xip + dx; const Eigen::Vector2d xip_dy = xip + dy;

		const Eigen::Vector2d e_prev = xi - xim;
		const Eigen::Vector2d e_next = xip - xi;

		const Eigen::Vector2d e_prev_im_dx = xi - xim_dx; const Eigen::Vector2d e_prev_im_dy = xi - xim_dy;
		const Eigen::Vector2d e_prev_i_dx = xi_dx - xim; const Eigen::Vector2d e_prev_i_dy = xi_dy - xim;

		const Eigen::Vector2d e_next_i_dx = xip - xi_dx; const Eigen::Vector2d e_next_i_dy = xip - xi_dy;
		const Eigen::Vector2d e_next_ip_dx = xip_dx - xi; const Eigen::Vector2d e_next_ip_dy = xip_dy - xi;

		/*
		const double omega = 2.0 * fabs(e_prev(0)*e_next(1)-e_prev(1)*e_next(0)) / (e_prev.norm()*e_next.norm() + e_prev.dot(e_next));

		const double omega_im_dx = 2.0 * fabs(e_prev_im_dx(0)*e_next(1)-e_prev_im_dx(1)*e_next(0)) / (e_prev_im_dx.norm()*e_next.norm() + e_prev_im_dx.dot(e_next));
		const double omega_im_dy = 2.0 * fabs(e_prev_im_dy(0)*e_next(1)-e_prev_im_dy(1)*e_next(0)) / (e_prev_im_dy.norm()*e_next.norm() + e_prev_im_dy.dot(e_next));
		const double omega_i_dx = 2.0 * fabs(e_prev_i_dx(0)*e_next_i_dx(1)-e_prev_i_dx(1)*e_next_i_dx(0)) / (e_prev_i_dx.norm()*e_next_i_dx.norm() + e_prev_i_dx.dot(e_next_i_dx));
		const double omega_i_dy = 2.0 * fabs(e_prev_i_dy(0)*e_next_i_dy(1)-e_prev_i_dy(1)*e_next_i_dy(0)) / (e_prev_i_dy.norm()*e_next_i_dy.norm() + e_prev_i_dy.dot(e_next_i_dy));
		const double omega_ip_dx = 2.0 * fabs(e_prev(0)*e_next_ip_dx(1)-e_prev(1)*e_next_ip_dx(0)) / (e_prev.norm()*e_next_ip_dx.norm() + e_prev.dot(e_next_ip_dx));
		const double omega_ip_dy = 2.0 * fabs(e_prev(0)*e_next_ip_dy(1)-e_prev(1)*e_next_ip_dy(0)) / (e_prev.norm()*e_next_ip_dy.norm() + e_prev.dot(e_next_ip_dy));
		//*/

		const double omega = computeOmega(e_prev, e_next);

		const double omega_im_dx = computeOmega(e_prev_im_dx, e_next);
		const double omega_im_dy = computeOmega(e_prev_im_dy, e_next);
		const double omega_i_dx = computeOmega(e_prev_i_dx, e_next_i_dx);
		const double omega_i_dy = computeOmega(e_prev_i_dy, e_next_i_dy);
		const double omega_ip_dx = computeOmega(e_prev, e_next_ip_dx);
		const double omega_ip_dy = computeOmega(e_prev, e_next_ip_dy);

		const double fi = sqrt(in_params->alpha/(restL_m + restL)) * omega; //omega_bar will be cancelled out, so omit it
		const double fi_im_dx = sqrt(in_params->alpha/(restL_m + restL)) * omega_im_dx;
		const double fi_im_dy = sqrt(in_params->alpha/(restL_m + restL)) * omega_im_dy;
		const double fi_i_dx = sqrt(in_params->alpha/(restL_m + restL)) * omega_i_dx;
		const double fi_i_dy = sqrt(in_params->alpha/(restL_m + restL)) * omega_i_dy;
		const double fi_ip_dx = sqrt(in_params->alpha/(restL_m + restL)) * omega_ip_dx;
		const double fi_ip_dy = sqrt(in_params->alpha/(restL_m + restL)) * omega_ip_dy;

		io_jacobian(i+offset_row, iim) = (fi_im_dx - fi) / epsilon;
		io_jacobian(i+offset_row, iim+in_curve->nVertices) = (fi_im_dy - fi) / epsilon;

		io_jacobian(i+offset_row, ii) = (fi_i_dx - fi) / epsilon;
		io_jacobian(i+offset_row, ii+in_curve->nVertices) = (fi_i_dy - fi) / epsilon;

		io_jacobian(i+offset_row, iip) = (fi_ip_dx - fi) / epsilon;
		io_jacobian(i+offset_row, iip+in_curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void computeNumericalDerivative_fit(const SParameters* in_params, const SCurve* in_curve, const Eigen::Matrix2Xd& in_position, double epsilon, Eigen::MatrixXd& io_jacobian, int offset_row)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;

	assert(io_jacobian.rows() >= nSegs + offset_row);
	assert(io_jacobian.cols() == in_curve->nVertices * 2);

	const Eigen::Vector2d dx(epsilon, 0.0);
	const Eigen::Vector2d dy(0.0, epsilon);

	//dfit/dxi, dfit/dyi
	for(int i=0; i<nSegs; i++)
	{
		//l_i is a function of x_i, x_{i+1}, y_i and y_{i+1}
		int ip = (i+1) % in_curve->nVertices;

		const Eigen::Vector2d xi = in_position.col(i);
		const Eigen::Vector2d xip = in_position.col(ip);
		const double restL = in_curve->restLengths(i);

		const double fi = sqrt(in_params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_params, xi, xip, restL));

		//dfit/dxi_i
		const Eigen::Vector2d xi_dx = xi + dx;
		const double fi_i_dx = sqrt(in_params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_params, xi_dx, xip, restL));
		io_jacobian(i+offset_row, i) = (fi_i_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xi_dy = xi + dy;
		const double fi_i_dy = sqrt(in_params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_params, xi_dy, xip, restL));
		io_jacobian(i+offset_row, i+in_curve->nVertices) = (fi_i_dy - fi) / epsilon;

		//dfit/dxi_ip
		const Eigen::Vector2d xip_dx = xip + dx;
		const double fi_ip_dx = sqrt(in_params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_params, xi, xip_dx, restL));
		io_jacobian(i+offset_row, ip) = (fi_ip_dx - fi) / epsilon;

		//dfit/dyi_i
		const Eigen::Vector2d xip_dy = xip + dy;
		const double fi_ip_dy = sqrt(in_params->fit * 0.5) * sqrt(integrateDF2OverSegment(in_params, xi, xip_dy, restL));
		io_jacobian(i+offset_row, ip+in_curve->nVertices) = (fi_ip_dy - fi) / epsilon;
	}
}

void ComputeNumericalDerivative(const SParameters* in_params, const SCurve* in_curve, const SVar& in_vars, double epsilon, Eigen::MatrixXd& io_jacobian)
{
	int nSegs = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 1;
	int nAngles = in_curve->closed ? in_curve->nVertices : in_curve->nVertices - 2;

	//assert(io_jacobian.rows() == nSegs + nAngles + nSegs);
	//assert(io_jacobian.cols() == in_curve->nVertices * 2);
	//assert(in_vars.pos.cols() == in_curve->nVertices);
	//assert(in_vars.conf.cols() == in_curve->nVertices);

	io_jacobian.setZero();

	computeNumericalDerivative_elastic(in_params, in_curve, in_vars.pos, epsilon, io_jacobian, 0);
	computeNumericalDerivative_bending(in_params, in_curve, in_vars.pos, epsilon, io_jacobian, nSegs);
	computeNumericalDerivative_fit(in_params, in_curve, in_vars.pos, epsilon, io_jacobian, nSegs+nAngles);

	/*
	printf("B: [\n");
	for(int j=0; j<io_jacobian.rows(); j++)
	{
		printf("[");
		for(int i=0; i<io_jacobian.cols(); i++)
		{
			if(i<io_jacobian.cols()-1) printf("%f, ", io_jacobian(j, i));
			else printf("%f],\n", io_jacobian(j, i));
		}
	}
	printf("]\n");
	//*/
}

void UpdateRestLength(const Eigen::Matrix2Xd& in_position, SCurve* io_curve)
{
	int nSegs = io_curve->closed ? io_curve->nVertices : io_curve->nVertices - 1;
	assert(io_curve->nVertices == in_position.cols());

	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_curve->nVertices;
		const Eigen::Vector2d diff = in_position.col(ip) - in_position.col(i);
		io_curve->restLengths(i) = diff.norm();
	}
}

void UpdateCurveSubdivision(const SParameters* in_params, SVar& io_initial_vars, SVar& io_vars, SCurve* io_curve, SSolverVars& io_solver_vars)
{
	int nSegs = io_curve->closed ? io_curve->nVertices : io_curve->nVertices - 1;
	//assert(io_curve->nVertices == in_position.cols());

	std::vector<SPoint2D> pos;
	std::vector<double> angles;
	std::vector<int> vids;

	bool subdivided = false;

	for(int i=0; i<nSegs; i++)
	{
		SPoint2D p; p.x[0] = io_vars.pos.col(i)(0); p.x[1] = io_vars.pos.col(i)(1);
		pos.push_back(p);
		vids.push_back(io_curve->vertexIDs(i));

		if(i!=0 || io_curve->closed) angles.push_back(io_curve->restAngles(i));

		int ip = (i+1) % io_curve->nVertices;
		const Eigen::Vector2d diff = io_vars.pos.col(ip) - io_vars.pos.col(i);
		if(diff.norm() > in_params->refLength)
		{
			SPoint2D p;
			p.x[0] = (io_vars.pos.col(ip)(0) + io_vars.pos.col(i)(0)) * 0.5;
			p.x[1] = (io_vars.pos.col(ip)(1) + io_vars.pos.col(i)(1)) * 0.5;
			pos.push_back(p);
			//angles.push_back((io_curve->restAngles(i) + io_curve->restAngles(ip)) * 0.5);
			angles.push_back(PI);
			vids.push_back(-1);
			subdivided = true;
		}
	}

	if(!io_curve->closed)
	{
		int ilast = io_curve->nVertices-1;
		SPoint2D p; p.x[0] = io_vars.pos.col(ilast)(0); p.x[1] = io_vars.pos.col(ilast)(1);
		pos.push_back(p);
		vids.push_back(io_curve->vertexIDs(ilast));
	}

	io_curve->nVertices = pos.size();
	nSegs = io_curve->closed ? io_curve->nVertices : io_curve->nVertices - 1;
	io_curve->restLengths.resize(nSegs);
	int nAngles = io_curve->closed ? io_curve->nVertices : io_curve->nVertices - 2;
	io_curve->restAngles.resize(nAngles);
	io_curve->vertexIDs.resize(io_curve->nVertices);

	resize(io_vars, io_curve->nVertices);
	resize(io_initial_vars, io_curve->nVertices);

	for(int i=0; i<io_curve->nVertices; i++)
	{
		io_vars.pos.col(i)(0) = pos[i].x[0];
		io_vars.pos.col(i)(1) = pos[i].x[1];
		io_vars.conf(i) = 0.0;
		io_curve->vertexIDs(i) = vids[i];
	}

	nSegs = io_curve->closed ? io_curve->nVertices : io_curve->nVertices - 1;
	for(int i=0; i<nSegs; i++)
	{
		int ip = (i+1) % io_curve->nVertices;
		const Eigen::Vector2d diff = io_vars.pos.col(ip) - io_vars.pos.col(i);
		io_curve->restLengths(i) = diff.norm();
	}

	for(int i=0; i<nAngles; i++)
		io_curve->restAngles(i) = angles[i];

	io_initial_vars = io_vars;

	initSolverVars(in_params, io_curve, io_initial_vars, io_solver_vars, true);
}

bool SecantLMMethodSingleUpdate(const SParameters* in_params, const SCurve* in_curve, const SVar& in_initial_vars, SSolverVars& io_solver_vars, SVar& solution)
{
	if(io_solver_vars.found || io_solver_vars.k > in_params->kmax)
		return true;

	io_solver_vars.k++;
	io_solver_vars.A_muI = io_solver_vars.B.transpose() * io_solver_vars.B + io_solver_vars.mu * io_solver_vars.I;
	io_solver_vars.h = io_solver_vars.A_muI.ldlt().solve(-io_solver_vars.g);

	if(io_solver_vars.h.norm() <= in_params->epsilon_2 * (io_solver_vars.x.pos.norm() + in_params->epsilon_2))
		io_solver_vars.found = true;
	else
	{
		for(int q=0; q<io_solver_vars.nV; q++)
		{
			io_solver_vars.xnew.pos(0, q) = io_solver_vars.x.pos(0, q) + io_solver_vars.h(q);
			io_solver_vars.xnew.pos(1, q) = io_solver_vars.x.pos(1, q) + io_solver_vars.h(q + io_solver_vars.nV);
		}
	}

	double Fnew = 0.5 * ComputeEnergy(in_params, in_curve, io_solver_vars.xnew);
	double F = 0.5 * ComputeEnergy(in_params, in_curve, io_solver_vars.x);

	double gain_denom = - io_solver_vars.h.dot(io_solver_vars.B.transpose() * io_solver_vars.f)
		- 0.5 * io_solver_vars.h.dot(io_solver_vars.B.transpose() * io_solver_vars.B * io_solver_vars.h);
	double gain = (F - Fnew) / gain_denom;

	if(gain > 0)
	{
		io_solver_vars.x = io_solver_vars.xnew;
		Compute_f(in_params, in_curve, io_solver_vars.x, io_solver_vars.f);
		ComputeNumericalDerivative(in_params, in_curve, io_solver_vars.x, io_solver_vars.epsilon, io_solver_vars.B);
		io_solver_vars.g = io_solver_vars.B.transpose() * io_solver_vars.f;
		io_solver_vars.found = (io_solver_vars.g.lpNorm<Eigen::Infinity>() <= in_params->epsilon_1);
		printf("k: %d, gain: %f, |g|_inf: %f\n", io_solver_vars.k, gain, io_solver_vars.g.lpNorm<Eigen::Infinity>());
		io_solver_vars.mu = io_solver_vars.mu * std::max(1.0/3.0, 1.0 - (2.0 * gain - 1.0) * (2.0 * gain - 1.0) * (2.0 * gain - 1.0));
		io_solver_vars.nu = 2.0;
	}
	else
	{
		io_solver_vars.mu = io_solver_vars.mu * io_solver_vars.nu;
		io_solver_vars.nu = io_solver_vars.nu * 2.0;
	}

	if(io_solver_vars.found)
		printf("found in %d steps\n", io_solver_vars.k);

	solution = io_solver_vars.x;

	return io_solver_vars.found || io_solver_vars.k > in_params->kmax;
}

void ShowFeaturePoints(const SCurve* in_curve, const SVar& solution)
{
	printf("Feature points:\n");
	for(int i=0; i<in_curve->nVertices; i++)
	{
		if(in_curve->vertexIDs(i) >= 0)
		{
			printf("%d: %f, %f\n", in_curve->vertexIDs(i), solution.pos.col(i).x(), solution.pos.col(i).y());
		}
	}
}

void SecantLMMethod(const SParameters* in_params, SCurve* in_curve, SVar& in_initial_vars, SSolverVars& io_solver_vars, SVar& solution)
{
	initSolverVars(in_params, in_curve, in_initial_vars, io_solver_vars);

	while(1)
	{
		if(SecantLMMethodSingleUpdate(in_params, in_curve, in_initial_vars, io_solver_vars, solution))
			break;
		UpdateCurveSubdivision(in_params, in_initial_vars, solution, in_curve, io_solver_vars);
	}

	printf("found in %d steps\n", io_solver_vars.k);
	solution = io_solver_vars.x;
	ShowFeaturePoints(in_curve, solution);
}


///////////////////////////////////////////////////////////////////////////////
// chi_square_summand.hpp                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_COMMON_CHISQ_SUMMAND_HPP_ER_2010
#include <boost/numeric/conversion/converter.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chi_square_statistic{

    // The sum over all cells of this quantity is a fraction, f, of Pearson's
    // Chi-square statistic. If (expected, observed) are expressed in counts,
    // f = 1. If they are expressed in frequencies, f = 1/n, where n is the
    // total number of counts.
    template<typename T1,typename T2,typename T3>
    T1  chi_square_summand(const T2& expected, const T3& observed)
    {
        typedef boost::numeric::converter<T1,T2> conv2_;
        typedef boost::numeric::converter<T1,T3> conv3_;
        T1 summand = conv2_::convert( expected ) - conv3_::convert( observed );
        summand *= summand;
        return summand / conv2_::convert( expected );
    }

}// pearson_chi_square_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>

#include <boost/numeric/mtl/mtl.hpp>

int main()
{
    using namespace mtl;

    dense2D<int> A(2, 2), B(2, 2), C(4, 4);

    for (size_t r= 0; r < 2; ++r)
        for (size_t c= 0; c < 2; ++c) {
            A[r][c]= (r+1) * 10 + c+1;
            B[r][c]= (r+1) * 1000 + (c+1) * 100;
        }

    C= kron(A, B);
    std::cout << "kron(A, B) is\n" << C;

    MTL_THROW_IF(C[0][0] != 12100, mtl::runtime_error("Wrong value in C[0][0]"));
    MTL_THROW_IF(C[3][3] != 48400, mtl::runtime_error("Wrong value in C[3][3]"));

    return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXd X = MatrixXd::Random(4,4);
MatrixXd A = X * X.transpose();
cout << "Here is a random positive-definite matrix, A:" << endl << A << endl << endl;

SelfAdjointEigenSolver<MatrixXd> es(A);
MatrixXd sqrtA = es.operatorSqrt();
cout << "The square root of A is: " << endl << sqrtA << endl;
cout << "If we square this, we get: " << endl << sqrtA*sqrtA << endl;

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  Distributed under the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt
//  or copy at http://www.boost.org/LICENSE_1_0.txt)

//! \file
//!\brief Tests for cbrt(fixed_point) round::fastest. Along the way, also test numerous fixed_point arithmetic operations.

#define BOOST_TEST_MODULE test_negatable_math_cbrt_fastest
#define BOOST_LIB_DIAGNOSTIC

#include <cmath>
#include <iomanip>
#include <iostream>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/special_functions/cbrt.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType>
  const FixedPointType& tolerance_maker(const int fuzzy_bits)
  {
    static const FixedPointType the_tolerance = ldexp(FixedPointType(1), FixedPointType::resolution + fuzzy_bits);

    return the_tolerance;
  }

  template<typename FixedPointType,
           typename FloatPointType = typename FixedPointType::float_type>
  void test_cbrt(const int fuzzy_bits)
  {
    // Use at least 6 resolution bits.
    // Use at least 8 range bits.

    BOOST_STATIC_ASSERT(-FixedPointType::resolution >= 6);
    BOOST_STATIC_ASSERT( FixedPointType::range      >= 8);

    const FixedPointType a1 (  2L    );                                      const FloatPointType b1(  2L    );
    const FixedPointType a2 (  3L    );                                      const FloatPointType b2(  3L    );
    const FixedPointType a3 (  8.375L);                                      const FloatPointType b3(  8.375L);
    const FixedPointType a4 ( 64.125L);                                      const FloatPointType b4( 64.125L);
    const FixedPointType a5 (100.875L);                                      const FloatPointType b5(100.875L);
    const FixedPointType a6 (FixedPointType(  1) /  3);                      const FloatPointType b6(FloatPointType(  1) /  3);
    const FixedPointType a7 (FixedPointType( 12) / 10);                      const FloatPointType b7(FloatPointType( 12) / 10);
    const FixedPointType a8 (FixedPointType(111) / 10);                      const FloatPointType b8(FloatPointType(111) / 10);
    const FixedPointType a9 (boost::math::constants::phi<FixedPointType>()); const FloatPointType b9(boost::math::constants::phi<FloatPointType>());

    // Use boost::math::cbrt for fixed-point negatable because cbrt(negatable) is not yet implemented.
    // Do not use a using directive for boost::math::cbrt() until cbrt is implemented for fixed-point negatable.
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a1), FixedPointType(boost::math::cbrt(b1)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a2), FixedPointType(boost::math::cbrt(b2)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a3), FixedPointType(boost::math::cbrt(b3)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a4), FixedPointType(boost::math::cbrt(b4)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a5), FixedPointType(boost::math::cbrt(b5)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a6), FixedPointType(boost::math::cbrt(b6)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a7), FixedPointType(boost::math::cbrt(b7)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a8), FixedPointType(boost::math::cbrt(b8)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(boost::math::cbrt(a9), FixedPointType(boost::math::cbrt(b9)), tolerance_maker<FixedPointType>(fuzzy_bits));
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_math_cbrt_fastest)
{
  // Test cbrt() for negatable round::fastest in various key digit regions.

  { typedef boost::fixed_point::negatable< 8,  -7, boost::fixed_point::round::fastest> fixed_point_type; local::test_cbrt<fixed_point_type>(4); }
  { typedef boost::fixed_point::negatable<15, -16, boost::fixed_point::round::fastest> fixed_point_type; local::test_cbrt<fixed_point_type>(((-fixed_point_type::resolution + 9) * 2) / 10); }
  { typedef boost::fixed_point::negatable<63, -64, boost::fixed_point::round::fastest> fixed_point_type; local::test_cbrt<fixed_point_type>(((-fixed_point_type::resolution + 9) * 1) / 10); }
}


#include "GL_Logger.h"
#include "KeySpline.h"
#include <Eigen/Geometry>
#include "Transform.h"
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
#include <GLFW/glfw3.h>

/**
 * Calculate the lenght of a segment using a Gaussian Quadrature
 * @param  u1 the first time point
 * @param  u2 the second point of the segemnt
 * @param  Gk 4 control poitns describing the spline
 * @return    the length of the segment.
 */
float gaussQuad3(float u1, float u2, const Eigen::MatrixXf & Gk, const Eigen::Matrix4f & B){
    float w[] = {5/9.0,8/9.0,5/9.0};
    float x[] = {-sqrtf(3/5.0),0,sqrtf(3/5.0)};
    float pointSummation = 0;
    for(int i = 0; i < 3; i++){
        float u = (u2-u1)/2.0 * x[i] + (u1 + u2)/2.0;
        Eigen::Vector4f du;
        du << 0,1,2*u,3*u*u;
        pointSummation+= w[i]*(Gk*B*du).norm();
    }
    return pointSummation * (u2-u1)/2.0;
}

//Calculate the fernet frame of a point.
glm::vec3 getNormalVector(const Eigen::Matrix4f & B, const Eigen::Vector3f points[],
    float u){
    Eigen::MatrixXf G(3,4);
    G << points[0](0), points[1](0), points[2](0), points[3](0),
         points[0](1), points[1](1), points[2](1), points[3](1),
         points[0](2), points[1](2), points[2](2), points[3](2);

    Eigen::Vector4f dUVec, d2UVec;
    dUVec << 0.0f, 1.0f, 2*u, 3*u*u;
    dUVec= dUVec.transpose();

    Eigen::Vector3f Tvec = (G*B*dUVec).normalized(); //T Vector
    return glm::vec3(Tvec(0),Tvec(1),Tvec(2));
}

KeySpline::KeySpline():
numSplines(0),
usTableDirty(true)
{
    setSplineType(CATMULL);
    vao.addAttribute(0,vertexBuffer);
};
KeySpline::~KeySpline(){};

void KeySpline::setSplineType(SplineType t){
    switch (t){
        case CATMULL:
            this->B << 0.0f, -1.0f,  2.0f, -1.0f,
                       2.0f,  0.0f, -5.0f,  3.0f,
                       0.0f,  1.0f,  4.0f, -3.0f,
                       0.0f,  0.0f, -1.0f,  1.0f;
            this->B*=0.5;
            break;
        case BSPLINE:
            this->B  << 1.0f, -3.0f,  3.0f, -1.0f,
                        4.0f,  0.0f, -6.0f,  3.0f,
                        1.0f,  3.0f,  3.0f, -3.0f,
                        0.0f,  0.0f,  0.0f,  1.0f;
            this->B /= 6.0f;
            break;
    }
    if(this->type != t){
        recalculateTable(6);
        this->type = t;
    }
}

void KeySpline::addNode(const SplineNode & node){
    nodePos.push_back(node.getPosition());
    nodeRot.push_back(node.getRotation());
    if(nodePos.size() >= 3){
        std::cout << "Increasing splines" << std::endl;
        numSplines++;
    }
    if(numSplines > 0){
           recalculateTable(6);
    }
    updateVBO = true;
}

//Replace with Geometry shader later
void KeySpline::draw(){
    if(updateVBO && numSplines > 0){

        Eigen::MatrixXf G(3,nodePos.size() + 1);
        Eigen::MatrixXf Gk(3,4);
        for(int i = 0; i < nodePos.size(); i++){
            G.block<3,1>(0,i) = nodePos[i];
        }
        G.block<3,1>(0,nodePos.size()) = nodePos.back();
        std::vector<glm::vec3>  positions;
        positions.clear();
        for(int k = 0; k < numSplines; k++){
            Gk = G.block<3,4>(0,k);
            int n = 32;
            for(int i = 0; i < n; i++){
                float u = i/(n-1.0f);
                Eigen::Vector4f uVec(1.0,u,u*u,u*u*u);
                Eigen::Vector3f P=Gk*B*uVec;
                positions.push_back(glm::vec3(P(0),P(1),P(2)));
            }
        }
        vertexBuffer.setData(positions);
        updateVBO = false;
    }
    if (numSplines > 0)
    {
        vao.bind();
        GL_Logger::LogError("Could not bind spline vao");

        glDrawArrays(GL_LINE_STRIP,0,vertexBuffer.getNumVerts());
        GL_Logger::LogError("Could not draw spline");
        vao.unbind();

    }
    //Drawing code here
}
void KeySpline::close(){
    nodePos.push_back(nodePos[0]);
    nodeRot.push_back(nodeRot[0]);
    //n-2,n-1,n,0
    nodePos.push_back(nodePos[1]);
    nodeRot.push_back(nodeRot[1]);
    //n-1,n,0,1
    nodePos.push_back(nodePos[2]);
    nodeRot.push_back(nodeRot[2]);
    //n,0,1,2
    numSplines+=3;
}

Transform KeySpline::transformAt(float s){
    assert(numSplines > 0);

    float kfloat;
    float uu = s;
    float u = std::modf(uu, &kfloat);
    int k = (int)std::floor(kfloat) % numSplines;
    Eigen::MatrixXf GPos(3,4);
    Eigen::MatrixXf GRot(4,4);
    std::vector<Eigen::Vector3f> cappedPos(nodePos);
    cappedPos.push_back(nodePos.back());
    for(int i = 0; i < 4; i++){
        GPos.block<3,1>(0,i) = cappedPos[k+i];
    }
    Eigen::Vector4f uVec(1.0,u,u*u,u*u*u);
    Eigen::Vector3f P= GPos * B * uVec;
    Transform t;
    t.setPosition(glm::vec3(P(0),P(1),P(2)));
    t.lookAlong(getNormalVector(B,&(cappedPos[k]),u));
    return t;
}

float KeySpline::sToU(float s)
{
    float sMod = std::fmod(s,usTable.back().second);
    if(usTableDirty)
    {
        recalculateTable(6);
        usTableDirty = false;
    }
    int i = 0;
    float last = 0;
    float cur = 0;

    while( i < (int)usTable.size() && sMod >= cur){
        i++;
        last = cur;
        cur = usTable[i].second;

    }
    if(i == 0){
        return 0;
    }
    float interpolationConst = (sMod-last)/(cur-last);
    return (1-interpolationConst)*usTable[i-1].first + interpolationConst*usTable[i].first;

}

void KeySpline::recalculateTable(int discretization){
    if(numSplines == 0)
        return;
    usTable.clear();
    int ncps = (int)nodePos.size();
    Eigen::MatrixXf G(3,ncps + 1);
    Eigen::MatrixXf Gk(3,4);

    usTable.push_back(std::make_pair(0.0f,0.0f));
    float s = 0;
    for(int i = 0; i < ncps; ++i) {
            G.block<3,1>(0,i) = nodePos[i];
    }
    //assign last node duplicate to cap the spline
    G.block<3,1>(0,ncps) = nodePos.back();
    ncps++;
    for(int k = 0; k < ncps - 3; ++k) {
        int n = discretization; // table resolution discretization
            // Gk is the 3x4 block starting at column k
        Gk = G.block<3,4>(0,k);
        for(int i = 0; i < n-1; ++i) {
            float u = i / (n - 1.0f);
            float u2 = u+1.0/(n - 1);
                // Compute spline point at u
            float ds = gaussQuad3(u, u2 , Gk, B);
            usTable.push_back(std::make_pair(u2+k,(s+=ds)));
        }
    }

}

int KeySpline::getNumNodes()
{
    return nodePos.size();
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2011 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

 //------------------ Author: Catarina Silva  -------------------------------------------------
// ------------------ E-mail: (catsilva20@gmail.com) ------------------------------------------

#include "attitudeintegration.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Astro-Core/EODE/eode.h>
#include <Astro-Core/statevector.h>

using namespace Eigen;

typedef Eigen::Matrix< double, 3, 3 > 	MyMatrix3d;
typedef Eigen::Matrix< double, 3, 1 > 	MyVector3d;
typedef Eigen::Matrix< double, 4, 4 > 	MyMatrix4d;
typedef Eigen::Matrix< double, 4, 1 > 	MyVector4d;

//Integrates the Euler equation and the quaternion rates equation.
//Inputs: initial body rates, inertia matrix and moments (now = 0)
//The propagation will use the Runge Kutta 4 integrator
//Outputs: final euler angles or quaternions

// WRITE THE REFERENCE OF THE FORMULAS

/**
  * Derivative of the Euler equation
  *
  * @param bodyRates        initial body rates
  * @param inertiaMatrix    inertia matrix
  *
  * @return derivBodyRates  body rates derivatives
  */
void derivEulerEquation (VectorXd bodyRates, double time, VectorXd inertiaANDmoments, VectorXd& derivBodyRates)
{
    double p = bodyRates[0];
    double q = bodyRates[1];
    double r = bodyRates[2];

    static double bodyCoeffs[9]={
        0,  -r,   q,
        r,   0,  -p,
       -q,   p,   0
    };
    static const Matrix3d bodyMatrix(bodyCoeffs);

    //the input of derivEulerEquation needs to be a VectorXd (in this case, 12elements vector).
    //This vector will include the components of the INERTIA MATRIX (the first 9 elements), and
    //the components of the external MOMENTS applied to the vehicle (last 3 elements)
    double I11 = inertiaANDmoments[0];
    double I12 = inertiaANDmoments[1];
    double I13 = inertiaANDmoments[2];
    double I21 = inertiaANDmoments[3];
    double I22 = inertiaANDmoments[4];
    double I23 = inertiaANDmoments[5];
    double I31 = inertiaANDmoments[6];
    double I32 = inertiaANDmoments[7];
    double I33 = inertiaANDmoments[8];

    //External Moments
    double M1 = inertiaANDmoments[9];
    double M2 = inertiaANDmoments[10];
    double M3 = inertiaANDmoments[11];

    Vector3d Moments(M1,M2,M3);

    static double inertiaCoeffs[9] = {
        I11, I12, I13,
        I21, I22, I23,
        I31, I32, I33

    };
    static const Matrix3d inertiaMatrix(inertiaCoeffs);

    //Euler's equation
    Matrix3d multipInertia= inertiaMatrix * inertiaMatrix.transpose();
    Matrix3d inertiaTimesBodyMatrix = bodyMatrix * multipInertia;

    derivBodyRates = Moments - inertiaTimesBodyMatrix * bodyRates;
}

/**
  * Propagate the Euler equation
  *
  * @param initbodyRates    initial body rates
  * @param time
  * @param timeStep
  * @param inertiaMatrix
  *
  * @return body rates      final body rates
  */
Vector3d propagateEulerEquation(const Vector3d& initbodyRates,
                                double time,
                                double timeStep,
                                const Matrix3d& inertiaMatrix)
{
    //Convert Vector3d to VectorSd so it can be fed to RK4
    VectorXd theBodyRates;
    theBodyRates << initbodyRates[0], initbodyRates[1],initbodyRates[2];

    //Convert the matrix3d of the inertia matrix to a vectorXd, so it can be an input in the intregator
    VectorXd inertiaMatrix_Vector;

    inertiaMatrix_Vector[0] = inertiaMatrix(0,0);
    inertiaMatrix_Vector[1] = inertiaMatrix(0,1);
    inertiaMatrix_Vector[2] = inertiaMatrix(0,2);
    inertiaMatrix_Vector[3] = inertiaMatrix(1,0);
    inertiaMatrix_Vector[4] = inertiaMatrix(1,1);
    inertiaMatrix_Vector[5] = inertiaMatrix(1,2);
    inertiaMatrix_Vector[6] = inertiaMatrix(2,0);
    inertiaMatrix_Vector[7] = inertiaMatrix(2,1);
    inertiaMatrix_Vector[8] = inertiaMatrix(2,2);

    //For now, a Runge Kutta 4 will be used. Later, this can be changed to a better integrator (Runge Kutta 7,8 ex.)
    rk4(theBodyRates, 3, time, timeStep, derivEulerEquation, inertiaMatrix_Vector);

    //Returns the body rates, after integration
    Vector3d integratedBodyRates;
    integratedBodyRates[0] = theBodyRates[0];
    integratedBodyRates[1] = theBodyRates[1];
    integratedBodyRates[2] = theBodyRates[2];
    return integratedBodyRates;
}

/**
  * This function calculates the quaternions rates
  *
  * @param initQuaternions  initial quaternions
  * @param parameters       body rates
  * @param time
  *
  * @return derivative      quaternions derivatives
  */

//This function calculates the derivatives of the quaternions
void derivQUATERNIONS (VectorXd initQuaternions, double time, VectorXd parameters, VectorXd& derivative)
{
    double p = parameters[0];
    double q = parameters[1];
    double r = parameters[2];

    static double bodyCoeffs[16]= {
        0, -p, -q, -r,
        p, 0, r, -q,
        q, -r, 0, p,
        r, q, -p, 0
    };
    static const MyMatrix4d bodyRatesMatrix(bodyCoeffs);
    derivative = bodyRatesMatrix * initQuaternions;
}

/**
  * This function integrates the quaternions rates
  *
  * @param quaternions  initial quaternions
  * @param time         time
  * @param timeStep     time step of the integration
  * @param bodyrates
  *
  * @return quats       final quaternions
  */

Quaterniond propagateQUATERNIONS(Quaterniond& quaternions,
                                 double time,
                                 double timeStep,
                                 Vector3d bodyrates)
{

    VectorXd parameters(3);
    parameters << bodyrates[0], bodyrates[1], bodyrates[2];

    VectorXd quats(4);
    quats << quaternions.coeffs().coeffRef(0), quaternions.coeffs().coeffRef(1),
    quaternions.coeffs().coeffRef(2), quaternions.coeffs().coeffRef(3);

    rk4 (quats, 4, time, timeStep, derivQUATERNIONS, parameters);

    //Transform VectorXd to quaternions
    Quaterniond propagatedQuaternion(quats(0),quats(1), quats(2),quats(3));

    return propagatedQuaternion;
}