/* Copyright (C) 2012-2019 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.
 */
/**
 * @file eqtesting.cpp
 * @brief Useful fucntions for equality testing...
 */
#include <NTL/lzz_pXFactoring.h>
#include "timing.h"
#include "EncryptedArray.h"

#include <cstdio>

namespace helib {

// Map all non-zero slots to 1, leaving zero slots as zero.
// Assumes that r=1, and that all the slot contain elements from GF(p^d).
//
// We compute x^{p^d-1} = x^{(1+p+...+p^{d-1})*(p-1)} by setting y=x^{p-1}
// and then outputting y * y^p * ... * y^{p^{d-1}}, with exponentiation to
// powers of p done via Frobenius.

// FIXME: the computation of the "norm" y * y^p * ... * y^{p^{d-1}}
// can be done using O(log d) automorphisms, rather than O(d).

void mapTo01(const EncryptedArray& ea, Ctxt& ctxt)
{
  long p = ctxt.getPtxtSpace();
  if (p != ea.getPAlgebra().getP()) // ptxt space is p^r for r>1
    throw helib::LogicError("mapTo01 not implemented for r>1");

  if (p>2)
    ctxt.power(p-1); // set y = x^{p-1}

  long d = ea.getDegree();
  if (d>1) { // compute the product of the d automorphisms
    std::vector<Ctxt> v(d, ctxt);
    for (long i=1; i<d; i++)
      v[i].frobeniusAutomorph(i);
    totalProduct(ctxt, v);
  }
}

// computes ctxt^{2^d-1} using a method that takes
// O(log d) automorphisms and multiplications
void fastPower(Ctxt& ctxt, long d)
{
  //OLD: assert(ctxt.getPtxtSpace()==2);
  helib::assertEq(ctxt.getPtxtSpace(), 2l, "ptxtSpace must be 2");
  if (d <= 1) return;

  Ctxt orig = ctxt;

  long k = NTL::NumBits(d);
  long e = 1;

  for (long i = k-2; i >= 0; i--) {
    Ctxt tmp1 = ctxt;
    tmp1.smartAutomorph(1L << e);
    ctxt.multiplyBy(tmp1);
    e = 2*e;

    if (NTL::bit(d, i)) {
      ctxt.smartAutomorph(2);
      ctxt.multiplyBy(orig);
      e += 1;
    }
  }
}

// ===> This function only works for p=2, r=1 <===
// Test if prefixes of bits in slots are all zero: Set slot j of res[i] to 0
// if bits 0..i of j'th slot in ctxt are all zero, else it is set to 1
// It is assumed that res and the res[i]'s are initialized by the caller.
// Complexity: O(d + n log d) smart automorphisms
//             O(n d)
void incrementalZeroTest(Ctxt* res[], const EncryptedArray& ea,
			 const Ctxt& ctxt, long n)
{
  FHE_TIMER_START;
  long nslots = ea.size();
  long d = ea.getDegree();

  // compute linearized polynomial coefficients

  std::vector< std::vector<NTL::ZZX> > Coeff;
  Coeff.resize(n);

  for (long i = 0; i < n; i++) {
    // coeffients for mask on bits 0..i
    // L[j] = X^j for j = 0..i, L[j] = 0 for j = i+1..d-1

    std::vector<NTL::ZZX> L;
    L.resize(d);

    for (long j = 0; j <= i; j++)
      SetCoeff(L[j], j);

    std::vector<NTL::ZZX> C;

    ea.buildLinPolyCoeffs(C, L);

    Coeff[i].resize(d);
    for (long j = 0; j < d; j++) {
      // Coeff[i][j] = to the encoding that has C[j] in all slots
      // FIXME: maybe encrtpted array should have this functionality
      //        built in
      std::vector<NTL::ZZX> T;
      T.resize(nslots);
      for (long s = 0; s < nslots; s++) T[s] = C[j];
      ea.encode(Coeff[i][j], T);
    }
  }

  std::vector<Ctxt> Conj(d, ctxt);
  // initialize Cong[j] to ctxt^{2^j}
  for (long j = 0; j < d; j++) {
    Conj[j].smartAutomorph(1L << j);
  }

  for (long i = 0; i < n; i++) {
    res[i]->clear();
    for (long j = 0; j < d; j++) {
      Ctxt tmp = Conj[j];
      tmp.multByConstant(Coeff[i][j]);
      *res[i] += tmp;
    }

    // *res[i] now has 0..i in each slot
    // next, we raise to the power 2^d-1

    fastPower(*res[i], d);
  }
  FHE_TIMER_STOP;
}

}


#ifndef MATH_AFFINE_HPP
#define MATH_AFFINE_HPP

#include <boost/operators.hpp>

namespace Math {

	template<typename R>
	struct point: boost::operators<point<R>> {
		R x, y, z;
		operator dual<R>(void) const {
			return {{1},{0,x,y,z}};
		}
		point& operator+=(const point<R> &rhs) {
			x += rhs.x;
			y += rhs.y;
			z += rhs.z;
			return *this;
		}
		point& operator-=(const point<R> &rhs) {
			x -= rhs.x;
			y -= rhs.y;
			z -= rhs.z;
			return *this;
		}
		point& operator*=(const R &rhs) {
			return *this = {x*rhs, y*rhs, z*rhs};
		}
		point& operator*=(R && rhs) {
			return *this = {x*rhs, y*rhs, z*rhs};
		}
		point operator*(R rhs) const {
			return Point(x*rhs, y*rhs, z*rhs);
		}
		point(R x = 0, R y = 0, R z = 0):
			x(x), y(y), z(z) {}
		point(const point &p):
			x(p.x), y(p.y), z(p.z) {}
	};

	template<typename S, typename R>
	point<R> operator*(const S &lhs, const point<R> &rhs) {
		point<R> out(rhs);
		return out *= static_cast<R>(lhs);
	}
	template<typename S, typename R>
	point<R> operator*(S && lhs, const point<R> &rhs) {
		return rhs * static_cast<R>(lhs);
	}

	template<typename R>
	struct unit {
		R x, y, z;
		operator dual<R>(void) const {
			return {1,0,0,0,0, x/2, y/2, z/2};
		}
		unit(R x, R y, R z): x(x), y(y), z(z) {}
		unit(unit<R> const& u): x(u.x), y(u.y), z(u.z) {}
	};

	template<typename R>
	struct ray: virtual dual<R> {
		R r;
		unit<R> n;
		ray(R r, R x, R y, R z):
			r(r), n(x, y, z),
			dual<R>(1, 0, 0, 0, 0,
				r*x/2, r*y/2, r*z/2) {}
		ray(R r, const unit<R> &dir):
			r(r), n(dir),
			dual<R>(1, 0, 0, 0, 0,
				r*n.x/2, r*n.y/2, r*n.z/2) {}
		ray(unit<R> const& dir):
			ray(1, dir) {}
		ray(ray<R> const& rhs):
			ray(rhs.r, rhs.n) {}
	};

	template<typename R>
	struct rotor: quat<R> {
		R theta;
		unit<R> n;
		operator dual<R>(void) const {
			return {*this, 0};
		}
		rotor(R theta, R nx, R ny, R nz):
			rotor(theta, unit<R>(nx, ny, nz)) {}
		rotor(R theta, const unit<R>& n):
			theta(theta), n(n),
			quat<R>(cos(theta/2), sin(theta/2)*n.x,
				sin(theta/2)*n.y, sin(theta/2)*n.z) {}
	};

	template<typename R>
	struct pivot: dual<R> {
		ray<R> translation;
		rotor<R> rotation;
		pivot(const ray<R> &offset, const rotor<R> &rotor):
			translation(offset), rotation(rotor),
			dual<R>((~dual<R>(offset))(rotor)) {}
		pivot(ray<R> && offset, rotor<R> && rotor):
			translation(offset), rotation(rotor),
			dual<R> ((~dual<R>(offset))(rotor)) {}
	};
}

#endif


/**
 * @file fastpam1.cpp
 * @date 2021-08-03
 *
 * This file contains the primary C++ implementation of the FastPAM1 code follows
 * from the paper: Erich Schubert and Peter J. Rousseeuw: Faster k-Medoids Clustering:
 * Improving the PAM, CLARA, and CLARANS Algorithms. The paper can be assessed at
 * https://arxiv.org/pdf/1810.05691.pdf. Also the original PAM papers:
 * 1) Leonard Kaufman and Peter J. Rousseeuw: Clustering by means of medoids.
 * 2) Leonard Kaufman and Peter J. Rousseeuw: Partitioning around medoids (program pam).
 *
 */
#include "fastpam1.hpp"

#include <carma>
#include <armadillo>
#include <unordered_map>
#include <regex>

/**
 * \brief Runs FastPAM1 algorithm.
 *
 * Run the FastPAM1 algorithm to identify a dataset's medoids.
 *
 * @param input_data Input data to cluster
 */
void FastPAM1::fit_fastpam1(const arma::mat& input_data) {
  data = input_data;
  data = arma::trans(data);
  arma::rowvec medoid_indices(n_medoids);
  FastPAM1::build_fastpam1(data, medoid_indices);
  steps = 0;
  medoid_indices_build = medoid_indices;
  arma::rowvec assignments(data.n_cols);
  size_t iter = 0;
  bool medoidChange = true;
  while (iter < max_iter && medoidChange) {
    auto previous{medoid_indices};
    FastPAM1::swap_fastpam1(data, medoid_indices, assignments);
    medoidChange = arma::any(medoid_indices != previous);
    iter++;
  }
  medoid_indices_final = medoid_indices;
  labels = assignments;
  steps = iter;
}

/**
 * \brief Build step for the FastPAM1 algorithm
 *
 * Runs build step for the FastPAM1 algorithm. Loops over all datapoint and
 * checks its distance from every other datapoint in the dataset, then checks if
 * the total cost is less than that of the medoid (if a medoid exists yet).
 *
 * @param data Transposed input data to cluster
 * @param medoid_indices Uninitialized array of medoids that is modified in place
 * as medoids are identified
 */
void FastPAM1::build_fastpam1(
  const arma::mat& data,
  arma::rowvec& medoid_indices
) {
  size_t N = data.n_cols;
  int p = (buildConfidence * N); // reciprocal
  bool use_absolute = true;
  arma::rowvec estimates(N, arma::fill::zeros);
  arma::rowvec best_distances(N);
  best_distances.fill(std::numeric_limits<double>::infinity());
  arma::rowvec sigma(N); // standard deviation of induced losses on reference points
  for (size_t k = 0; k < n_medoids; k++) {
    double minDistance = std::numeric_limits<double>::infinity();
    int best = 0;
    sigma = km::KMedoids::build_sigma(
                          data, best_distances, batchSize, use_absolute);
    // fixes a base datapoint
    for (int i = 0; i < data.n_cols; i++) {
      double total = 0;
      for (size_t j = 0; j < data.n_cols; j++) {
        // computes distance between base and all other points
        double cost = (this->*lossFn)(data, i, j);
        // compares this with the cached best distance
        if (best_distances(j) < cost) {
          cost = best_distances(j);
        }
        total += cost;
      }
      if (total < minDistance) {
        minDistance = total;
        best = i;
      }
    }
    medoid_indices(k) = best;

    // update the medoid assignment and best_distance for this datapoint
    for (size_t l = 0; l < N; l++) {
        double cost = (this->*lossFn)(data, l, medoid_indices(k));
        if (cost < best_distances(l)) {
            best_distances(l) = cost;
        }
    }
    use_absolute = false; // use difference of loss for sigma and sampling,
                          // not absolute
    logHelper.loss_build.push_back(minDistance/N);
    logHelper.p_build.push_back(static_cast<float>(1)/static_cast<float>(p));
    logHelper.comp_exact_build.push_back(N);
  }
}

/**
 * \brief Swap step for the FastPAM1 algorithm
 *
 * Runs swap step for the FastPAM1 algorithm. Loops over all datapoint and
 * compute the loss change when a medoid is replaced by the datapoint. The
 * loss change is stored in an array of size n_medoids and the update is
 * based on an if conditional outside of the loop. The best medoid is chosen
 * according to the best loss change.
 *
 * @param data Transposed input data to cluster
 * @param medoid_indices Array of medoid indices created from the build step
 * that is modified in place as better medoids are identified
 * @param assignments Uninitialized array of indices corresponding to each
 * datapoint assigned the index of the medoid it is closest to
 */
void FastPAM1::swap_fastpam1(
  const arma::mat& data,
  arma::rowvec& medoid_indices,
  arma::rowvec& assignments
) {
  double bestChange = 0;
  double minDistance = std::numeric_limits<double>::infinity();
  size_t best = 0;
  size_t medoid_to_swap = 0;
  size_t N = data.n_cols;
  int p = (N * n_medoids * swapConfidence); // reciprocal
  arma::mat sigma(n_medoids, N, arma::fill::zeros);
  arma::rowvec best_distances(N);
  arma::rowvec second_distances(N);
  arma::rowvec delta_td(n_medoids, arma::fill::zeros);

  // calculate quantities needed for swap, best_distances and sigma
  km::KMedoids::calc_best_distances_swap(
    data, medoid_indices, best_distances, second_distances, assignments);

  sigma = km::KMedoids::swap_sigma(data,
                                   batchSize,
                                   best_distances,
                                   second_distances,
                                   assignments);

  // write the sigma distribution to logfile
  km::KMedoids::sigma_log(sigma);
  // for every point in our dataset, let it serve as a new medoid
  for (size_t i = 0; i < data.n_cols; i++) {
      double di = best_distances(i);
      // loss change for making i a medoid
      delta_td.fill(-di);
      for (size_t j = 0; j < data.n_cols; j++) {
          if (j != i) {
              double dij = (this->*lossFn)(data, i, j);
              // update loss change for the current
              if (dij < second_distances(j)) {
                  delta_td.at(assignments(j)) += (dij - best_distances(j));
              } else {
                  delta_td.at(assignments(j)) += (second_distances(j) - best_distances(j));
              }
              // reassignment check
              if (dij < best_distances(j)) {
                  // update loss change for others
                  delta_td += (dij -  best_distances(j));
                  // remove the update for the current
                  delta_td.at(assignments(j)) -= (dij -  best_distances(j));
              }
          }
      }
      // choose the best medoid-to-swap
      arma::uword min_medoid = delta_td.index_min();
      // if the loss change is better than the best loss change
      // update the best index identified so far
      if (delta_td.min() < bestChange) {
          bestChange = delta_td.min();
          best = i;
          medoid_to_swap = min_medoid;
      }
  }
  // update the loss and medoid if the loss is improved
  if (bestChange < 0) {
      minDistance = arma::sum(best_distances) + bestChange;
      medoid_indices(medoid_to_swap) = best;
  } else {
      minDistance = arma::sum(best_distances);
  }
  logHelper.loss_swap.push_back(minDistance/N);
  logHelper.p_swap.push_back(static_cast<float>(1)/static_cast<float>(p));
  logHelper.comp_exact_swap.push_back(N*n_medoids);
}


#include "nonlinear_elastic_energy.hpp"
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

void NonlinearElasticEnergy::precompute(const TriMesh& mesh)
{
    elements_.resize(mesh.f.rows());

    Mat3x2d selector;
    selector << 0.0, 0.0,
                1.0, 0.0,
                0.0, 1.0;

    for (int idx=0; idx<mesh.f.rows(); idx++) {
        int i0 = mesh.f(idx, 0);
        int i1 = mesh.f(idx, 1);
        int i2 = mesh.f(idx, 2);

        Mat2d Dm;
        Dm.col(0) = mesh.u.segment<2>(2*i1) - mesh.u.segment<2>(2*i0);
        Dm.col(1) = mesh.u.segment<2>(2*i2) - mesh.u.segment<2>(2*i0);

        const Real A = 0.5 * (Vec3d() << Dm.col(0), 0.0).finished().cross(
                             (Vec3d() << Dm.col(1), 0.0).finished()).norm();

        elements_[idx].A = A;

        for (size_t i=0; i<2; i++)
            elements_[idx].edgeLengths[i] = Dm.col(i).norm();

        Mat3d P;
        P <<   1.0,   1.0,   1.0,
            mesh.u[2*i0], mesh.u[2*i1], mesh.u[2*i2],
            mesh.u[2*i0+1], mesh.u[2*i1+1], mesh.u[2*i2+1];

        // 2x3 matrix containing the gradients of the
        // 3 shape functions (one for each node)
        elements_[idx].Bm = A * (P.inverse() * selector).transpose();

        elements_[idx].DmInverse = Dm.inverse();

        Mat2d DmI = elements_[idx].DmInverse;
        elements_[idx].dFdu = MatrixXd::Zero(6,9);
        elements_[idx].dFdu << -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0), 0.0, 0.0,
                               -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1), 0.0, 0.0,
                                0.0, -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0), 0.0,
                                0.0, -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1), 0.0,
                                0.0, 0.0, -DmI(0,0)-DmI(1,0), 0.0, 0.0, DmI(0,0), 0.0, 0.0, DmI(1,0),
                                0.0, 0.0, -DmI(0,1)-DmI(1,1), 0.0, 0.0, DmI(0,1), 0.0, 0.0, DmI(1,1);

    }
}

void NonlinearElasticEnergy::getForceAndHessian(const TriMesh& mesh,
                                                const VecXd& x,
                                                VecXd& F,
                                                SparseMatrixd& dFdx,
                                                SparseMatrixd& dFdv) const {
    assert(F.size() >= x.size());

    for (int idx=0; idx<mesh.f.rows(); idx++) {
        // We are assuming that poisson ratio is 0.0, and thus, lambda = 0.0
        // Convert (E,v) (Young's modulus and poisson ratio) to lame parameters
        Real mu = ksx_ * 0.5;

        int idxs[3] = { mesh.f(idx,0), mesh.f(idx,1), mesh.f(idx,2) };

        Mat3x2d Dw;
        for (int i=0; i<2; i++) {
            Dw.col(i) = x.segment<3>(3*idxs[i+1]) - x.segment<3>(3*idxs[0]);
        }

        Vec2d ls = Dw.colwise().norm();
        for (int i=0; i<2; i++) {
            if (ls[i] > 2.5 * elements_[idx].edgeLengths[i]) {
                Dw.col(i) *= 2.5 * elements_[idx].edgeLengths[i] / ls[i];
            }
        }

        // Compute the deformation gradient Fdg,
        // 1st and 2nd Piola-Kirchoff stress tensors (S and P)
        Mat3x2d Fdg = Dw * elements_[idx].DmInverse;
	Mat2d     S = mu * (Fdg.transpose() * Fdg - Mat2d::Identity());
        Mat3x2d   P = Fdg * S;

        // Force is the 2nd P-K stress projected onto the node
        for (int i=0; i<3; i++) {
            F.segment<3>(3*idxs[i]).noalias() -= P * elements_[idx].Bm.col(i);
        }

        S = clampMatrixEigenvalues(S);

        // Derivative of 2nd P-K stress tensor w.r.t. F, the deformation gradient
        //
        // \partial S11 / \partial F = 4 mu ( F.col(0)  0 )
        // \partial S22 / \partial F = 4 mu ( 0  F.col(1) )
        // \partial S12 / \partial F = \partial S21 / \partial F = ( F.col(1)  F.col(0) )
        Mat3x2d dSdF[4];
        dSdF[0].col(0) = 2.0 * mu * Fdg.col(0);
        dSdF[0].col(1) = dSdF[3].col(0) = Vec3d::Zero();
        dSdF[3].col(1) = 2.0 * mu * Fdg.col(1);
        dSdF[1].col(0) = dSdF[2].col(0) = mu * Fdg.col(1);
        dSdF[1].col(1) = dSdF[2].col(1) = mu * Fdg.col(0);

        const Mat2x3d& dN = elements_[idx].Bm;

        // TODO: Work out compact expressions that take advantage of dFdu_jk only having one
        // non-zero row. The row is also a function of DmInverse above. No need to pre-compute more.
        Vec3d fi_jk;
        Mat2d dSdu_jk;
        for (size_t i=0; i<3; ++i)
        {
            for (size_t j=0; j<3; ++j)
            {
                for (size_t k=0; k<3; ++k) // \partial f_i / \partial u_j^k
                {
                    // 3 x 2 matrix that is the derivative of F with respect to u_j^k
                    //
                    Mat3x2d dFdu_jk = elements_[idx].dFdu.block<2,3>(2*k, 3*j).transpose();

                    for (size_t l=0; l<2; ++l)
                        for (size_t m=0; m<2; ++m)
                            dSdu_jk(l,m) = (dSdF[2*l+m].array() * dFdu_jk.array()).sum();

                    // Force on vertex i is f_i = F P dN_i, so
                    // derivative w.r.t. DOF u_j^k (j-th vertex, k-th DOF) is:
                    //
                    // \partial f_i / \partial u_j^k =
                    //     ( \partial F / \partial u_j^k S + F \partial S / \partial u_j^k ) dN_i
                    fi_jk = (dFdu_jk * S + Fdg * dSdu_jk) * dN.col(i);

                    for (size_t l=0; l<3; ++l)
                        //J.coeffRef(3*idxs[j]+k, 3*idxs[i]+l) += fi_jk[l];
                        dFdx.coeffRef(3*idxs[i]+l, 3*idxs[j]+k) -= fi_jk[l];
                }
            }
        }
    }
}

Mat2d NonlinearElasticEnergy::clampMatrixEigenvalues(Mat2d &input) const {
    static const Real eps = 1.0e-6;

    SelfAdjointEigenSolver<Mat2d> ev(input);
    assert(ev.info() == Success);

    return (ev.eigenvectors() *
            ev.eigenvalues().cwiseMax(eps).asDiagonal() *
            ev.eigenvectors().transpose());
}

void NonlinearElasticEnergy::getHessianPattern(const TriMesh& mesh, vector<SparseTripletd> &triplets) const {
    for (int idx=0; idx<mesh.f.rows(); idx++) {
        int idxs[3] = { mesh.f(idx,0), mesh.f(idx,1), mesh.f(idx,2) };
        for (size_t j=0; j<3; ++j)
            for (size_t k=0; k<3; ++k)
                for (size_t l=0; l<3; ++l)
                    for (size_t n=0; n<3; ++n)
                        triplets.push_back(SparseTripletd(3*idxs[j]+l,3*idxs[k]+n, 1.0));
    }
}


// Main code for peak bagging by means of nested sampling analysis
// Created by Enrico Corsaro @ INAF-OACT - August 2019
// e-mail: emncorsaro@gmail.com
// Source code file "Asymptotic.cpp"

#include <cstdlib>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <Eigen/Dense>
#include "Functions.h"
#include "File.h"
#include "MultiEllipsoidSampler.h"
#include "KmeansClusterer.h"
#include "EuclideanMetric.h"
#include "Prior.h"
#include "UniformPrior.h"
#include "NormalLikelihood.h"
#include "RadialModesPatternModel.h"
#include "NonRadialModesPatternModel.h"
#include "PowerlawReducer.h"
#include "Results.h"
#include "Ellipsoid.h"
#include "PrincipalComponentProjector.h"

int main(int argc, char *argv[])
{
    // Check number of arguments for main function

    if (argc != 8)
    {
        cerr << "Usage: ./asymptotic <Catalog ID> <Star ID> <output sub-directory> <run number> <input prior base filename> "
                "<input nuMax> <input angular degree>" << endl;
        exit(EXIT_FAILURE);
    }

    // ---------------------------
    // ----- Read input data -----
    // ---------------------------

    unsigned long Nrows;
    int Ncols;
    ArrayXXd data;
    string CatalogID(argv[1]);
    string StarID(argv[2]);
    string runNumber(argv[4]);
    string inputPriorBaseName(argv[5]);
    string inputNuMax(argv[6]);
    string inputDegree(argv[7]);
    double nuMax = stod(inputNuMax);
    int angularDegree = stoi(inputDegree);

    // Read the local path for the working session from an input ASCII file
    ifstream inputFile;
    File::openInputFile(inputFile, "localPath.txt");
    File::sniffFile(inputFile, Nrows, Ncols);
    vector<string> myLocalPath;
    myLocalPath = File::vectorStringFromFile(inputFile, Nrows);
    inputFile.close();

    // Set up some string paths used in the computation
    string outputSubDirName(argv[3]);
    string baseOutputDirName = myLocalPath[0] + "results/" + CatalogID + StarID + "/";
    string outputDirName = baseOutputDirName + outputSubDirName + "/";
    string outputPathPrefix = outputDirName + runNumber + "/asymptotic_";
    string baseInputDirName = baseOutputDirName + outputSubDirName + "/data/";
    string inputFileName = baseOutputDirName + outputSubDirName + "/data/" + runNumber + ".txt";

    cout << "------------------------------------------------------ " << endl;
    cout << " Performing asymptotic fit for l = " + inputDegree + " modes in " + CatalogID + StarID << endl;
    cout << "------------------------------------------------------ " << endl;

    // Read the input dataset
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nrows, Ncols);
    data = File::arrayXXdFromFile(inputFile, Nrows, Ncols);
    inputFile.close();

    // Creating frequency and PSD arrays
    ArrayXd covariates = data.col(0);
    ArrayXd observations = data.col(1);
    ArrayXd uncertainties = data.col(2);

    // -------------------------------------------------------
    // ----- First step. Set up all prior distributions ------
    // -------------------------------------------------------

    unsigned long Nparameters;              // Number of parameters for which prior distributions are defined

    // ---- Read prior hyper parameters for resolved modes -----
    inputFileName = outputDirName + inputPriorBaseName + "_" + runNumber + ".txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    ArrayXXd hyperParameters;

    if (Ncols == 1)
    {
        Ncols = 3;
        hyperParameters.conservativeResize(Nparameters,Ncols);
    }

    hyperParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    ArrayXd hyperParametersMinima = hyperParameters.col(0);
    ArrayXd hyperParametersMaxima = hyperParameters.col(1);
    // ---------------------------------------------------------

    int Ndimensions = Nparameters;              // Total number of dimensions of the peak bagging model

    if (angularDegree == 0)
    {
        if (Nparameters != 3)
        {
            cerr << "Wrong number of input prior hyper-parameters." << endl;
            cerr << "When performing an asymptotic fit for radial modes, three lines are " << endl;
            cerr << "expected from the input prior list (1 - DeltaNu, 2 - epsilon, 3 - alpha)." << endl;
        }
    }
    else
    {
        if (Nparameters != 5)
        {
            cerr << "Wrong number of input prior hyper-parameters." << endl;
            cerr << "When performing an asymptotic fit for radial modes, five lines are " << endl;
            cerr << "expected from the input prior list (1 - DeltaNu, 2 - epsilon, 3 - deltaNu0Degree, 4 - alpha, 5 - beta)." << endl;
        }
    }

    // Uniform Prior

    int NpriorTypes = 1;                                        // Total number of prior types included in the computation
    vector<Prior*> ptrPriors(NpriorTypes);

    double DeltaNu = 0;
    double epsilon = 0;
    double alpha = -99;
    double beta = -99;
    ArrayXd parametersMinima;                      // Minima for prior PDF
    ArrayXd parametersMaxima;                      // Maxima for prior PDF

    if (angularDegree == 0)
    {
        parametersMinima.resize(Ndimensions);
        parametersMaxima.resize(Ndimensions);
        parametersMinima << hyperParametersMinima;
        parametersMaxima << hyperParametersMaxima;

        if (hyperParametersMinima(1) == hyperParametersMaxima(1))
        {
            // In this case epsilon is a fixed value. Then reduce the number
            // of free parameters by 1 and remove the epsilon prior line from
            // the list of input prior parameters (only Deltanu and alpha are left).

            Ndimensions--;
            parametersMinima.resize(Ndimensions);
            parametersMaxima.resize(Ndimensions);
            parametersMinima << hyperParametersMinima(0), hyperParametersMinima(2);
            parametersMaxima << hyperParametersMaxima(0), hyperParametersMaxima(2);
            epsilon = hyperParametersMinima(1);
        }

        if (hyperParametersMinima(2) == hyperParametersMaxima(2))
        {
            // In this case alpha is a fixed value. Then reduce the number
            // of free parameters by 1 and remove the alpha prior line from
            // the list of input prior parameters (only DeltaNu and epsilon, or only DeltaNu are left).

            Ndimensions--;
            parametersMinima.conservativeResize(Ndimensions);
            parametersMaxima.conservativeResize(Ndimensions);
            alpha = hyperParametersMinima(2);
        }
    }

    if (angularDegree == 1 || angularDegree == 2 || angularDegree == 3)
    {
        // Here DeltaNu, epsilon are fixed parameters. Then reduce the
        // number of free parameters by 2 and remove DeltaNu and epsilon
        // prior lines from the list of input prior parameters.

        Ndimensions = 3;
        parametersMinima.resize(Ndimensions);
        parametersMaxima.resize(Ndimensions);
        parametersMinima << hyperParametersMinima.segment(2,Ndimensions);
        parametersMaxima << hyperParametersMaxima.segment(2,Ndimensions);
        DeltaNu = hyperParametersMinima(0);
        epsilon = hyperParametersMinima(1);

        // Check if alpha is fixed or not. If fixed, remove it from the prior
        // list and set its value to the one given in the prior file.

        if (hyperParametersMinima(3) == hyperParametersMaxima(3))
        {
            Ndimensions--;
            parametersMinima.resize(Ndimensions);
            parametersMaxima.resize(Ndimensions);
            parametersMinima << hyperParametersMinima(2), hyperParametersMinima(4);
            parametersMaxima << hyperParametersMaxima(2), hyperParametersMaxima(4);
            alpha = hyperParametersMinima(3);
        }

        // Check if beta is fixed or not. If fixed, remove it from the prior
        // list and set its value to the one given in the prior file.

        if (hyperParametersMinima(4) == hyperParametersMaxima(4))
        {
            Ndimensions--;
            parametersMinima.conservativeResize(Ndimensions);
            parametersMaxima.conservativeResize(Ndimensions);
            beta = hyperParametersMinima(4);
        }
    }

    UniformPrior uniformPrior(parametersMinima, parametersMaxima);
    ptrPriors[0] = &uniformPrior;

    string fullPathHyperParameters = outputPathPrefix + "hyperParametersUniform.txt";
    uniformPrior.writeHyperParametersToFile(fullPathHyperParameters);

    // -------------------------------------------------------------------
    // ---- Second step. Set up the asymptotic model ---------------------
    // -------------------------------------------------------------------

    Model *model = nullptr;

    if (angularDegree == 0)
    {
        model = new RadialModesPatternModel(covariates, nuMax, epsilon, alpha);
    }

    if (angularDegree > 0)
    {
        model = new NonRadialModesPatternModel(covariates, angularDegree, nuMax, DeltaNu, epsilon, alpha, beta);
    }

    // -----------------------------------------------------------------
    // ---- Third step. Set up the likelihood function to be used ------
    // -----------------------------------------------------------------

    NormalLikelihood likelihood(observations, uncertainties, *model);

    // -------------------------------------------------------------------------------
    // ----- Fourth step. Set up the X-means clusterer using an Euclidean metric -----
    // -------------------------------------------------------------------------------

    inputFileName = outputDirName + "Xmeans_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);

    if (Nparameters != 2)
    {
        cerr << "Wrong number of input parameters for X-means algorithm." << endl;
        exit(EXIT_FAILURE);
    }

    ArrayXd configuringParameters;
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    int minNclusters = configuringParameters(0);
    int maxNclusters = configuringParameters(1);

    if ((minNclusters <= 0) || (maxNclusters <= 0) || (maxNclusters < minNclusters))
    {
        cerr << "Minimum or maximum number of clusters cannot be <= 0, and " << endl;
        cerr << "minimum number of clusters cannot be larger than maximum number of clusters." << endl;
        exit(EXIT_FAILURE);
    }

    int Ntrials = 10;
    double relTolerance = 0.01;     // k-means

    bool printNdimensions = false;
    PrincipalComponentProjector projector(printNdimensions);
    bool featureProjectionActivated = false;
    EuclideanMetric myMetric;

    KmeansClusterer clusterer(myMetric, projector, featureProjectionActivated,
                           minNclusters, maxNclusters, Ntrials, relTolerance);

    // ---------------------------------------------------------------------
    // ----- Fifth step. Configure and start nested sampling inference -----
    // ---------------------------------------------------------------------

    inputFileName = outputDirName + "NSMC_configuringParameters.txt";
    File::openInputFile(inputFile, inputFileName);
    File::sniffFile(inputFile, Nparameters, Ncols);
    configuringParameters.setZero();
    configuringParameters = File::arrayXXdFromFile(inputFile, Nparameters, Ncols);
    inputFile.close();

    if (Nparameters > 9 || Nparameters < 8)
    {
        cerr << "Wrong number of input parameters for NSMC algorithm." << endl;
        cerr << "There must be either 8 or 9 parameters. In this case the last parameter is always ignored." << endl;
        exit(EXIT_FAILURE);
    }

    // Print results on the screen

    bool printOnTheScreen = true;

    // Initial number of live points

    int initialNobjects = configuringParameters(0);

    // Minimum number of live points

    int minNobjects = configuringParameters(1);

    // Maximum number of attempts when trying to draw a new sampling point

    int maxNdrawAttempts = configuringParameters(2);

    // The first N iterations, we assume that there is only 1 cluster

    int NinitialIterationsWithoutClustering = configuringParameters(3);

    // Clustering is only happening every N iterations.

    int NiterationsWithSameClustering = configuringParameters(4);

    // Fraction by which each axis in an ellipsoid has to be enlarged
    // It can be a number >= 0, where 0 means no enlargement. configuringParameters(5)
    // Calibration from Corsaro et al. (2018)

    double initialEnlargementFraction = 0.369*pow(Ndimensions,0.574);

    // Exponent for remaining prior mass in ellipsoid enlargement fraction.
    // It is a number between 0 and 1. The smaller the slower the shrinkage of the ellipsoids.

    double shrinkingRate = configuringParameters(6);

    // Termination factor for nested sampling process.

    double terminationFactor = configuringParameters(7);

    // Total maximum number of nested iterations required to carry out the computation.
    // This is used only in the multi-modal approach.

    int maxNiterations = 0;

    MultiEllipsoidSampler nestedSampler(printOnTheScreen, ptrPriors, likelihood, myMetric, clusterer,
                                        initialNobjects, minNobjects, initialEnlargementFraction, shrinkingRate);

    double tolerance = 1.e2;
    double exponent = 0.4;
    PowerlawReducer livePointsReducer(nestedSampler, tolerance, exponent, terminationFactor);

    nestedSampler.run(livePointsReducer, NinitialIterationsWithoutClustering, NiterationsWithSameClustering,
                      maxNdrawAttempts, terminationFactor, maxNiterations, outputPathPrefix);

    nestedSampler.outputFile << "# List of configuring parameters used for the ellipsoidal sampler and X-means" << endl;
    nestedSampler.outputFile << "# Row #1: Minimum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #2: Maximum Nclusters" << endl;
    nestedSampler.outputFile << "# Row #3: Initial Enlargement Fraction" << endl;
    nestedSampler.outputFile << "# Row #4: Shrinking Rate" << endl;
    nestedSampler.outputFile << minNclusters << endl;
    nestedSampler.outputFile << maxNclusters << endl;
    nestedSampler.outputFile << initialEnlargementFraction << endl;
    nestedSampler.outputFile << shrinkingRate << endl;
    nestedSampler.outputFile << "# Other information on the run" << endl;
    nestedSampler.outputFile << "# Row #1: Local working path used" << endl;
    nestedSampler.outputFile << "# Row #2: Catalog and Star ID" << endl;
    nestedSampler.outputFile << "# Row #3: Run Directory" << endl;
    nestedSampler.outputFile << "# Row #4: Run Number" << endl;
    nestedSampler.outputFile << "# Row #5: nuMax (microHz)" << endl;
    nestedSampler.outputFile << "# Row #7: angular degree (either 0, 1, 2, or 3)" << endl;
    nestedSampler.outputFile << myLocalPath[0] << endl;
    nestedSampler.outputFile << CatalogID + StarID << endl;
    nestedSampler.outputFile << outputSubDirName << endl;
    nestedSampler.outputFile << runNumber << endl;
    nestedSampler.outputFile << nuMax << endl;
    nestedSampler.outputFile << angularDegree << endl;
    nestedSampler.outputFile.close();

    // -------------------------------------------------------
    // ----- Last step. Save the results in output files -----
    // -------------------------------------------------------

    Results results(nestedSampler);
    results.writeParametersToFile("parameter");
    results.writeLogLikelihoodToFile("logLikelihood.txt");
    results.writePosteriorProbabilityToFile("posteriorDistribution.txt");
    results.writeLogEvidenceToFile("logEvidence.txt");
    results.writeLogMeanLiveEvidenceToFile("logMeanLiveEvidence.txt");
    results.writeEvidenceInformationToFile("evidenceInformation.txt");

    // Print out parameter estimates only in the case of a uni-modal high-dimensional fit.

    double credibleLevel = 68.3;
    bool writeMarginalDistributionToFile = true;
    results.writeParametersSummaryToFile("parameterSummary.txt", credibleLevel, writeMarginalDistributionToFile);

    cout << "Process # " << runNumber << " under subdir: " + outputSubDirName + " has been completed." << endl;

    return EXIT_SUCCESS;
}


#include "bend_forces.h"

#include "../adjacency.h"

#include <Eigen/Dense>
#include <igl/edges.h>
#include <igl/sparse_cached.h>
#include <iostream>
#include <iostream>
#include <cmath>
#include <cfloat>

using namespace std;
using namespace Eigen;

typedef Eigen::Triplet<double> Tri;

template <typename T> int sgn(T val) {
	return (T(0) < val) - (val < T(0));
}

// Refer to: Discrete bending forces and their Jacobians by Tamstorf et al.
//
//         x0
//         /\
//        /  \
//     e2/    \e1
//      /  t   \
//     /        \
//    /    e0    \
//  x1------------x2
//    \          /
//     \   t~   /
//      \      /
//    e2~\    /e1~
//        \  /
//         \/
//         x3
//
// Edge orientation: e0,e2,e2~ point away from x1
//                      e1,e1~ point away from x2
// E4 keeps them in the following order: x1, x2, x0, x3

Bend::Bend() {}

void Bend::init(
	const double k_bend,
	const double k_damping,
	const MatrixXd& X,			// in: vertex positions
	const MatrixXi& T			// in: mesh triangles
) {
	this->k_bend = k_bend;
	this->k_damping = k_damping;
	this->T = T;
	this->n = X.rows();
	this->m = T.rows();

	// create adjacency information - E4: list of 4 vertices for each face pair (each internal edge)
	vector< vector<int> > VF_adj;
	igl::edges(T, E);
	createVertexFaceAdjacencyList(T, VF_adj);
	createFaceEdgeAdjecencyList(T, E, VF_adj, FE_adj);
	createFacePairEdgeListWith4VerticeIDs(T, E, VF_adj, E4, EF6, EF_adj);
	this->no_edges = E4.rows();

	// define size of output
	n3 = 3 * n;
	k_entries = no_edges * 16 * 9;

	// precompute phi and scaling coefficient
	phi_bar.resize(no_edges);
	phi_hat.resize(no_edges);
	a.resize(no_edges);
	precompute_rest_shape(X);
}

void Bend::precompute_rest_shape(const MatrixXd & X) {
	for (int e = 0; e < no_edges; e++) {
		if (EF_adj(e, 1) != -1) { // no boundary edge
			// edges
			Vector3d e0 = X.row(E4(e, 2)) - X.row(E4(e, 1));
			Vector3d e2 = X.row(E4(e, 0)) - X.row(E4(e, 1));
			Vector3d e2_dot = X.row(E4(e, 3)) - X.row(E4(e, 1));

			// normals
			Vector3d normal = e0.cross(e2).normalized();
			Vector3d normal_dot = e2_dot.cross(e0).normalized();

			// phi of theta/2 of reference configuration
			Matrix3d det;
			det.col(0) = normal;
			det.col(1) = normal_dot;
			det.col(2) = e0;
			phi_bar(e) = 2. * sgn(det.determinant()) * (normal - normal_dot).norm() / (normal + normal_dot).norm();
			phi_hat(e) = phi_bar(e);

			//area and scaling coefficient
			double a0 = 0.5 * e0.cross(e2).norm();
			double a1 = 0.5 * e0.cross(e2_dot).norm();
			double length = e0.norm();
			a(e) = 3. * length * length / (a0 + a1);
		}
		else {
			phi_bar(e) = 0;
			phi_hat(e) = 0;
			a(e) = 0;
		}
	}
}

void Bend::compute_forces(
	const MatrixXd& X,			// in: vertex positions
	const double timestep,		// in: delta t
	VectorXd& F,				// out: forces
	SparseMatrix<double>& K,	// out: stiffness matrix
	SparseMatrix<double>& D		// out: damping matrix
) {
	F = VectorXd::Zero(n3);
	vector<Tri> triK, triD;
	triK.reserve(k_entries);
	triD.reserve(k_entries);

	// only do calculations if k_bend != 0
	if (k_bend == 0) {
		K = SparseMatrix<double>(n3, n3);	// zero matrices
		D = SparseMatrix<double>(n3, n3);
		return;
	}

	// compute
	MatrixXd normal(m, 3);
	for (int f = 0; f < m; f++) {
		Vector3d e1 = X.row(T(f, 1)) - X.row(T(f, 0));
		Vector3d e2 = X.row(T(f, 2)) - X.row(T(f, 0));
		normal.row(f) = e1.cross(e2).normalized();
	}

	VectorXd edge_length(no_edges);
	VectorXd phi_d(no_edges);
	VectorXd psi_d(no_edges);
	VectorXd psi_dd(no_edges);
	for (int e = 0; e < no_edges; e++) {
		// hinge edge
		Vector3d e0 = X.row(E4(e, 2)) - X.row(E4(e, 1));
		edge_length(e) = e0.norm();

		if (EF_adj(e, 1) != -1) { // no border edge
			Vector3d n1 = normal.row(EF_adj(e, 0));
			Vector3d n2 = normal.row(EF_adj(e, 1));

			Matrix3d det;
			det.col(0) = n1;
			det.col(1) = n2;
			det.col(2) = e0;

			double n1_plus_n2_norm = (n1 + n2).norm();
			double sec_theta_half = 2. / n1_plus_n2_norm;
			double phi = 2. * sgn(det.determinant()) * (n1 - n2).norm() / n1_plus_n2_norm;
			phi_d(e) = sec_theta_half * sec_theta_half;
			double phi_dd = 0.5 * phi * sec_theta_half;

			psi_d(e) = 2. * a(e) * ( k_bend * phi_d(e) * (phi - phi_bar(e))				// force
							    + k_damping * phi_d(e) * (phi - phi_hat(e)) / timestep );	// damping

			psi_dd(e) = 2. * a(e) * ( k_bend * (phi_dd * (phi - phi_bar(e)) + phi_d(e) * phi_d(e))				// force
								 + k_damping * (phi_dd * (phi - phi_hat(e)) + phi_d(e) * phi_d(e)) / timestep);	// damping

			// update last phi
			phi_hat(e) = phi;
		}
		else {
			psi_d(e) = 0;
			psi_dd(e) = 0;
		}
	}

	MatrixXd cos_alpha(m,3), h_inverse(m,3);
	vector<MatrixXd> H_triangle(m);
	for (int f = 0; f < m; f++) {
		vector<Vector3d> e(3);
		e[0] = X.row(T(f, 2)) - X.row(T(f, 1));		// edge opposite of v_0
		e[1] = X.row(T(f, 0)) - X.row(T(f, 2));		// opposite of v_1
		e[2] = X.row(T(f, 1)) - X.row(T(f, 0));		// opposite of v_2

		double area = 0.5 * e[0].cross(-e[2]).norm();

		vector<Vector3d> en(3);
		for (int i = 0; i < 3; i++)
			en[i] = e[i].normalized();			// direction is important, therefore we do not precompute them per edge earlier

		vector<Vector3d> edge_normal(3);
		for (int i = 0; i < 3; i++) {
			cos_alpha(f, i) = -en[(i + 1) % 3].dot(en[(i + 2) % 3]);	// angle at v_i
			h_inverse(f, i) = 0.5 * edge_length(FE_adj[f][i]) / area;	// height ending in v_i
			Vector3d nt = normal.row(f).transpose();
			edge_normal[i] = en[i].cross(nt);							// edge normal to edge opposite to v_i
		}

		vector<double> c(3);
		vector<Matrix3d> M(3), N(3), R(3);
		for (int i = 0; i < 3; i++) {
			int e_id = FE_adj[f][i];
			M[i] = normal.row(f).transpose() * edge_normal[i].transpose();
			N[i] = M[i] / (edge_length(e_id) * edge_length(e_id));

			bool edge_on_boundary = EF_adj(e_id, 1) == -1;
			double sigma = edge_on_boundary ? 0. : 1.;
			c[i] = sigma * psi_d(e_id);
			R[i] = c[i] * N[i];
		}

		vector<double> d(3);
		for (int i = 0; i < 3; i++) {
			int i_minus = (i + 2) % 3;
			int i_plus = (i + 1) % 3;
			d[i] = c[i_minus] * cos_alpha(f, i_plus) + c[i_plus] * cos_alpha(f, i_minus) - c[i];
		}

		// calculate the stiffness matrix contribution for a single triangle
		H_triangle[f].resize(9, 9);
		for (int i = 0; i < 3; i++) {
			int j = (i + 1) % 3;
			int k = (i + 2) % 3;

			double omega_ii = h_inverse(f, i) * h_inverse(f, i);
			Matrix3d H_ii = omega_ii * d[i] * (M[i].transpose() + M[i]) - R[j] - R[k];
			H_triangle[f].block(3*i, 3*i, 3, 3) = H_ii;

			// check if global orientation of edge matches with local ccw orientation -> only for matching orientations do we transpose R
			//
			//		   e0
			//   v2 -------- v1			Of e0 (i = 0) the first vertex in ccw order is
			//    \		    /			v1 = F(f, (i+1)%3)
			//     \   f   /
			//	e1  \     /  e2
			//       \	 /
			//		  \ /
			//        v0
			//
			int e_id = FE_adj[f][i];
			int first_vertex_local = T(f, j);
			int first_vertex_global = E(e_id, 0);
			Matrix3d R_local = R[k];
			if (first_vertex_local == first_vertex_global)
				R_local.transposeInPlace();

			double omega_ij = h_inverse(f, i) * h_inverse(f, j);
			Matrix3d H_ij = omega_ij * (d[i] * M[j].transpose() + d[j] * M[i]) + R_local;
			H_triangle[f].block(3 * i, 3 * j, 3, 3) = H_ij;
			H_triangle[f].block(3 * j, 3 * i, 3, 3) = H_ij.transpose();
		}
	}

	for (int e = 0; e < no_edges; e++) {
		if (EF_adj(e, 1) != -1) { // no border edge

			// indexing
			int f = EF_adj(e, 0);		// adjacent face 1
			int f_dot = EF_adj(e, 1);	// adjacent face 2

			vector<int> f_v(3), f_dot_v(3);	// f_v 0,1,2 corresponds to v0,v1,v2 --- f_dot_v 0,1,2 corresponds to v1,v2,v3
			for (int i = 0; i < 3; i++) {
				f_v[i] = EF6(e, i);
				f_dot_v[i] = EF6(e, 3 + i);
			}

			// first derivative
			VectorXd delta_theta(12);
			delta_theta.segment(0, 3) = -h_inverse(f, f_v[0]) * normal.row(f);
			delta_theta.segment(3, 3) = cos_alpha(f, f_v[2]) * h_inverse(f, f_v[1]) * normal.row(f) + cos_alpha(f_dot, f_dot_v[1]) * h_inverse(f_dot, f_dot_v[0]) * normal.row(f_dot);
			delta_theta.segment(6, 3) = cos_alpha(f, f_v[1]) * h_inverse(f, f_v[2]) * normal.row(f) + cos_alpha(f_dot, f_dot_v[0]) * h_inverse(f_dot, f_dot_v[1]) * normal.row(f_dot);
			delta_theta.segment(9, 3) = -h_inverse(f_dot, f_dot_v[2]) * normal.row(f_dot);

			for (int i = 0; i < 3; i++)
				for (int v = 0; v < 4; v++)
					F(E4(e, v) + i * n) += -psi_d(e) * delta_theta(3*v + i);

			// second derivative
			// stiffness matrix
			MatrixXd delta_f = MatrixXd::Zero(12, 12);
			delta_f -= psi_dd(e) * delta_theta * delta_theta.transpose();	// second of two terms that build delta_f

			for (int i = 0; i < 4; i++) {
				for (int j = 0; j <= i; j++) {	// go through vertice pairs i-j

					for (int p = 0; p < 3; p++) {			// go through x,y,z coordinates
						int q = 0;
						if (i == j) q = p;					// for central blocks, exploit symmetry
						for (; q < 3; q++) {
							if (i < 3 && j < 3) delta_f(3 * i + p, 3 * j + q) -= H_triangle[f](f_v[i] * 3 + p, f_v[j] * 3 + q);					// contribution of first term of face f
							if (i > 0 && j > 0) delta_f(3 * i + p, 3 * j + q) -= H_triangle[f_dot](f_dot_v[i - 1] * 3 + p, f_dot_v[j - 1] * 3 + q);	// contribution of first term of face f_dot
						}
					}
				}
			}

			// project onto positive eigenvalues
			// these 9x9 matrices are real symmetric => diagonizable (spectral theorem)
			// symmetry also gives orthogonal eigenvalues, therefore Eval.transpose() = Eval.inverse()
			SelfAdjointEigenSolver<MatrixXd> eig(-delta_f);
			VectorXd eigenvalues = eig.eigenvalues();
			if (eigenvalues.minCoeff() < -1e-10) {
				MatrixXd eigenvectors = eig.eigenvectors();
				MatrixXd S = MatrixXd::Zero(eigenvalues.rows(), eigenvalues.rows());
				for (int s = 0; s < eigenvalues.rows(); s++)
					S(s, s) = eigenvalues(s) > -1e-10 ? eigenvalues(s) : 0;
				delta_f = -eigenvectors * S * eigenvectors.transpose();
			}

			// create triplets
			for (int i = 0; i < 4; i++) {
				for (int j = 0; j <= i; j++) {	// go through vertice pairs i-j

					for (int p = 0; p < 3; p++) {			// go through x,y,z coordinates
						int q = 0;
						if (i == j) q = p;					// for central blocks, exploit symmetry
						for (; q < 3; q++) {
							int row = E4(e, i) + p * n;		// corresponds to vertex id (i) and shifted by number of vertices for y and z coordinates (p)
							int col = E4(e, j) + q * n;
							if(col <= row) triK.push_back(Tri(row, col, delta_f(3 * i + p, 3 * j + q)));
							else triK.push_back(Tri(col, row, delta_f(3 * i + p, 3 * j + q)));
							//if((i != j) || (i == j && p != q)) triK.push_back(Tri(col, row, value));					// exploit symmetry of delta_f blocks
						}
					}
				}
			}

			// damping matrix
			// this is a hand-made damping matrix, that is not mentioned in the paper. Allows for bigger time steps.
			VectorXd D_diagonal = -2. * a(f) * k_damping * phi_d(e) * delta_theta;
			for (int i = 0; i < 3; i++){
				for (int v = 0; v < 4; v++) {
					int row = E4(e, v) + i * n;
					triD.push_back(Tri(row, row, D_diagonal(3 * v + i)));
				}
			}
		}
	}

	// build the sparse matrices from the triplets
	if (K_data.rows() == 0) {
		this->K = SparseMatrix<double>(n3, n3);
		this->D = SparseMatrix<double>(n3, n3);
		igl::sparse_cached_precompute(triK, K_data, this->K);
		igl::sparse_cached_precompute(triD, D_data, this->D);
		cout << "initialize matrix structure" << endl;
		K = this->K;
		D = this->D;
	}
	else {
		igl::sparse_cached(triK, K_data, this->K);
		igl::sparse_cached(triD, D_data, this->D);
		K = this->K;
		D = this->D;
	}

	//K.setFromTriplets(triK.begin(), triK.end());
	//D.setFromTriplets(triD.begin(), triD.end());
}


#pragma once

#include <math.h>
#include <time.h>
#include <boost/multiprecision/gmp.hpp>

#include "FiniteFields.hpp"
#include "NumberTheoreticTransform.hpp"

namespace ligero {

/**
 *  Secret Sharing Interface
 *   Handles the sharing and reconstruction of the private witness, as well as
 * support for multiple tests for inter-block, intra-block and quadratic
 * constraints
 */
class SecretSharingNTT {
 public:
  /* Constructors allowing some flexibility in the choice of domains */
  ~SecretSharingNTT() {
    delete _smallDomain;
    delete _largeDomain;
  }
  SecretSharingNTT(size_t modulusIdx, size_t lSecretLength, size_t kDegree,
                   size_t nShares,
                   void (*computeSmall)(uint64_t *, uint64_t const *),
                   void (*computeLarge)(uint64_t *, uint64_t const *))
      : modulusIdx_(modulusIdx),
        lSecretLength_(lSecretLength),
        kDegree_(kDegree),
        nNumberShares_(nShares) {
    this->dCompositeDomainSize_ = nShares;

    _smallDomain = new NTT(modulusIdx, this->kDegree_, computeSmall);
    _largeDomain = new NTT(modulusIdx, this->nNumberShares_, computeLarge);
  };

  void share(uint64_t *secret);
  void reconstruct(uint64_t *eval, bool expanded = false);

  void shareMany(uint64_t *secrets, size_t lrows);
  void reconstructMany(uint64_t *secrets, size_t lrows);

  bool degreeTest(uint64_t *eval);
  bool zeroTest(uint64_t *eval, bool expanded = false);
  bool zeroSumTest(uint64_t *eval);

 protected:
  void padPolynominal(uint64_t *secret);
  void padIntrablocRandomness(uint64_t *secret);

  size_t modulusIdx_;
  size_t lSecretLength_;
  size_t kDegree_;
  size_t nNumberShares_;
  size_t dCompositeDomainSize_;

  NTT *_smallDomain;
  NTT *_largeDomain;
};

template <typename FieldT>
class SecretSharingInterface : SecretSharingNTT {
 public:
  /* Constructors allowing some flexibility in the choice of domains */
  SecretSharingInterface(size_t modulusIdx, size_t lSecretLength,
                         size_t kDegree, size_t nShares,
                         void (*computeSmall)(uint64_t *, uint64_t const *),
                         void (*computeLarge)(uint64_t *, uint64_t const *))
      : SecretSharingNTT(modulusIdx, lSecretLength, kDegree, nShares,
                         computeSmall, computeLarge) {
    this->dCompositeDomainSize_ = nShares;
  };

  using SecretSharingNTT::degreeTest;
  using SecretSharingNTT::reconstruct;
  using SecretSharingNTT::share;
  using SecretSharingNTT::zeroSumTest;
  using SecretSharingNTT::zeroTest;

  void share(FieldT *secret);
  void reconstruct(FieldT *eval, bool expanded = false);

  void padMany(FieldT *secrets, size_t lrows);
  void padIntrablocRandomness(FieldT *secret);
  void shareMany(FieldT *secrets, size_t lrows);
  void reconstructMany(FieldT *secrets, size_t lrows);

  bool degreeTest(FieldT *eval);
  bool zeroTest(FieldT *eval, bool expanded = false);
  bool zeroSumTest(FieldT *eval);
  FieldT sumReconstruct(FieldT *eval);

 protected:
  void padSecret(FieldT *secret);
  void padPolynominal(FieldT *secret);

  // adapter
  std::vector<uint64_t> demote(FieldT *eval, size_t length);
  void promote(uint64_t *eval, size_t length, FieldT *dest);
};

/** Implementation
/*
=============================================================================================
 */

/** Pads a block to k degree with numbers generated at random from the
 * underlying field
 * @param secret witness block to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padSecret(FieldT *secret) {
  FieldT::randomVector(secret + this->lSecretLength_,
                       this->kDegree_ - this->lSecretLength_, true);
}

/** Pads randomness for intrabloc constraints up to degree k with zeroes
 * @param secret randomness block to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padIntrablocRandomness(
    FieldT *secret) {
  for (size_t i = this->lSecretLength_; i < kDegree_; i++) {
    secret[i] = FieldT(0);
  }
}

/** Pads polynomial coefficients from k degree to n number of shares
 * @param secret array of polynomial coefficients to be padded
 */
template <typename FieldT>
inline void SecretSharingInterface<FieldT>::padPolynominal(FieldT *secret) {
  for (size_t i = this->kDegree_; i < nNumberShares_; i++) {
    secret[i] = FieldT(0);
  }
}

/** Pads multiple blocks
 * @param secret witness blocks to be padded
 * @param lrows number of blocks
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::padMany(FieldT *secret, size_t lrows) {
  /* Add randomness for padding */
  for (size_t i = 0; i < lrows; i++) {
    this->padSecret(secret + i * this->nNumberShares_);
  }
}

/** Secret-sharing interface for a block in the witness: this demotes field
 * elements into 64-bit numbers and calls the specialized 64-bit secret-sharing
 * method
 * @param secret witness block to be padded
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::share(FieldT *secret) {
  std::vector<uint64_t> secret64 = demote(secret, this->nNumberShares_);
  this->share(&secret64[0]);
  promote(&secret64[0], this->nNumberShares_, secret);
}

/** Reconstructs the secret corresponding to a given shared block
 * @param eval array of field elements representing a shared block
 * @param expandedDegree flag indicating that the degree of the polynomial for
 * this bloc is more than k
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::reconstruct(FieldT *eval,
                                                 bool expandedDegree) {
  std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
  this->reconstruct(&eval64[0], expandedDegree);
  promote(&eval64[0], this->kDegree_, eval);
}

/** Demotes an array of field elements to 64-bit numbers
 * @param eval array of field elements
 * @param length length of the array to be demoted
 * @return array of demoted 64-bit numbers
 */
template <typename FieldT>
std::vector<uint64_t> SecretSharingInterface<FieldT>::demote(FieldT *eval,
                                                             size_t length) {
  std::vector<uint64_t> temp(length);
  for (size_t idx = 0; idx < length; idx++) {
    temp[idx] = static_cast<uint64_t>(eval[idx].getValue());
  }

  return temp;
}

/** promotes an array of 64-bit numbers to field elements
 * @param eval array of 64-bit numbers
 * @param length length of the array to be promoted
 * @param dest location of the promoted content
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::promote(uint64_t *eval, size_t length,
                                             FieldT *dest) {
  for (size_t idx = 0; idx < length; idx++) {
    dest[idx] = FieldT(eval[idx]);
  }
}

/** performs low degree testing on the shared block provided
 * @param eval pointer to the shared block of field elements
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::degreeTest(FieldT *eval) {
  std::vector<uint64_t> eval64 = demote(eval, this->nNumberShares_);
  return this->degreeTest(&eval64[0]);
}

/** tests whether the polynomial corresponding to the shared block provided
 * evaluates to 0 on each point of the secret domain
 * @param eval pointer to the shared block of field elements
 * @param largerDegree boolean indicates whether the underlying polynomial is of
 * degree > k
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroTest(FieldT *eval, bool largerDegree) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], largerDegree);

  for (size_t i = 0; i < this->lSecretLength_; i++) {
    if (!(localCopy[i] == FieldT(0))) return false;
  }

  return true;
}

/** tests whether the sum of the evaluations for the polynomial corresponding to
 * the shared block over all points of the secret domain is 0
 * @param eval pointer to the shared block of field elements
 * @return boolean indicating whether the test succeeded or failed
 */
template <typename FieldT>
bool SecretSharingInterface<FieldT>::zeroSumTest(FieldT *eval) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  FieldT sum = FieldT(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  if (sum == FieldT(0))
    return true;
  else
    return false;
}

/** computes and returns the sum of the evaluations for the polynomial
 * corresponding to the shared block over all points of the secret domain
 * @param eval pointer to the shared block of field elements
 * @return sum of evaluations over the secret domain
 */
template <typename FieldT>
FieldT SecretSharingInterface<FieldT>::sumReconstruct(FieldT *eval) {
  std::vector<FieldT> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  FieldT sum = FieldT(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  return sum;
}

/** performs secret-sharing for multiple blocks at once. The sharing is
 * performed in-place, i.e. the shared block will be replacing the secret in
 * memory
 * @param secret pointer to a set of private blocks of field elements
 * @param lrows total number of blocks to share
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::shareMany(FieldT *secret, size_t lrows) {
  // std::cout << "Performing FFTs: " << lrows << std::endl;
  for (size_t i = 0; i < lrows; i++) {
    share(secret + i * this->nNumberShares_);
  }
}

/** reconstructs multiple private blocs based on the corresponding shared
 * elements at once. The sharing is performed in-place, i.e. the secrets will be
 * replacing the shared blocks in memory
 * @param eval pointer to a set of shared blocks of field elements
 * @param lrows total number of blocks to share
 */
template <typename FieldT>
void SecretSharingInterface<FieldT>::reconstructMany(FieldT *eval,
                                                     size_t lrows) {
  for (size_t i = 0; i < lrows; i++) {
    reconstruct(eval + i * this->nNumberShares_);
  }
}

/** pads intrabloc randomness with zeroes from the length of a block to the
 * polynomial degree
 * @param randomness pointer to a private block of field elements
 */
void SecretSharingNTT::padIntrablocRandomness(uint64_t *randomness) {
  for (size_t i = this->lSecretLength_; i < kDegree_; i++) {
    randomness[i] = uint64_t(0);
  }
}

/** pads polynomial coefficients with zeroes from degree k to number of shares n
 * @param coefs pointer to the coefficients
 */
void SecretSharingNTT::padPolynominal(uint64_t *coefs) {
  for (size_t i = this->kDegree_; i < nNumberShares_; i++) {
    coefs[i] = uint64_t(0);
  }
}

/** secret-sharing of a demoted vector of 64-bit secrets. The secret sharing is
 * performed in place so secrets are replaced by the shared block in memory
 * @param data pointer to the demoted vector of secrets
 */
void SecretSharingNTT::share(uint64_t *data) {
  // Interpolate Poly from Secret
  _smallDomain->inv_ntt(data);

  this->padPolynominal(data);

  // Evaluate Publicly Shared Data from Poly
  _largeDomain->ntt(data);
}

/** reconstructing a secret block based on the input shared block
 * so secrets are replaced by the shared block in memory
 * @param eval point to the shared block
 * @param exapandedDegree boolean indicating whether the polynomial linked to
 * this block has a degree > k
 */
void SecretSharingNTT::reconstruct(uint64_t *eval, bool expandedDegree) {
  size_t degree = kDegree_;

  // Interpolate Poly from Publicly Shared Data
  _largeDomain->inv_ntt(eval);

  // if (expandedDegree) degree = kDegree_*2;
  // Because we evaluate on roots of unity, we can use coefs[i] - coefs[i+k]
  // on k coefficients instead of a 2k evaluation
  if (expandedDegree) {
    for (size_t i = 0; i < kDegree_; i++) {
      if (eval[i + kDegree_] > eval[i]) {
        eval[i] = eval[i] - eval[i + kDegree_] +
                  params<uint64_t>::P[this->modulusIdx_];
      } else {
        eval[i] = eval[i] - eval[i + kDegree_];
      }
    }
  }

  // Evaluate Poly Into Secret
  _smallDomain->ntt(eval);
}

/** performs low degree testing on the demoted shared block provided
 * @param eval pointer to the shared block of demoted 64-bit numbers
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::degreeTest(uint64_t *eval) {
  // Interpolate Poly from Publicly Shared Data
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  _largeDomain->inv_ntt(&localCopy[0]);

  for (size_t i = kDegree_; i < nNumberShares_; i++) {
    if (!(localCopy[i] == uint64_t(0))) {
      DBG("val:" << localCopy[i] << ",degree>" << i);
      return false;
    }
  }

  return true;
}

/** tests whether the polynomial corresponding to the demoted shared block
 * provided evaluates to 0 on each point of the secret domain
 * @param eval pointer to the demoted shared block of 64-bit numbers
 * @param largerDegree boolean indicates whether the underlying polynomial is of
 * degree > k
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::zeroTest(uint64_t *eval, bool largerDegree) {
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], largerDegree);

  for (size_t i = 0; i < this->lSecretLength_; i++) {
    if (!(localCopy[i] == uint64_t(0))) return false;
  }

  return true;
}

/** tests whether the sum of the evaluations for the polynomial corresponding to
 * the demoted shared block over all points of the secret domain is 0
 * @param eval pointer to the demoted shared block of 64-bit numbers
 * @return boolean indicating whether the test succeeded or failed
 */
bool SecretSharingNTT::zeroSumTest(uint64_t *eval) {
  std::vector<uint64_t> localCopy(eval, eval + this->nNumberShares_);
  reconstruct(&localCopy[0], true);

  uint64_t sum = uint64_t(0);
  for (size_t i = 0; i < this->lSecretLength_; i++) {
    sum += localCopy[i];
  }

  if (sum == uint64_t(0))
    return true;
  else
    return false;
}

/** performs secret-sharing for multiple demoted blocks at once. The sharing is
 * performed in-place, i.e. the shared block will be replacing the secret in
 * memory
 * @param secret pointer to a set of demoted private blocks of 64-bit numbers
 * @param lrows total number of blocks to share
 */
void SecretSharingNTT::shareMany(uint64_t *secret, size_t lrows) {
  // std::cout << "Performing FFTs: " << lrows << std::endl;
  for (size_t i = 0; i < lrows; i++) {
    share(secret + i * this->nNumberShares_);
  }
}

/** reconstructs multiple private blocs based on the corresponding demoted
 * shared elements at once. The sharing is performed in-place, i.e. the secrets
 * will be replacing the shared blocks in memory
 * @param eval pointer to a set of demoted shared blocks of 64-bit numbers
 * @param lrows total number of blocks to share
 */
void SecretSharingNTT::reconstructMany(uint64_t *secret, size_t lrows) {
  for (size_t i = 0; i < lrows; i++) {
    reconstruct(secret + i * this->nNumberShares_);
  }
}

}  // namespace ligero


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/harmonic_map.h>
#include <cinolib/laplacian.h>
#include <Eigen/Sparse>

namespace cinolib
{

template<class M, class V, class E, class P>
CINO_INLINE
ScalarField harmonic_map(const AbstractMesh<M,V,E,P> & m,
                         const std::map<unsigned int,double> & bc,
                         const unsigned int                    n,
                         const int                     laplacian_mode,
                         const int                     solver)
{
    assert(n > 0);
    assert(bc.size() > 0);
    assert(laplacian_mode == COTANGENT || laplacian_mode == UNIFORM);
    assert(solver == SIMPLICIAL_LLT || solver == SIMPLICIAL_LDLT || solver == SparseLU || solver == BiCGSTAB);

    ScalarField f(m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode);
    Eigen::SparseMatrix<double> Ln = -L;
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(m.num_verts());

    for(unsigned int i=1; i<n; ++i) Ln  = Ln * (-L); // keep it PSD

    solve_square_system_with_bc(Ln, rhs, f, bc, solver);

    return f;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<vec3d> harmonic_map_3d(const AbstractMesh<M,V,E,P> & m,
                                   const std::map<unsigned int,vec3d>  & bc,
                                   const unsigned int                    n,
                                   const int                     laplacian_mode,
                                   const int                     solver)
{
    assert(n > 0);
    assert(bc.size() > 0);
    assert(laplacian_mode == COTANGENT || laplacian_mode == UNIFORM);
    assert(solver == SIMPLICIAL_LLT || solver == SIMPLICIAL_LDLT || solver == SparseLU || solver == BiCGSTAB);

    ScalarField f(3*m.num_verts());

    Eigen::SparseMatrix<double> L   = laplacian(m, laplacian_mode, 3);
    Eigen::SparseMatrix<double> Ln = -L;
    Eigen::VectorXd             rhs = Eigen::VectorXd::Zero(3*m.num_verts());

    for(unsigned int i=1; i<n; ++i) Ln  = Ln * (-L); // keep it PSD

    unsigned int y_off = m.num_verts();
    unsigned int z_off = m.num_verts() + y_off;
    std::map<unsigned int,double> bc_1d;
    for(auto obj : bc)
    {
        unsigned int  vid = obj.first;
        vec3d pos = obj.second;
        bc_1d[      vid] = pos.x();
        bc_1d[y_off+vid] = pos.y();
        bc_1d[z_off+vid] = pos.z();
    }

    solve_square_system_with_bc(Ln, rhs, f, bc_1d, solver);

    std::vector<vec3d> res(m.num_verts());
    for(unsigned int vid=0; vid<m.num_verts(); ++vid)
    {
        res.at(vid) = vec3d(f[vid], f[y_off+vid], f[z_off+vid]);
    }

    return res;
}

}


// Copyright (C) INRIA 1999-2008
//
// This program is free software; you can redistribute it and/or modify it
// under the terms of the GNU General Public License version 2 as published
// by the Free Software Foundation.
//
// 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 GNU General
// Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//%
// @file ActuationModel/NoDynamics/TaskFunctionControl/ControlLaw.cpp
// @author Florence Billet
//
// Affiliation(s): INRIA, team BIPOP
//
// Email(s): Florence.Billet@inria.fr
//
// @brief Compute the voltage required to follow the reference trajectory
//
#ifdef _WIN32
#define SICONOS_EXPORT extern "C" __declspec(dllexport)
#else
#define SICONOS_EXPORT extern "C"
#endif
#include <iostream>
#include <string>

#include <boost/numeric/bindings/atlas/clapack.hpp>
#include <boost/numeric/ublas/io.hpp>

extern "C" {
#include "Trajectory.h"
#include "TaskFunctionDefinition.h"

#include "LagrangianModel.h"
#include "SomeDefinitions.h"
}

#include "KernelSomeDefinitions.hpp"
#include "Utils.hpp"

//#include "../../../LagrangianDynamics/Complete/util.hpp" //a mettre ailleurs un jour

SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
  int ndof;
  int contacts;
  char lagrangianModelName[20] = "";

  getLagrangianModelName(lagrangianModelName);
  getActiveDOFsize(&ndof);
  vector<int> activeDOF(ndof);
  if (ndof > 0)
    getActiveDOF(&(activeDOF[0]));
  else
    ndof = *NDOF;

  ///////////////////////////////////////////
  // Computation of the Lagrangian model
  ///////////////////////////////////////////

  matrix<double, column_major> M(*NDOF, *NDOF);
  vector<double> N(*NDOF);

  if (strcmp(lagrangianModelName, "Human36") == 0)
  {
    double L[31];
    double addl[84];
    double mass;
    GetModelMass(&mass);
    GetAnatomicalLengths(L);
    GetTag2JointLengths(addl);
    InertiaH36(&(M(0, 0)), q, L, addl, mass);
    NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
  }
  else
  {
    Inertia(&(M(0, 0)), q);
    NLEffects(&(N[0]), q, qdot);
  }

  //////////////////////////////////////////////////
  //Additionnal forces
  /////////////////////////////////////////////////

  vector<double> fAdditionnal(*NDOF);
  if (strcmp(lagrangianModelName, "PA10") == 0)
    Friction(&(fAdditionnal[0]), q, qdot);
  else if (strcmp(lagrangianModelName, "RX90") == 0)
    SpringForce(&(fAdditionnal[0]), q);
  else
    fAdditionnal.clear();

  ///////////////////////////////////////////////////
  // Limitation to the selected degrees of freedom
  ///////////////////////////////////////////////////
  reduceMatrix(M, activeDOF, activeDOF);
  N = reduceVector(N, activeDOF);
  fAdditionnal = reduceVector(fAdditionnal, activeDOF);

  //////////////////////////////////////////////////
  // Proportionnel-derivee in the task space
  //////////////////////////////////////////////////

  vector<double> sDesired(*NDOF);
  vector<double> sdotDesired(*NDOF);
  vector<double> sddotDesired(*NDOF);

  trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);

  //v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
  vector<double> TF(*NDOF);
  vector<double> h(*NDOF);
  matrix<double, column_major> H(*NDOF, *NDOF);

  TaskFunction(&(TF[0]), q);
  TaskJacobian(&(H(0, 0)), q);
  TaskNLEffects(&(h[0]), q, qdot);

  double Kp = 100.0;
  double Kv = 30.0;
  vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
  sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;

  /////////////////////////////////////////////////////////////
  // Generalized Torques to make the realisation of the command
  /////////////////////////////////////////////////////////////

  //qddot = inv(H)*v;
  vector<int> ipiv(*NDOF);
  boost::numeric::bindings::atlas::getrf(H, ipiv);
  boost::numeric::bindings::atlas::getri(H, ipiv);
  sddotDesired = prod(H, sddotDesired);

  //D = M*qddot(ActiveDOF) + N + FAdditionnal;
  sddotDesired = reduceVector(sddotDesired, activeDOF);

  N += prod(M, sddotDesired) + fAdditionnal;

  //nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
  //Torques = zeros(q);
  //Torques(ActiveDOF(1:nmot)) = D(1:nmot);
  for (int i = 0; i < *NDOF; i++)
    torques[i] = 0;
  expandVector(torques, N, activeDOF);

}