#include <utility>
#include <random>
#include "Gain.h"
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Gain.h>
#include <valarray>
#include <set>
#include <iostream>
#include "RNG.h"
#include "Utils.h"

using Eigen::VectorXd;
using Eigen::MatrixXd;

Gain::Gain(std::set<double> new_times_amp, std::set<double> new_times_phase) :
    logamp_amp(-3.0),
    logamp_phase(-2.0),
    logscale_amp(7.0),
    logscale_phase(5.0),
    phase_mean(0.0)

    {
    // Get unique times of gain amplitudes
    std::vector<double> times_amp_vec;
    times_amp_vec.assign(new_times_amp.begin(), new_times_amp.end());
    // Get unique times of gain phases
    std::vector<double> times_phase_vec;
    times_phase_vec.assign(new_times_phase.begin(), new_times_phase.end());
    // Convert to Eigen VectorXd
    times_amp = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_amp_vec.data(), times_amp_vec.size());
    times_phase = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_phase_vec.data(), times_phase_vec.size());

    amplitudes = std::valarray<double>(1.0, times_amp.size());
    phases = std::valarray<double>(0.0, times_amp.size());
    v_amp = std::valarray<double>(0.0, times_amp.size());
    v_phase = std::valarray<double>(0.0, times_phase.size());
    C_amp = MatrixXd(times_amp.size(), times_amp.size());
    C_phase = MatrixXd(times_phase.size(), times_phase.size());
    L_amp = MatrixXd(times_amp.size(), times_amp.size());
    L_phase = MatrixXd(times_phase.size(), times_phase.size());

    calculate_C_amp();
    calculate_C_phase();
    calculate_L_amp();
    calculate_L_phase();
    }

Gain::Gain(std::set<double> new_times) :
    logamp_amp(-3.0),
    logamp_phase(-2.0),
    logscale_amp(7.0),
    logscale_phase(5.0),
    phase_mean(0.0)

    {
    // Get unique times of gain amplitude & phase
    std::vector<double> times_vec;
    times_vec.assign(new_times.begin(), new_times.end());
    // Convert to Eigen VectorXd
    times_amp = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_vec.data(), times_vec.size());
    times_phase = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(times_vec.data(), times_vec.size());

    amplitudes = std::valarray<double>(1.0, times_amp.size());
    phases = std::valarray<double>(0.0, times_amp.size());
    v_amp = std::valarray<double>(0.0, times_amp.size());
    v_phase = std::valarray<double>(0.0, times_phase.size());
    C_amp = MatrixXd(times_amp.size(), times_amp.size());
    C_phase = MatrixXd(times_phase.size(), times_phase.size());
    L_amp = MatrixXd(times_amp.size(), times_amp.size());
    L_phase = MatrixXd(times_phase.size(), times_phase.size());

    calculate_C_amp();
    calculate_C_phase();
    calculate_L_amp();
    calculate_L_phase();
    }

//Gain::Gain(const Gain &other) {
//    logamp_amp = other.logamp_amp;
//    logamp_phase = other.logamp_phase;
//    logscale_amp = other.logscale_amp;
//    logscale_phase = other.logscale_phase;
//    times_amp = other.times_amp;
//    times_phase = other.times_phase;
//    amplitudes =other.amplitudes;
//    phases = other.phases;
//    v_amp = other.v_amp;
//    v_phase = other.v_phase;
//    C_amp = other.C_amp;
//    C_phase = other.C_phase;
//    L_amp = other.L_amp;
//    L_phase = other.L_phase;
//}

void Gain::print_times(std::ostream &out) const
{
    out << "times amp = " << std::endl;
    for (int i=0; i<times_amp.size(); i++) {
        out << times_amp[i] << ", ";
    }
    out << std::endl;

    out << "times phase = " << std::endl;
    for (int i=0; i<times_phase.size(); i++) {
        out << times_phase[i] << ", ";
    }
    out << std::endl;

}

void Gain::print_hp(std::ostream &out) const
{
    out << "For amplitude: logamplitude = " << logamp_amp << ", " << "logscale = " << logscale_amp << std::endl;
    out << "For phase: logamplitude = " << logamp_phase << ", " << "logscale = " << logscale_phase << std::endl;

}

void Gain::print_v(std::ostream &out) const
{
    out << "v amp = " << std::endl;
    for (int i=0; i<v_amp.size(); i++) {
        out << v_amp[i] << ", ";
    }
    out << std::endl;

    out << "v_phase = " << std::endl;
    for (int i=0; i<v_phase.size(); i++) {
        out << v_phase[i] << ", ";
    }
    out << std::endl;
}

void Gain::print_C(std::ostream &out) const
{
    out << "C amp = " << std::endl;
    out << C_amp << std::endl;
    out << "C phase = " << std::endl;
    out << C_phase << std::endl;
}

void Gain::print_L(std::ostream &out) const
{
    out << "L amp = " << std::endl;
    out << L_amp << std::endl;
    out << "L phase= " << std::endl;
    out << L_phase << std::endl;
}

void Gain::set_hp_amp(std::valarray<double> params) {
    logamp_amp = params[0];
    logscale_amp = params[1];
}

void Gain::set_hp_phase(std::valarray<double> params) {
    logamp_phase = params[0];
    logscale_phase = params[1];
}

//void Gain::set_v_amp(std::valarray<double> params) {
//    v_amp = std::move(params);
//}
//
//
//void Gain::set_v_phase(std::valarray<double> params) {
//    v_phase = std::move(params);
//}

void Gain::from_prior_v_amp() {
    //v_amp = make_normal_random(size_amp());
    v_amp = std::valarray<double> (0.0, size_amp());
}

void Gain::from_prior_v_phase() {
    //v_phase = make_normal_random(size_phase());
    v_phase = std::valarray<double> (0.0, size_phase());
}

void Gain::from_prior_phase_mean(DNest4::RNG& rng) {
    //phase_mean = -1.0*M_PI + 2.0*M_PI*rng.rand();
    phase_mean = 0.0;
}

void Gain::from_prior_hp_amp(DNest4::RNG& rng) {
    //std::cout << "Generating from prior Gain HP AMP" << std::endl;
    //logamp_amp = -3.0 + 1.0*rng.randn();
    //logscale_amp = 7.0 + 1.0*rng.randn();
    logamp_amp = -3.0;
    logscale_amp = 7.0;
}

void Gain::from_prior_hp_phase(DNest4::RNG& rng) {
    //logamp_phase = -3.0 + 1.0*rng.randn();
    //logscale_phase = 5.0 + 1.0*rng.randn();
    logamp_phase = -2.0;
    logscale_phase = 5.0;
}

void Gain::calculate_C_amp() {
    //std::cout << "Calculating C of amp GP" << std::endl;
    Eigen::MatrixXd sqdist = - 2*times_amp*times_amp.transpose();
    sqdist.rowwise() += times_amp.array().square().transpose().matrix();
    sqdist.colwise() += times_amp.array().square().matrix();
    sqdist *= (-0.5/(exp(2.0*logscale_amp)));
    C_amp = exp(logamp_amp) * sqdist.array().exp();
}

void Gain::calculate_C_phase() {
    Eigen::MatrixXd sqdist = - 2*times_phase*times_phase.transpose();
    sqdist.rowwise() += times_phase.array().square().transpose().matrix();
    sqdist.colwise() += times_phase.array().square().matrix();
    sqdist *= (-0.5/(exp(2.0*logscale_phase)));
    C_phase = exp(logamp_phase) * sqdist.array().exp();
}

void Gain::calculate_L_amp() {
    //std::cout << "Calculating L of amp GP" << std::endl;
    // perform the Cholesky decomposition of covariance matrix
    Eigen::LLT<Eigen::MatrixXd> cholesky = C_amp.llt();
    // get the lower triangular matrix L
    L_amp = cholesky.matrixL();
}

void Gain::calculate_L_phase() {
    // perform the Cholesky decomposition of covariance matrix
    Eigen::LLT<Eigen::MatrixXd> cholesky = C_phase.llt();
    // get the lower triangular matrix L
    L_phase = cholesky.matrixL();
}

int Gain::size_amp() {
    return times_amp.size();
}

int Gain::size_phase() {
    return times_phase.size();
}

std::valarray<double>& Gain::get_amplitudes() {
    return amplitudes;
}

std::valarray<double>& Gain::get_phases() {
    return phases;
}

void Gain::calculate_amplitudes() {
    //std::cout << "Calculating amplitudes" << std::endl;
    // Convert std::valarray ``v_amp`` to Eigen::VectorXd
    VectorXd v_amp_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&v_amp[0], v_amp.size());

    //// Debug print out v
    //std::valarray<double> v_amp = std::valarray<double>(v_amp_vec.data(), v_amp_vec.size());
    ////std::cout << "V amplitudes = " << std::endl;
    //for (int i=0; i < times_amp.size(); i++) {
    //    std::cout << v_amp[i] << ", ";
    //}
    //std::cout << std::endl;

    VectorXd amp = L_amp*v_amp_vec;
    // Convert Eigen::VectorXd to std::valarray
    amplitudes = 1.0 + std::valarray<double>(amp.data(), amp.size());
    //std::cout << "Calculated amplitudes: " << std::endl;
    //print_amplitudes(std::cout);

}

void Gain::calculate_phases() {
    // Convert std::valarray ``v_amp`` to Eigen::VectorXd
    VectorXd v_phase_vec = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&v_phase[0], v_phase.size());

    VectorXd phase = L_phase*v_phase_vec;

    //// Debug print out v
    //std::valarray<double> v_phases = std::valarray<double>(v_phase_vec.data(), v_phase_vec.size());
    //std::cout << "V phases = " << std::endl;
    //for (int i=0; i < times_phase.size(); i++) {
    //    std::cout << v_phases[i] << ", ";
    //}
    //std::cout << std::endl;

    // Convert Eigen::VectorXd to std::valarray
    phases = phase_mean + std::valarray<double>(phase.data(), phase.size());
    //std::cout << "Calculated phases: " << std::endl;
    //print_phases(std::cout);

}

void Gain::print_amplitudes(std::ostream &out) const {
    out << "amplitudes = " << std::endl;
    for (int i=0; i < times_amp.size(); i++) {
        out << amplitudes[i] << ", ";
    }
    out << std::endl;
}

void Gain::print_phases(std::ostream &out) const {
    out << "phases = " << std::endl;
    for (int i=0; i < times_phase.size(); i++) {
        out << phases[i] << ", ";
    }
    out << std::endl;
}

double Gain::perturb(DNest4::RNG &rng) {
    double logH = 0.;

    int which = rng.rand_int(2);
    // Amplitude GP
    if(which == 0) {

        // Choose what value to perturb
        int which = rng.rand_int(amplitudes.size());

        logH -= -0.5*pow(v_amp[which], 2.0);
        v_amp[which] += rng.randn();
        logH += -0.5*pow(v_amp[which], 2.0);

        // Pre-reject
        if(rng.rand() >= exp(logH)) {
            return -1E300;
        }
        else
            logH = 0.0;

        calculate_amplitudes();
    }
    // Phase GP
    else {

        // More often perturb phase latent variables
        if(rng.rand() <= 0.9) {

            // Choose what value to perturb
            int which = rng.rand_int(phases.size());

            logH -= -0.5*pow(v_phase[which], 2.0);
            v_phase[which] += rng.randn();
            logH += -0.5*pow(v_phase[which], 2.0);

            // Pre-reject
            if (rng.rand() >= exp(logH)) {
                return -1E300;
            } else
                logH = 0.0;

        }
        else {
            //logH -= -0.5*pow(phase_mean/1.5, 2.0);
            phase_mean += 2.0*M_PI*rng.randh();
            //logH += -0.5*pow(phase_mean/1.5, 2.0);
            DNest4::wrap(phase_mean, -1.0*M_PI, M_PI);

            //// Pre-reject
            //if (rng.rand() >= exp(logH)) {
            //    return -1E300;
            //} else
            //    logH = 0.0;
        }

        calculate_phases();

    }

    return logH;
}

std::string Gain::description() const {
    std::string descr;
    for (int i = 0; i < times_amp.size(); i++) {
        descr += ("amp" + std::to_string(i) + " ");
    }
    descr += "phase_mean ";
    for (int i = 0; i < times_phase.size(); i++) {
        descr += ("phase" + std::to_string(i) + " ");
    }
    descr.pop_back();
    return descr;
}

void Gain::print(std::ostream &out) const {
    for (int i = 0; i < times_amp.size(); i++) {
        out << amplitudes[i] << '\t';
    }
    out << phase_mean << '\t';
    for (int i = 0; i < times_phase.size(); i++) {
        out << phases[i] << '\t';
    }
}

std::valarray<double> make_normal_random(int number)
{
    std::random_device rd;
    std::normal_distribution<double> normalDistr(0, 1);
    std::minstd_rand generator(rd());
    std::valarray<double> randNums(number);

    for (int i=0; i<number; i++) {
        randNums[i] = normalDistr(generator);
    }
    return randNums;
}


/*!
 * @file
 * @brief
 * @copyright alphya 2021
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef NYARUGA_UTIL_ALGEBRA_HPP
#define NYARUGA_UTIL_ALGEBRA_HPP

#pragma once

#include <functional>
#include <iostream>
#include <concepts>
#include <ranges>
#include <ratio>
#include <complex>
#include <type_traits>
#include <array>
#include <algorithm>
#include <utility>
#include <boost/hana/functional/arg.hpp>

#include <nyaruga_util/config.hpp>

#ifdef NYARUGA_UTIL_HAS_TCBRINDLER_RATIONAL
#    include <tcb/rational.hpp> // https://github.com/tcbrindle/rational.git
#endif

namespace nyaruga {

namespace util {

namespace detail {

template <std::unsigned_integral  T = unsigned int>
struct N {
    using value_type = T;
    T value;
    friend constexpr auto operator<=>(const N&, const N&) noexcept = default;
    constexpr operator T() const noexcept { return value; }
    constexpr N(T&& v) noexcept : value(std::forward<decltype(v)>(v)) {}
};

template <std::floating_point T = double>
struct R {
    using value_type = T;
    T value;
    friend constexpr auto operator<=>(const R&, const R&) noexcept = default;
    constexpr operator T() const noexcept { return value; }
    constexpr R(T&& v) noexcept : value(std::forward<decltype(v)>(v)) {}
};

template <std::integral T = int>
struct Z {
    using value_type = T;
    T value;
    friend constexpr auto operator<=>(const Z&, const Z&) noexcept = default;
    constexpr operator T() const noexcept { return value; }
    constexpr Z(T&& v) noexcept : value(std::forward<decltype(v)>(v)) {}
};

template <typename T>
concept  arithmetic = std::is_arithmetic_v<T>;

} // namespace detail

// Number

using detail::N;

using detail::Z;

using detail::R;

#ifdef NYARUGA_UTIL_HAS_TCBRINDLER_RATIONAL
template <std::integral T = int>
using Q = tcb::rational<T>;
#endif

template <detail::arithmetic T = double>
using C = std::complex<T>;

namespace detail {

// 足し算ができる
template <class T, class U>
concept weak_addable = requires (const std::remove_reference_t<T>& a, const std::remove_reference_t<U>& b) {
  {a + b} -> std::common_with<T>;
  {b + a} -> std::common_with<T>;
  {a + b} -> std::common_with<U>;
  {b + a} -> std::common_with<U>;
};

// 引き算ができる
template <class T, class U>
concept weak_subtractable = requires (const std::remove_reference_t<T>& a, const std::remove_reference_t<U>& b) {
  {a - b} -> std::common_with<T>;
  {b - a} -> std::common_with<T>;
  {a - b} -> std::common_with<U>;
  {b - a} -> std::common_with<U>;
};

// 掛け算ができる
template <class T, class U>
concept weak_multipliable = requires (const std::remove_reference_t<T>& a, const std::remove_reference_t<U>& b) {
  {a * b} -> std::common_with<T>;
  {b * a} -> std::common_with<T>;
  {a * b} -> std::common_with<U>;
  {b * a} -> std::common_with<U>;
};

//割り算ができる
template <class T, class U>
concept weak_dividable = requires (const std::remove_reference_t<T>& a, const std::remove_reference_t<U>& b){
  {a / b} -> std::common_with<T>;
  {b / a} -> std::common_with<T>;
  {a / b} -> std::common_with<U>;
  {b / a} -> std::common_with<U>;
};

} // namespace detail

// concept

template <typename T>
concept addable = detail::weak_addable<T,T>;

template <typename T>
concept subtractable = detail::weak_subtractable<T,T>;

template <typename T>
concept multipliable = detail::weak_multipliable<T,T>;

template <typename T>
concept dividable = detail::weak_dividable<T,T>;

template <class T, class U>
concept addable_with =
  addable<T> &&
  addable<U> &&
  std::common_reference_with<
    const std::remove_reference_t<T>&,
    const std::remove_reference_t<U>&> &&
  addable<
    std::common_reference_t<
      const std::remove_reference_t<T>&,
      const std::remove_reference_t<U>&>> &&
  detail::weak_addable<T, U>;

template <class T, class U>
concept subtractable_with =
  subtractable<T> &&
  subtractable<U> &&
  std::common_reference_with<
    const std::remove_reference_t<T>&,
    const std::remove_reference_t<U>&> &&
  subtractable<
    std::common_reference_t<
      const std::remove_reference_t<T>&,
      const std::remove_reference_t<U>&>> &&
  detail::weak_subtractable<T, U>;

template <class T, class U>
concept multipliable_with =
  multipliable<T> &&
  multipliable<U> &&
  std::common_reference_with<
    const std::remove_reference_t<T>&,
    const std::remove_reference_t<U>&> &&
  multipliable<
    std::common_reference_t<
      const std::remove_reference_t<T>&,
      const std::remove_reference_t<U>&>> &&
  detail::weak_multipliable<T, U>;

template <class T, class U>
concept dividable_with =
  dividable<T> &&
  dividable<U> &&
  std::common_reference_with<
    const std::remove_reference_t<T>&,
    const std::remove_reference_t<U>&> &&
  dividable<
    std::common_reference_t<
      const std::remove_reference_t<T>&,
      const std::remove_reference_t<U>&>> &&
  detail::weak_dividable<T, U>;

template <typename T>
concept group = // std::equality_comparable<T> && // 等しさの検証が難しい対象もあるため ex) 関数環
                                 addable<T> &&
                                 subtractable<T>;

template <typename T>
concept abelian_group =  // C++ 上で普通の群と見分ける方法なし
                                 // std::equality_comparable<T> &&
                                 addable<T> &&
                                 subtractable<T>;

template <typename T>
concept ring = // std::equality_comparable<T> &&
                             addable<T> &&
                             subtractable<T> &&
                             multipliable<T>;

template <typename T>
concept commutative_ring =  // C++ 上で普通の環と見分ける方法なし
                             // std::equality_comparable<T> &&
                             addable<T> &&
                             subtractable<T> &&
                             multipliable<T>;

template <typename T>
concept field = // std::equality_comparable<T> &&
                              addable<T> &&
                              subtractable<T> &&
                              multipliable<T> &&
                              dividable<T>;

namespace detail {

template <ring A = R<double>, size_t rank = 1>
struct module : std::array<A, rank>
{
    using std::array<A,rank>::array;
    constexpr module() noexcept = default;
    constexpr module(const std::array<A,rank>& a) : std::array<A,rank>::array(a) {};
    constexpr module(std::array<A,rank>&& a) : std::array<A,rank>::array(std::move(a)) {};
    template <std::same_as<A> ... T>
    constexpr module(T ... list) : std::array<A,rank>::array({list...}) {};
    friend constexpr auto operator<=>(const module&, const module&) noexcept = default;

};

template <typename ... T>
module(T ... list) -> module<std::remove_cvref_t<decltype(boost::hana::arg<1>(list...))> ,sizeof...(list)>;

namespace my_detail {

template <typename T, size_t N, size_t... I>
constexpr std::array<std::remove_cv_t<T>, N>
    array_plus_impl(const std::array<T,N>& a, const std::array<T,N>& b, std::index_sequence<I...>)
{
    return { {(a[I]+b[I])...} };
}

template <typename T, size_t N, size_t... I>
constexpr std::array<std::remove_cv_t<T>, N>
    array_mult_impl(const T& a, const std::array<T,N>& b, std::index_sequence<I...>)
{
    return { {(a*b[I])...} };
}

}

template <typename T, size_t N>
constexpr module<T,N>  operator+(std::array<T,N>& a, std::array<T,N>& b)
{
    return my_detail::array_plus_impl(a, b,std::make_index_sequence<N>{});
}

template <typename T, size_t N>
constexpr module<T,N>  operator*(const T& a, std::array<T,N>& b)
{
    return my_detail::array_mult_impl(a, b,std::make_index_sequence<N>{});
}

} // namespace detail

// 加群
template <ring A, size_t rank>
using module_t = detail::module<A, rank>;

template <typename A, typename G>
concept module = ring<A> && abelian_group<G> &&
requires(const std::remove_reference_t<A>& a, const std::remove_reference_t<A>& b,
const std::remove_reference_t<G>& g, const std::remove_reference_t<G>&  h)
{
     {a*g} -> std::common_with<G>;
     {g+h} -> std::common_with<G>;
     {g-h} -> std::common_with<G>;
     {a+b} -> std::common_with<A>;
     {a*b}-> std::common_with<A>;
     // { (a + b)(g + h) == (a+b)*g+(a+b)*h };
};

// 多元環または代数(algebra) 係数環の可換性を確かめられないので、C++上だと加群と変わらず
template <commutative_ring A, size_t rank>
using algebra_t = detail::module<A, rank>;

template <typename A, typename G>
concept algebra = module<A,G>;

} // namespace util

} // namespace nyaruga

#endif // #ifndef NYARUGA_UTIL_ALGEBRA_HPP


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkRansacPlaneModel.cxx
  Author: Pierre Guilbert

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

// LOCAL
#include "vtkRansacPlaneModel.h"

#include "vtkConversions.h"

// STD
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>

// VTK
#include <vtkCellArray.h>
#include <vtkCellData.h>
#include <vtkDataArray.h>
#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkInformationVector.h>
#include <vtkMath.h>
#include <vtkNew.h>
#include <vtkObjectFactory.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyLine.h>
#include <vtkSmartPointer.h>
#include <vtkStreamingDemandDrivenPipeline.h>
#include <vtkTransform.h>
#include <vtkTupleInterpolator.h>
#include <vtkUnsignedCharArray.h>
#include <vtkUnsignedIntArray.h>
#include <vtkUnsignedShortArray.h>

// BOOST
#include <boost/algorithm/string.hpp>

// Eigen
#include <Eigen/Dense>

//----------------------------------------------------------------------------
struct RansacSampleInfo
{
public:
  RansacSampleInfo() {}

  RansacSampleInfo(unsigned int nInliers, unsigned int index1,
                   unsigned int index2, unsigned int index3)
  {
    this->NInliers = nInliers;
    this->Index1 = index1;
    this->Index2 = index2;
    this->Index3 = index3;
  }

  unsigned int NInliers;
  unsigned int Index1;
  unsigned int Index2;
  unsigned int Index3;
};

//----------------------------------------------------------------------------
void RefineRansac(std::vector<Eigen::Matrix<double, 3, 1> >& Points, vtkPolyData* output,
                  RansacSampleInfo sampleInfo, double threshold, double PlaneParam[4])
{
  // Create inliers / outliers array information
  vtkNew<vtkUnsignedIntArray> inliersArray;
  inliersArray->SetName("ransac_plane_inliers");

  // compute plane PlaneParameters
  Eigen::Matrix<double, 3, 1> pointPlane = Points[sampleInfo.Index1];
  Eigen::Matrix<double, 3, 1> normalPlane = (Points[sampleInfo.Index3] - pointPlane).cross(Points[sampleInfo.Index2] - pointPlane);
  normalPlane.normalize();

  // compute inliers
  std::vector<Eigen::Matrix<double, 3, 1> > inliersPoints;
  for (unsigned int k = 0; k < Points.size(); ++k)
  {
    if (std::abs((Points[k] - pointPlane).dot(normalPlane)) < threshold)
    {
      inliersPoints.push_back(Points[k]);
      inliersArray->InsertNextValue(1);
    }
    else
    {
      inliersArray->InsertNextValue(0);
    }
  }

  // Now, compute the best plane using all inliers
  Eigen::MatrixXd centeredSamples(3, inliersPoints.size());
  Eigen::Matrix<double, 3, 1> center = Eigen::Matrix<double, 3, 1>::Zero();
  for (unsigned int k = 0; k < inliersPoints.size(); ++k)
  {
    centeredSamples.col(k) = inliersPoints[k];
    center += inliersPoints[k];
  }
  center /= static_cast<double>(inliersPoints.size());
  for (unsigned int k = 0; k < inliersPoints.size(); ++k)
  {
    centeredSamples.col(k) -= center;
  }
  Eigen::Matrix<double, 3, 3> varianceCovariance = centeredSamples * centeredSamples.transpose();
  varianceCovariance /= static_cast<double>(inliersPoints.size());

  // since the variance covariance matrix is a real
  // symmetric matrix it can be diagonalized in a orthonormal
  // basis. We will use the AutoAdjoint eigen solver
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3> > eigenSolver(varianceCovariance);

  // PlaneParameters
  normalPlane = eigenSolver.eigenvectors().col(0);
  pointPlane = center;
  PlaneParam[0] = normalPlane(0);
  PlaneParam[1] = normalPlane(1);
  PlaneParam[2] = normalPlane(2);
  PlaneParam[3] = -normalPlane.dot(pointPlane);

  output->GetPointData()->AddArray(inliersArray.Get());
}

//----------------------------------------------------------------------------
unsigned int ComputeNumberOfInlier(std::vector<Eigen::Matrix<double, 3, 1> >& Points, Eigen::Matrix<double, 3, 1> planePoint,
                                   Eigen::Matrix<double, 3, 1> planeNormal, double threshold)
{
  unsigned int nInliers = 0;
  for (unsigned int k = 0; k < Points.size(); ++k)
  {
    if (std::abs((Points[k] - planePoint).dot(planeNormal)) < threshold)
    {
      nInliers++;
    }
  }

  return nInliers;
}

// Implementation of the New function
vtkStandardNewMacro(vtkRansacPlaneModel)

//----------------------------------------------------------------------------
vtkRansacPlaneModel::vtkRansacPlaneModel()
{
  // 500 maximal ransac iteration
  this->MaxRansacIteration = 500;

  // 50 cm distance to plane threshold
  this->Threshold = 0.5;

  // 30% of inliers required
  this->RatioInliersRequired = 0.30;

  // fill PlaneParams with 0 values
  std::fill(this->PlaneParam, this->PlaneParam + 4, 0);

  this->AlignOutput = false;
  this->TemporalAveraging = true;
  this->MaxTemporalAngleChange = 45.0;
  this->PreviousEstimationWeight = 0.9;
}

//----------------------------------------------------------------------------
vtkRansacPlaneModel::~vtkRansacPlaneModel()
{
}

//-----------------------------------------------------------------------------
void vtkRansacPlaneModel::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

//-----------------------------------------------------------------------------
int vtkRansacPlaneModel::RequestData(vtkInformation *vtkNotUsed(request),
  vtkInformationVector **inputVector, vtkInformationVector *outputVector)
{
  // Save the previous plane estimate
  double prevPlaneEst[4];
  std::copy(this->PlaneParam, this->PlaneParam + 4, prevPlaneEst);

  // Get the input
  vtkPolyData * input = vtkPolyData::GetData(inputVector[0]->GetInformationObject(0));

  // Get the output
  vtkPolyData *output = vtkPolyData::GetData(outputVector->GetInformationObject(0));
  output->ShallowCopy(input);

  // Convert the point cloud in Eigen data structure point cloud
  std::vector<Eigen::Vector3d> Points = vtkPointsToEigenVector(input->GetPoints());

  // Create a random order of points index
  std::vector<double> randomIndex(input->GetNumberOfPoints(), 0);
  for (unsigned int k = 0; k < randomIndex.size(); ++k)
  {
    randomIndex[k] = k;
  }
  std::random_shuffle(randomIndex.begin(), randomIndex.end());

  // information variable about ransac iterations
  bool shouldStopRansac = false;
  unsigned int iterationMade = 0;
  unsigned int iterationPointer = 0;

  // keep information of iterations
  std::vector<RansacSampleInfo> samplesInfo;
  unsigned int maxInliers = 0;
  unsigned int indexMaxInliers = 0;

  // Affine plane PlaneParameters
  Eigen::Matrix<double, 3, 1> planeNormal, planePoint;

  // indicate if ransac has "converged"
  bool hasConverged = false;

  // Ransac loop
  while (!shouldStopRansac)
  {
    // if we went throught the all random index we need
    // to random shuffle again
    if ((iterationPointer + 2) >= randomIndex.size())
    {
      std::random_shuffle(randomIndex.begin(), randomIndex.end());
      iterationPointer = 0;
    }

    // Compute current sample plane PlaneParameters
    planePoint = Points[randomIndex[iterationPointer]];
    planeNormal = (Points[randomIndex[iterationPointer + 2]] - planePoint).cross(Points[randomIndex[iterationPointer + 1]] - planePoint);
    planeNormal.normalize();

    // Compute the number of inliers / outliers
    unsigned int nInliers = ComputeNumberOfInlier(Points, planePoint, planeNormal, this->Threshold);

    // keep info
    RansacSampleInfo info(nInliers, randomIndex[iterationPointer], randomIndex[iterationPointer + 1], randomIndex[iterationPointer + 2]);
    samplesInfo.push_back(info);

    if (nInliers > maxInliers)
    {
      maxInliers = nInliers;
      indexMaxInliers = iterationMade;
    }

    // Check that the number of inliers is enought to
    // break the ransac algorithm loop
    if (nInliers > input->GetNumberOfPoints() * this->RatioInliersRequired)
    {
      shouldStopRansac = true;
      hasConverged = true;
    }

    // check if the maximum iteration has been reached
    if (iterationMade > this->MaxRansacIteration)
    {
      shouldStopRansac = true;
    }

    // Updates iterations index
    iterationMade++;
    iterationPointer += 3;
  }

  // Now refine using all inliers
  RefineRansac(Points, output, samplesInfo[indexMaxInliers], this->Threshold, this->PlaneParam);

  // output info
  std::cout << "ransac algorithm has converged: " << hasConverged << std::endl;
  std::cout << "number of iteration made: " << iterationMade << std::endl;
  std::cout << "number of inliers: " << maxInliers << ", " << samplesInfo[indexMaxInliers].NInliers << std::endl;
  std::cout << "plane PlaneParams: [" << this->PlaneParam[0] << "," << this->PlaneParam[1] << "," << this->PlaneParam[2] << "," << this->PlaneParam[3] << "]" << std::endl;

  // flip normal if needed
  if (this->PlaneParam[2] < 0)
  {
    this->PlaneParam[0] *= -1.0;
    this->PlaneParam[1] *= -1.0;
    this->PlaneParam[2] *= -1.0;
    this->PlaneParam[3] *= -1.0;
  }

  double d = this->PlaneParam[3];
  Eigen::Vector3d n(this->PlaneParam[0], this->PlaneParam[1], this->PlaneParam[2]);
  n.normalize();

  // previous normal and bias estimation
  Eigen::Vector3d nPrev(prevPlaneEst[0], prevPlaneEst[1], prevPlaneEst[2]);
  double dPrev = prevPlaneEst[3];

  double newEstimationWeight = 1.0 - this->PreviousEstimationWeight;  // how much do we trust the new plane estimate

  if (this->TemporalAveraging && std::abs(nPrev.norm() - 1) < 1e-3)   // if the previous estimate is valid
  {
    // if the angle between the new normal estimate and the previous is larger than the threshold,
    // the current normal is discarded
    if (std::asin(n.cross(nPrev).norm()) > vtkMath::RadiansFromDegrees(MaxTemporalAngleChange))
    {
      n = nPrev;
      d = dPrev;
    }
    else
    {
      // update the plane estimate with the current one
      n = newEstimationWeight * n + this->PreviousEstimationWeight * nPrev;
      d = newEstimationWeight * d + this->PreviousEstimationWeight * dPrev;
    }
  }
  n.normalize();

  this->PlaneParam[0] = n[0];
  this->PlaneParam[1] = n[1];
  this->PlaneParam[2] = n[2];
  this->PlaneParam[3] = d;

  // transform output if enabled
  if (this->AlignOutput)
  {
    Eigen::Vector3d v = n.cross(Eigen::Vector3d::UnitZ());
    double angle = std::asin(v.norm());
    Eigen::AngleAxisd rot(angle, v.normalized());
    Eigen::Vector3d shift(0.0, 0.0, d);

    // transform points
    for (auto& pt : Points)
    {
      pt =  rot * pt + shift;
    }
    // copy points to output
    output->SetPoints(eigenVectorToVTKPoints(Points));
  }

  return 1;
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef CELLWISESOURCEELLIPTICPDE_HPP_
#define CELLWISESOURCEELLIPTICPDE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include "AbstractCellPopulation.hpp"
#include "AbstractLinearEllipticPde.hpp"

/**
 * An elliptic PDE to be solved numerically using the finite element method, for
 * coupling to a cell-based simulation.
 *
 * The PDE takes the form
 *
 * Grad.(Grad(u)) + k*u*rho(x) = 0,
 *
 * where the scalar k is specified by the member mSourceCoefficient, whose value must
 * be set in the constructor.
 *
 * For a node of the finite element mesh with location x, the function rho(x)
 * equals one if there is a non-apoptotic cell associated with x, and
 * zero otherwise. Here, 'associated with' takes a different meaning for each
 * cell population class, and is encoded in the method IsPdeNodeAssociatedWithNonApoptoticCell().
 *
 * \todo make member names and methods consistent with those of CellwiseSourceParabolicPde
 */
template<unsigned DIM>
class CellwiseSourceEllipticPde : public AbstractLinearEllipticPde<DIM,DIM>
{
    friend class TestCellBasedEllipticPdes;

private:

    /** Needed for serialization.*/
    friend class boost::serialization::access;
    /**
     * Serialize the PDE and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
       archive & boost::serialization::base_object<AbstractLinearEllipticPde<DIM, DIM> >(*this);
       archive & mSourceCoefficient;
    }

protected:

    /** The cell population member. */
    AbstractCellPopulation<DIM, DIM>& mrCellPopulation;

    /** Coefficient of the source term. */
    double mSourceCoefficient;

public:

    /**
     * Constructor.
     *
     * @param rCellPopulation reference to the cell population
     * @param sourceCoefficient the source term coefficient (defaults to 0.0)
     */
    CellwiseSourceEllipticPde(AbstractCellPopulation<DIM, DIM>& rCellPopulation, double sourceCoefficient=0.0);

    /**
     * @return const reference to the cell population (used in archiving).
     */
    const AbstractCellPopulation<DIM>& rGetCellPopulation() const;

    /**
     * @return mSourceCoefficient (used in archiving).
     */
    double GetCoefficient() const;

    /**
     * Overridden ComputeConstantInUSourceTerm() method.
     *
     * @param rX The point in space
     * @param pElement The element
     *
     * @return the constant in u part of the source term, i.e g(x) in
     *  Div(D Grad u)  +  f(x)u + g(x) = 0.
     */
    virtual double ComputeConstantInUSourceTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement);

    /**
     * Overridden ComputeLinearInUCoeffInSourceTerm() method.
     *
     * @param rX The point in space
     * @param pElement the element
     *
     * @return the coefficient of u in the linear part of the source term, i.e f(x) in
     *  Div(D Grad u)  +  f(x)u + g(x) = 0.
     */
    virtual double ComputeLinearInUCoeffInSourceTerm(const ChastePoint<DIM>& rX, Element<DIM,DIM>* pElement);

    /**
     * Overridden ComputeLinearInUCoeffInSourceTermAtNode() method.
     *
     * @param rNode reference to the node
     * @return the coefficient of u in the linear part of the source term, i.e f(x) in
     *  Div(D Grad u)  +  f(x)u + g(x) = 0.
     */
    virtual double ComputeLinearInUCoeffInSourceTermAtNode(const Node<DIM>& rNode);

    /**
     * Overridden ComputeDiffusionTerm() method.
     *
     * @param rX The point in space at which the diffusion term is computed
     *
     * @return a matrix.
     */
    virtual c_matrix<double,DIM,DIM> ComputeDiffusionTerm(const ChastePoint<DIM>& rX);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(CellwiseSourceEllipticPde)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a CellwiseSourceEllipticPde.
 */
template<class Archive, unsigned DIM>
inline void save_construct_data(
    Archive & ar, const CellwiseSourceEllipticPde<DIM>* t, const unsigned int file_version)
{
    // Save data required to construct instance
    const AbstractCellPopulation<DIM, DIM>* p_cell_population = &(t->rGetCellPopulation());
    ar & p_cell_population;
}

/**
 * De-serialize constructor parameters and initialise a CellwiseSourceEllipticPde.
 */
template<class Archive, unsigned DIM>
inline void load_construct_data(
    Archive & ar, CellwiseSourceEllipticPde<DIM>* t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    AbstractCellPopulation<DIM, DIM>* p_cell_population;
    ar >> p_cell_population;

    // Invoke inplace constructor to initialise instance
    ::new(t)CellwiseSourceEllipticPde<DIM>(*p_cell_population);
}
}
} // namespace ...

#endif /*CELLWISESOURCEELLIPTICPDE_HPP_*/


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

int main(int, char**)
{
    using namespace mtl; using mtl::iall;

    typedef std::complex<double>      cdouble;
    const unsigned                    xd= 2, yd= 5, n= xd * yd;
    dense2D<cdouble>                  A(n, n);
    mat::laplacian_setup(A, xd, yd);

    // Fill imaginary part of the matrix
    A*= cdouble(1, -1);
    std::cout << "A is\n" << with_format(A, 7, 1) << "\n";

    std::cout << "sub_matrix(A, 2, 4, 1, 7) is\n"
	      << with_format(sub_matrix(A, 2, 4, 1, 7), 7, 1) << "\n";

    //col-vector from matrix
    dense_vector<cdouble>   v_c(A[iall][0]);

    std::cout << "col-vector v_c is\n" << v_c << "\n";

    //row-vector from matrix
    dense_vector<cdouble, mtl::vec::parameters<tag::row_major> > v_r(A[0][iall]);

    std::cout << "row-vector v_r is\n" << v_r << "\n";

    //row-vector in matrix
    RowInMatrix<dense2D<cdouble> >::type v_r2(A[0][iall]);

    std::cout << "row-vector v_r2 is\n" << v_r2 << "\n";

    //submatrix from matrix per begin and end of row and column
    dense2D<cdouble> B= sub_matrix(A, 2, 4, 1, 7);
    B[1][2]= 88;

    std::cout << "B is\n" << B << "\n";

    //submatrix from matrix per irange
    using mtl::irange;
    irange row(2, 4), col(1, 7);
    dense2D<cdouble> B1= A[row][col];

    std::cout << "B1 is\n" << B1 << "\n";

    //scalar from matrix
    cdouble C= A[1][1];

    std::cout << "C is\n" << C << "\n";

    return 0;
}


// Define this to enable debugging
// #define BOOST_SPIRIT_QI_DEBUG

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/optional.hpp>

#include <iostream>
#include <string>

namespace ast
{
    struct Nil;

    struct Signed;

    struct Expression;

    struct RightAssocExpr;

    struct Arg;

    struct FunctionCall;

    typedef boost::variant<
            Nil
          , double
          , boost::recursive_wrapper< Signed >
          , boost::recursive_wrapper< Expression >
          , boost::recursive_wrapper< RightAssocExpr >
          , boost::recursive_wrapper< Arg >
          , boost::recursive_wrapper< FunctionCall >
        >
    Operand;

    struct Nil
    {
    };

    inline std::ostream & operator<<( std::ostream & out, Nil ) { out << "nil"; return out; }

    struct Signed
    {
        char sign_;
        Operand operand_;
    };

    struct Arg
    {
        char digit_;
    };

    struct RightAssocExpr
    {
        Operand left_;
        boost::optional< Operand > right_;
    };

    struct FunctionCall
    {
        std::string name_;
        Operand arg_;
    };

    struct Operation
    {
        std::string operator_;
        Operand operand_;
    };

    struct Expression
    {
        Operand head_;
        std::list< Operation > tail_;
    };

}

BOOST_FUSION_ADAPT_STRUCT(
    ast::Signed,
    ( char, sign_ )
    ( ast::Operand, operand_ )
)

BOOST_FUSION_ADAPT_STRUCT(
    ast::Arg,
    ( char, digit_ )
)

BOOST_FUSION_ADAPT_STRUCT(
    ast::RightAssocExpr,
    ( ast::Operand, left_ )
    ( boost::optional< ast::Operand >, right_ )
)

BOOST_FUSION_ADAPT_STRUCT(
    ast::FunctionCall,
    ( std::string, name_ )
    ( ast::Operand, arg_ )
)

BOOST_FUSION_ADAPT_STRUCT(
    ast::Operation,
    ( std::string, operator_ )
    ( ast::Operand, operand_ )
)

BOOST_FUSION_ADAPT_STRUCT(
    ast::Expression,
    ( ast::Operand, head_ )
    ( std::list< ast::Operation >, tail_ )
)

namespace ast
{
    struct Calc
    {
    public:

        typedef double result_type;

    private:

        template< typename... TDoubles >
        void assign( double * args, int index, double head, TDoubles &&... tail )
        {
            args[ index ] = head;
            assign( args, index+1, std::forward< TDoubles >( tail )... );
        }

        void assign( double * args, int index )
        {
        }

    public:

        template< typename... Doubles >
        Calc( Doubles &&... ds )
        {
            static_assert( sizeof...( Doubles ) < 10, "sizeof...(Doubles) has to be less then 10" );
            args_[ 0 ] = sizeof...( Doubles );

            assign( & args_[ 0 ], 1, std::forward< Doubles >( ds )... );
        }

        double operator()( Nil ) const
        {
            BOOST_ASSERT( 0 ); return 0;
        }

        double operator()( double n ) const
        {
            return n;
        }

        double operator()( Operation const & x, double lhs ) const
        {
            if( x.operator_ == "&&" )
            {
                if( fabs( lhs ) < 0.000001 ){
                    return 0;
                }
                else{
                    return boost::apply_visitor( *this, x.operand_ );
                }
            }

            if( x.operator_ == "||" )
            {
                if( fabs( lhs ) < 0.000001 ){
                    return boost::apply_visitor( *this, x.operand_ );
                }
                else{
                    return lhs;
                }
            }

            double const rhs = boost::apply_visitor( *this, x.operand_ );

            if( x.operator_ == "+" ){
                return lhs + rhs;
            }

            if( x.operator_ == "-" ){
                return lhs - rhs;
            }

            if( x.operator_ == "*" ){
                return lhs * rhs;
            }

            if( x.operator_ == "/" ){
                return lhs / rhs;
            }

            if( x.operator_ == "<" ){
                return lhs < rhs;
            }

            if( x.operator_ == "<=" ){
                return lhs <= rhs;
            }

            if( x.operator_ == ">" ){
                return lhs > rhs;
            }

            if( x.operator_ == ">=" ){
                return lhs >= rhs;
            }

            if( x.operator_ == "==" ){
                return lhs == rhs;
            }

            if( x.operator_ == "!=" ){
                return lhs != rhs;
            }

            return 0;
        }

        double operator()( Signed const & x ) const
        {
            double const rhs = boost::apply_visitor( *this, x.operand_ );

            switch( x.sign_ )
            {
                case '-':
                    return - rhs;

                case '+':
                    return + rhs;

                case '!':
                    return ( fabs( rhs ) < 0.000001 ) ? 1 : 0;

                default:
                    BOOST_ASSERT( 0 );
            }
        }

        double operator()( Arg const & x ) const
        {
            return args_[ x.digit_ - '0' ];
        }

        double operator()( RightAssocExpr const & x ) const
        {
            double const base = boost::apply_visitor( *this, x.left_ );

            if( ! x.right_ ){
                return base;
            }

            double const exp = boost::apply_visitor( *this, x.right_.get() );

            return std::pow( base, exp );
        }

        double operator()( FunctionCall const & x ) const
        {
            if( x.name_ == "pi" ){
                return M_PI;
            }

            if( x.name_ == "e" ){
                return std::exp( 1.0 );
            }

            double const arg = boost::apply_visitor( *this, x.arg_ );

            if( x.name_ == "sin" ){
                return sin( arg );
            }

            if( x.name_ == "cos" ){
                return cos( arg );
            }

            if( x.name_ == "tan" ){
                return tan( arg );
            }

            if( x.name_ == "abs" ){
                return fabs( arg );
            }

            if( x.name_ == "rad" ){
                return arg * 2 * M_PI / 360;
            }

            if( x.name_ == "deg" ){
                return arg * 360 / 2 / M_PI;
            }

            if( x.name_ == "log" ){
                return log( arg );
            }

            if( x.name_ == "log10" ){
                return log10( arg );
            }

            if( x.name_ == "log2" ){
                return log2( arg );
            }

            return 0;
        }

        double operator()( Expression const & x ) const
        {
            double state = boost::apply_visitor( *this, x.head_ );

            for( Operation const & oper : x.tail_ )
            {
                state = ( *this )( oper, state );
            }

            return state;
        }

    private:

        double args_[ 10 ];
    };
}

namespace qi = boost::spirit::qi;
namespace ascii = boost::spirit::ascii;
using boost::phoenix::function;

struct error_handler_
{
    template< typename, typename, typename >
    struct result { typedef void type; };

    template< typename TIterator >
    void operator()(
        qi::info const & what
      , TIterator err_pos, TIterator last ) const
    {
        std::cout
            << "Error! Expecting "
            << what
            << " here: \""
            << std::string( err_pos, last )
            << "\""
            << std::endl
        ;
    }
};

function< error_handler_ > const error_handler = error_handler_();

template< typename TIterator >
struct Calculator
    : qi::grammar< TIterator, ast::Expression(), ascii::space_type >
{
    Calculator()
        : Calculator::base_type( expression )
    {
        qi::char_type char_;
        qi::digit_type digit_;
        qi::double_type double_;
        qi::lit_type lit_;
        qi::string_type string_;
        qi::alpha_type alpha_;
        qi::alnum_type alnum_;

        using qi::on_error;
        using qi::fail;
        using qi::lexeme;

        expression =
            relationalExpression
            >> *(   (string_( "&&" ) > relationalExpression )
                |   (string_( "||" ) > relationalExpression )
                )
            ;

        relationalExpression =
            additiveExpression
            >> *(   (string_( "<=" ) > additiveExpression )
                |   (string_( "<" ) > additiveExpression )
                |   (string_( ">=" ) > additiveExpression )
                |   (string_( ">" ) > additiveExpression )
                |   (string_( "!=" ) > additiveExpression )
                |   (string_( "==" ) > additiveExpression )
                )
            ;

        additiveExpression =
            multiplicativeExpression
            >> *(   (char_( '+' ) > multiplicativeExpression )
                |   (char_( '-' ) > multiplicativeExpression )
                )
            ;

        multiplicativeExpression =
            exponentialExpression
            >> *(   ( char_( '*' ) > exponentialExpression )
                |   ( char_( '/' ) > exponentialExpression )
                )
            ;

        exponentialExpression =
            unaryExpression
            >> -( '^' >> exponentialExpression );

        unaryExpression =
                ( char_( '-' ) > unaryExpression )
            |   ( char_( '+' ) > unaryExpression )
            |   ( char_( '!' ) > unaryExpression )
            |   primaryExpression
            ;

        primaryExpression =
                double_
            |   arg
            |   functionCall
            |   '(' > expression > ')'
            ;

        arg =
            ( '_' > digit_ )
            ;

        functionCall =
            ( +alnum_ > '(' > -unaryExpression > ')' )
            ;

        BOOST_SPIRIT_DEBUG_NODE( expression );
        BOOST_SPIRIT_DEBUG_NODE( relationalExpression );
        BOOST_SPIRIT_DEBUG_NODE( additiveExpression );
        BOOST_SPIRIT_DEBUG_NODE( multiplicativeExpression );
        BOOST_SPIRIT_DEBUG_NODE( exponentialExpression );
        BOOST_SPIRIT_DEBUG_NODE( unaryExpression );
        BOOST_SPIRIT_DEBUG_NODE( primaryExpression );
        BOOST_SPIRIT_DEBUG_NODE( arg );
        BOOST_SPIRIT_DEBUG_NODE( functionCall );

        on_error< fail >(
            expression,
            error_handler( qi::_4_type(), qi::_3_type(), qi::_2_type() )
        );
    }

private:

    qi::rule< TIterator, ast::Expression(), ascii::space_type > expression;
    qi::rule< TIterator, ast::Expression(), ascii::space_type > relationalExpression;
    qi::rule< TIterator, ast::Expression(), ascii::space_type > additiveExpression;
    qi::rule< TIterator, ast::Expression(), ascii::space_type > multiplicativeExpression;
    qi::rule< TIterator, ast::RightAssocExpr(), ascii::space_type > exponentialExpression;
    qi::rule< TIterator, ast::Operand(), ascii::space_type > unaryExpression;
    qi::rule< TIterator, ast::Operand(), ascii::space_type > primaryExpression;
    qi::rule< TIterator, ast::Arg(), ascii::space_type > arg;
    qi::rule< TIterator, ast::FunctionCall(), ascii::space_type > functionCall;
};

template< typename... TDoubles>
double calc(
    std::string const & expr,
    TDoubles... ds
)
{
    std::string::const_iterator iter = expr.begin();
    std::string::const_iterator end = expr.end();

    Calculator< std::string::const_iterator > calc;

    boost::spirit::ascii::space_type space;

    ast::Expression expression;

    bool const r = phrase_parse( iter, end, calc, space, expression );

    if( r && iter == end )
    {
        ast::Calc calc( std::forward< TDoubles >( ds )... );
        return calc( expression );
    }

    throw std::runtime_error( "Parsing failed: " + std::string( iter, end ) );
}

void test()
{
    assert( calc( "1+2*3" ) == 7 );
    assert( calc( "2^3^2" ) == 512 );
    assert( calc( "(1+2)*(3+4)" ) == 21 );

    assert( calc( "1+-1" ) == 0 );
    assert( calc( "1--1" ) == 2 );

    assert( calc( "4/2" ) == 2 );
    assert( calc( "4.5/2" ) == 2.25 );
    assert( calc( "5/2" ) == 2.5 );

    assert( calc( "!1" ) == 0 );
    assert( calc( "!0" ) == 1 );

    assert( calc( "pi()" ) == double(M_PI) );
    assert( calc( "e()" ) == std::exp( 1.0 ) );

    assert( calc( "12||(1/0)" ) == 12 );
    assert( calc( "0&&(1/0)" ) == 0 );
    assert( calc( "1&&2" ) == 2 );
    assert( calc( "2||1" ) == 2 );
    assert( calc( "0||2" ) == 2 );

    assert( calc( "1<2" ) == 1 );
    assert( calc( "2<1" ) == 0 );

    assert( calc( "1>2" ) == 0 );
    assert( calc( "2>1" ) == 1 );

    assert( calc( "1<=2" ) == 1 );
    assert( calc( "2<=2" ) == 1 );
    assert( calc( "3<=2" ) == 0 );

    assert( calc( "1>=2" ) == 0 );
    assert( calc( "2>=2" ) == 1 );
    assert( calc( "3>=2" ) == 1 );

    assert( calc( "abs(1)" ) == 1 );
    assert( calc( "abs(-1)" ) == 1 );

    assert( calc( "sin(rad(0))" ) == 0 );
    assert(( fabs( calc( "sin(rad(90))" ) - 1.0 ) < 0.000001 ));

    assert( calc( "cos(rad(0))" ) == 1 );
    assert(( fabs( calc( "cos(rad(90))" ) - 0.0 ) < 0.0000001 ));

    assert( calc( "log10(1)" ) == 0 );
    assert( calc( "log10(10)" ) == 1 );
    assert( calc( "log10(100)" ) == 2 );

    assert( calc( "log2(1)" ) == 0 );
    assert( calc( "log2(2)" ) == 1 );
    assert( calc( "log2(4)" ) == 2 );

    assert( calc( "_1+_2+_3+_4+_5+_6+_7+_8+_9", 1, 2, 3, 4, 5, 6, 7, 8, 9 ) == 45 );
    assert( calc( "_0" ) == 0 );
    assert( calc( "_0", 1 ) == 1 );
    assert( calc( "_0", 1, 2 ) == 2 );
    assert( calc( "_0", 1, 2, 3 ) == 3 );
}

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

    using std::cout;
    using std::endl;
    using std::stof;

    try
    {
        if( argc == 2 ){
            cout << calc( argv[1] ) << endl;
        }
        else if( argc == 3 ){
            cout << calc( argv[1], stof(argv[2]) ) << endl;
        }
        else if( argc == 4 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]) ) << endl;
        }
        else if( argc == 5 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]) ) << endl;
        }
        else if( argc == 6 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]) ) << endl;
        }
        else if( argc == 7 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]), stof(argv[6]) ) << endl;
        }
        else if( argc == 8 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]), stof(argv[6]),
                stof(argv[7]) ) << endl;
        }
        else if( argc == 9 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]), stof(argv[6]),
                stof(argv[7]), stof(argv[8]) ) << endl;
        }
        else if( argc == 10 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]), stof(argv[6]),
                stof(argv[7]), stof(argv[8]), stof(argv[9]) ) << endl;
        }
        else if( argc == 11 ){
            cout << calc( argv[1], stof(argv[2]), stof(argv[3]),
                stof(argv[4]), stof(argv[5]), stof(argv[6]),
                stof(argv[7]), stof(argv[8]), stof(argv[9]),
                stof(argv[10]) ) << endl;
        }
    }
    catch( std::runtime_error const & e ){
        std::cerr << e.what() << std::endl;
    }
}


/// @file  cmp.hpp
/// @brief Declarations for embedding methods using classic ordinal constraints

#pragma once
#ifndef OGT_EMBED_CMP_HPP
#define OGT_EMBED_CMP_HPP

#include <ogt/config.hpp>
#include <ogt/core/oracle.hpp>
#include <ogt/embed/embed.hpp>
#include <Eigen/Dense>
#include <vector>

namespace OGT_NAMESPACE {
namespace embed {

/// An ordinal constraint specifying that dist(a,b) < dist(c,d).
struct CmpConstraint {

	/// Default constructor.
	CmpConstraint() : a(0), b(0), c(0), d(0) {}

	/// Constructor with four points
	CmpConstraint(size_t a, size_t b, size_t c, size_t d)
		: a(a), b(b), c(c), d(d) {}

	/// Constructor with three points: dist(a,b) < dist(a,c).
	CmpConstraint(size_t a, size_t b, size_t c)
		: CmpConstraint(a, b, c, a) {}

	/// Constructor from an oracle comparison
	CmpConstraint(const OGT_NAMESPACE::core::CmpOutcome& cmp)
		: CmpConstraint(
			cmp.a,
			cmp.cmp == OGT_NAMESPACE::core::AB_LT_AC ? cmp.b : cmp.c,
			cmp.cmp == OGT_NAMESPACE::core::AB_LT_AC ? cmp.c : cmp.b) {}

	/// The objects being constrained
	size_t a, b, c, d;
};

/// Embeds a dataset by recovering a dissimilarity kernel using the Crowd Kernel
/// embedding algorithm.
/// The parameter lambda effects the scaling of the embedding.
///
/// Citation: O. Tamuz, C. Liu, S. Belongie, O. Shamir, and A. T. Kalai,
/// "Adaptively Learning the Crowd Kernel," International Conference on Machine
/// Learning, 2011.
EmbedResult embedCmpWithCKForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using the Crowd Kernel embedding algorithm.
/// The parameter lambda effects the scaling of the embedding.
///
/// Citation: O. Tamuz, C. Liu, S. Belongie, O. Shamir, and A. T. Kalai,
/// "Adaptively Learning the Crowd Kernel," International Conference on Machine
/// Learning, 2011.
EmbedResult embedCmpWithCKForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset by recovering a dissimilarity kernel using Generalized
/// Non-metric Multidimensional Scaling.
/// The lambda parameter specifies the amount of regularization to apply to
/// keep the rank small.
/// An embedding can be recovered from the kernel using, for instance,
/// embeddingFromKernelSVD().
///
/// Citation: S. Agarwal, J. Wills, and L. Cayton, "Generalized non-metric
/// multidimensional scaling,"" International Conference on Machine Learning,
/// 2007.
EmbedResult embedCmpWithGNMDSForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using Generalized Non-metric Multidimensional Scaling.
/// The lambda parameter specifies the amount of regularization to apply to
/// keep the rank small.
///
/// Citation: S. Agarwal, J. Wills, and L. Cayton, "Generalized non-metric
/// multidimensional scaling,"" International Conference on Machine Learning,
/// 2007.
EmbedResult embedCmpWithGNMDSForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using Soft Ordinal Embedding.
/// This method uses the margin parameter to set the scale. A default value of
/// 0.1 will be used if no margin is provided.
///
/// Citation: Y. Terada and U. von Luxburg, "Local ordinal embedding," presented
/// at the Proceedings of the 31st International Conference on Machine Learning,
/// 2014.
EmbedResult embedCmpWithSOE(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds using a weighted variant of Soft Ordinal Embedding. The loss function
/// is modified so that each constraint has a corresponding weight which is
/// multiplied to the loss incurred when that constraint is violated.
/// It is assumed, but not verified, that the weights are non-negative and sum
/// to one.
EmbedResult embedCmpWithSOEWeighted(std::vector<CmpConstraint> cons,
	const Eigen::VectorXd &weights, const Eigen::MatrixXd &X0,
	EmbedConfig config);

/// Embeds a point to satisfy a ranking of some set of fixed points, as much
/// as possible.
/// rank is an ordered list of rows from X. The resulting row in EmbedResult.X
/// gives a position which satisfies this ranking, if possible.
EmbedResult embedPtRankingWithSOE(const std::vector<size_t> &rank,
	const Eigen::MatrixXd& X, const Eigen::VectorXd& pos0, EmbedConfig config);

/// Embeds a dataset using Local Ordinal Embedding.
/// This method uses the margin parameter to set the scale. A default value of
/// 0.1 will be used if no margin is provided.
///
/// Citation: Y. Terada and U. von Luxburg, "Local ordinal embedding," presented
/// at the Proceedings of the 31st International Conference on Machine Learning,
/// 2014.
EmbedResult embedKnnWithLOE(const Eigen::MatrixXi& knn,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Pick an optimal SOE scale parameter for the embedding.
double fitSOEScale(std::vector<CmpConstraint> cons, const Eigen::MatrixXd& X);

/// Embeds a dataset by recovering a dissimilarity kernel using Stochastic
/// Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithSTEForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& K0, double lambda, EmbedConfig config);

/// Embeds a dataset using Stochastic Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithSTEForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using t-Distributed Stochastic Triplet Embedding.
/// The lambda parameter specifies the degree of regularization.
/// The alpha parameter indicates the degrees of freedom for the Student's-t
/// distribution.
///
/// Citation: L. Van der Maaten and K. Weinberger, "Stochastic triplet
/// embedding," 2012 IEEE International Workshop on Machine Learning for Signal
/// Processing (MLSP), 2012.
EmbedResult embedCmpWithTSTE(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, double alpha, EmbedConfig config);

/// Embeds a dataset using Rank-d Projected Gradient Descent (PGD), adapted
/// to work directly on a n x d matrix X rather than to project after each step.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithPGDForX(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds a dataset using Rank-d Projected Gradient Descent (PGD).
/// This is the original version of the algorithm.
/// After each learning step, we project to the nearest rank d Gram matrix.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithPGDForK(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, EmbedConfig config);

/// Embeds a dataset using Nuclear Norm Projected Gradient Descent.
/// After each learning step, we project onto the nuclear norm ball, which has
/// the effect of minimizing the rank.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithNNPGD(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Embeds a dataset using Nuclear Norm Projected Gradient Descent Debiased.
/// This is similar to embedCmpWithNNPGD() but with an extra debiasing step.
/// This debiasing improves accuracy by rescaling the non-zero eigenvalues
/// to prevent them from shrinking toward zero. It is the default behavior.
///
/// Citation: L. Jain, K. Jamieson, and R. Nowak, "Finite Sample Prediction and
/// Recovery Bounds for Ordinal Embedding," NIPS, 2016.
EmbedResult embedCmpWithNNPGDDebiased(std::vector<CmpConstraint> cons,
	const Eigen::MatrixXd& X0, double lambda, EmbedConfig config);

/// Compute a kernel matrix where K_ij is derived by how similarly points i and
/// j are ranked by the provided comparisons.
/// The parameters k1 and k2 indicate how much of the two possible kernels to
/// mix into the final output; k1 corresponds to an anchor-based kernel while k2
/// corresponds to a tail-based kernel.
///
/// Citation: M. Kleindessner and U. von Luxburg, “Kernel functions based on
/// triplet similarity comparisons,” arXiv.org, vol. stat.ML. 28-Jul-2016.
EmbedResult embedCmpWithTauForK(std::vector<CmpConstraint> cons, double k1,
	double k2);

} // end namespace embed
} // end namespace OGT_NAMESPACE
#endif /* OGT_EMBED_CMP_HPP */


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

#ifndef LINEAR_ELASTICITY_HPP
#define LINEAR_ELASTICITY_HPP

#include <Eigen/Core>
#include <cassert>
#include <exception>

#include "C0_triangles.hpp"
#include "Galerkin.hpp"
#include "SymSparse.hpp"
#include "mesh_traits.hpp"
#include "src/FunctionBase.hpp"

namespace Elasticity
{

template <class Element>
struct basis_size_struct
{
};

template <>
struct basis_size_struct<C0Triangle<1>>
{
    constexpr static size_t value = 3;
};

template <>
struct basis_size_struct<C0Triangle<2>>
{
    constexpr static size_t value = 6;
};

template <>
struct basis_size_struct<C0Triangle<3>>
{
    constexpr static size_t value = 10;
};

template <>
struct basis_size_struct<C0Triangle<4>>
{
    constexpr static size_t value = 15;
};

template <class Element>
constexpr size_t basis_size = basis_size_struct<Element>::value;

template <class Element>
using coeffs_type = Eigen::Matrix<double, basis_size<Element>, 1>;

template <class Element>
using vector_coeffs_type = Eigen::Matrix<double, 2, basis_size<Element>>;

template <class Mesh>
auto get_scalar_coeffs(const Mesh &mesh, size_t eli, const Eigen::VectorXd &uc)
{
    const auto &el_info = mesh.element(eli);
    Eigen::Matrix<double, std::decay_t<decltype(el_info)>::num_nodes(), 1> coeffs;

    auto nn = el_info.node_numbers();
    for (size_t i = 0; i < el_info.num_nodes(); ++i)
    {
        coeffs[i] = uc[nn[i]];
    }

    return coeffs;
}

template <class Mesh>
auto get_vector_coeffs(const Mesh &mesh, size_t eli, const Eigen::Matrix2Xd &uc)
{
    const auto &el_info = mesh.element(eli);
    Eigen::Matrix<double, 2, std::decay_t<decltype(el_info)>::num_nodes()> coeffs;

    auto nn = el_info.node_numbers();
    for (size_t i = 0; i < el_info.num_nodes(); ++i)
    {
        coeffs.col(i) = uc.col(nn[i]);
    }

    return coeffs;
}

/*
 * Given the coefficients of a function u in the finite element basis for el,
 * construct a function object that evaluates that function (and whose
derivatives,
 * etc. can be used).
 */
template <class Element>
auto make_function(const Element &, const coeffs_type<Element> &uc)
{
    using Galerkin::Functions::ConstantFunction;
    return Galerkin::static_sum<1, basis_size<Element>>(
        [&uc](auto I) { return ConstantFunction(uc[I()]) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc[0]) * Galerkin::get<0>(Element::basis));
}

/*
 * Given the coefficients of a *vector* function u in the finite element basis
 * for el, construct 2 function objects evaluating the X and Y parts of the
 * vector function.
 */
template <class Element>
auto make_vector_functions(const Element &el, const vector_coeffs_type<Element> &uc)
{
    using Galerkin::Functions::ConstantFunction;

    auto ux = Galerkin::static_sum<1, basis_size<Element>>(
        [&](auto I) { return ConstantFunction(uc(0, I())) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc(0, 0)) * Galerkin::get<0>(Element::basis));
    auto uy = Galerkin::static_sum<1, basis_size<Element>>(
        [&](auto I) { return ConstantFunction(uc(1, I())) * Galerkin::get<I()>(Element::basis); },
        ConstantFunction(uc(1, 0)) * Galerkin::get<0>(Element::basis));

    return std::pair(ux, uy);
}

template <class Element>
using el_stiffness_type = Eigen::Matrix<double, 2 * basis_size<Element>, 2 * basis_size<Element>>;

template <class Element>
el_stiffness_type<Element> element_stiffness_matrix(const Element &el, double lambda, double mu);

extern template el_stiffness_type<C0Triangle<1>>
element_stiffness_matrix(const C0Triangle<1> &, double, double);
extern template el_stiffness_type<C0Triangle<2>>
element_stiffness_matrix(const C0Triangle<2> &, double, double);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template el_stiffness_type<C0Triangle<3>>
element_stiffness_matrix(const C0Triangle<3> &, double, double);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template el_stiffness_type<C0Triangle<4>>
element_stiffness_matrix(const C0Triangle<4> &, double, double);
#endif

template <class Element>
el_stiffness_type<Element> element_stiffness_matrix(
    const Element &el, const coeffs_type<Element> &lambda, const coeffs_type<Element> &mu);

extern template el_stiffness_type<C0Triangle<1>> element_stiffness_matrix(
    const C0Triangle<1> &, const coeffs_type<C0Triangle<1>> &,
    const coeffs_type<C0Triangle<1>> &);
extern template el_stiffness_type<C0Triangle<2>> element_stiffness_matrix(
    const C0Triangle<2> &, const coeffs_type<C0Triangle<2>> &,
    const coeffs_type<C0Triangle<2>> &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template el_stiffness_type<C0Triangle<3>> element_stiffness_matrix(
    const C0Triangle<3> &, const coeffs_type<C0Triangle<3>> &,
    const coeffs_type<C0Triangle<3>> &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template el_stiffness_type<C0Triangle<4>> element_stiffness_matrix(
    const C0Triangle<4> &, const coeffs_type<C0Triangle<4>> &,
    const coeffs_type<C0Triangle<4>> &);
#endif

template <class Element>
Eigen::Matrix<double, basis_size<Element>, basis_size<Element>>
element_mass_matrix(const Element &el);

extern template Eigen::Matrix<double, basis_size<C0Triangle<1>>, basis_size<C0Triangle<1>>>
element_mass_matrix(const C0Triangle<1> &);
extern template Eigen::Matrix<double, basis_size<C0Triangle<2>>, basis_size<C0Triangle<2>>>
element_mass_matrix(const C0Triangle<2> &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix<double, basis_size<C0Triangle<3>>, basis_size<C0Triangle<3>>>
element_mass_matrix(const C0Triangle<3> &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix<double, basis_size<C0Triangle<4>>, basis_size<C0Triangle<4>>>
element_mass_matrix(const C0Triangle<4> &);
#endif

template <class Mesh>
auto instantiate_element(const Mesh &mesh, size_t which)
{
    constexpr int order = msh::element_order<Mesh>;
    const auto &el = mesh.element(which);
    return C0Triangle<order>(
        mesh.coord(el.control_nodes[0]), mesh.coord(el.control_nodes[1]),
        mesh.coord(el.control_nodes[2]));
}

constexpr auto lame_parameters(double E, double nu) noexcept
{
    double lambda = E * nu / ((1 + nu) * (1 - nu));
    double mu = E / (2 * (1 + nu));
    return std::make_pair(lambda, mu);
}

typedef SymSparse::SymmetricSparseMatrix<double, 2 * max_node_adjacencies> StiffnessType;

template <class Mesh>
StiffnessType assemble_stiffness(const Mesh &mesh, double E, double nu)
{
    StiffnessType K(mesh.num_nodes() * 2);
    auto [lambda, mu] = lame_parameters(E, nu);

    for (size_t i = 0; i < mesh.num_elements(); ++i)
    {
        const auto &el_info = mesh.element(i);
        const auto nn = el_info.node_numbers();
        const auto el = instantiate_element(mesh, i);
        const auto local_K = element_stiffness_matrix(el, lambda, mu);
        for (size_t j = 0; j < nn.size(); ++j)
        {
            for (size_t k = j; k < nn.size(); ++k)
            {
                K.insert_entry(nn[j] * 2, nn[k] * 2, local_K(2 * j, 2 * k));
                if (k != j)
                {
                    K.insert_entry(nn[j] * 2 + 1, nn[k] * 2, local_K(2 * j + 1, 2 * k));
                }
                K.insert_entry(nn[j] * 2, nn[k] * 2 + 1, local_K(2 * j, 2 * k + 1));
                K.insert_entry(nn[j] * 2 + 1, nn[k] * 2 + 1, local_K(2 * j + 1, 2 * k + 1));
            }
        }
    }
    return K;
}

template <class Mesh>
StiffnessType
assemble_stiffness(const Mesh &mesh, const Eigen::VectorXd &lambda, const Eigen::VectorXd &mu)
{
    StiffnessType K(mesh.num_nodes() * 2);

    for (size_t eli = 0; eli < mesh.num_elements(); ++eli)
    {
        auto lc = get_scalar_coeffs(mesh, eli, lambda);
        auto uc = get_scalar_coeffs(mesh, eli, mu);
        const auto &el_info = mesh.element(eli);
        const auto nn = el_info.node_numbers();
        const auto el = instantiate_element(mesh, eli);
        const auto local_K = element_stiffness_matrix(el, lc, uc);

        for (size_t j = 0; j < nn.size(); ++j)
        {
            for (size_t k = j; k < nn.size(); ++k)
            {
                K.insert_entry(nn[j] * 2, nn[k] * 2, local_K(2 * j, 2 * k));
                if (k != j)
                {
                    K.insert_entry(nn[j] * 2 + 1, nn[k] * 2, local_K(2 * j + 1, 2 * k));
                }
                K.insert_entry(nn[j] * 2, nn[k] * 2 + 1, local_K(2 * j, 2 * k + 1));
                K.insert_entry(nn[j] * 2 + 1, nn[k] * 2 + 1, local_K(2 * j + 1, 2 * k + 1));
            }
        }
    }

    return K;
}

extern template StiffnessType assemble_stiffness<Mesh1>(const Mesh1 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh1>(const Mesh1 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
extern template StiffnessType assemble_stiffness<Mesh2>(const Mesh2 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh2>(const Mesh2 &, const Eigen::VectorXd &, const Eigen::VectorXd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template StiffnessType assemble_stiffness<Mesh3>(const Mesh3 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh3>(const Mesh3 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template StiffnessType assemble_stiffness<Mesh4>(const Mesh4 &, double, double);
extern template StiffnessType
assemble_stiffness<Mesh4>(const Mesh4 &, const Eigen::VectorXd &, const Eigen::VectorXd &);
#endif

template <>
inline StiffnessType
assemble_stiffness<MeshVariant>(const MeshVariant &mv, double lambda, double mu)
{
    return std::visit(
        [lambda, mu](const auto &mesh) { return assemble_stiffness(mesh, lambda, mu); }, mv);
}

template <>
inline StiffnessType assemble_stiffness<MeshVariant>(
    const MeshVariant &mv, const Eigen::VectorXd &lambda, const Eigen::VectorXd &mu)
{
    return std::visit(
        [&lambda, &mu](const auto &mesh) { return assemble_stiffness(mesh, lambda, mu); }, mv);
}

/*
 * Add a homogeneous Dirichlet condition on the boundary segment of the mesh
 * indexed by `which`. K is the stiffness matrix obtained from
 * `assemble_stiffness`, and `rhs` is the forcing vector.
 */
template <class Mesh, class RHS>
void impose_homogeneous_condition(
    const Mesh &mesh, StiffnessType &K, RHS &rhs, size_t which, double scale = 1.0)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which, scale](const auto &mesh)
            { impose_homogeneous_condition(mesh, K, rhs, which, scale); },
            mesh);
    }
    else
    {
        std::vector<size_t> adjacent;
        adjacent.reserve(2 * max_node_adjacencies);
        const auto &boundary = mesh.boundary(which);

        for (auto n : boundary.nodes)
        {
            adjacent.clear();
            // Get all of the adjacent DOFs to this one; since there are two
            // components of displacement there are 2 degrees of freedom (2*n,
            // 2*n+1) corresponding to each node.
            for (auto n2 : mesh.adjacent_nodes(n))
            {
                adjacent.push_back(2 * n2);
                adjacent.push_back(2 * n2 + 1);
            }
            K.eliminate_dof(2 * n, 0.0, scale, rhs, adjacent);
            K.eliminate_dof(2 * n + 1, 0.0, scale, rhs, adjacent);
        }
    }
}

struct PreEliminatedStiffness
{
};

template <class Mesh, class RHS>
void impose_homogeneous_condition(
    const Mesh &mesh, PreEliminatedStiffness, RHS &rhs, size_t which)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which](const auto &mesh)
            { impose_homogeneous_condition(mesh, PreEliminatedStiffness{}, rhs, which); },
            mesh);
    }
    else
    {
        const auto &boundary = mesh.boundary(which);

        for (auto n : boundary.nodes)
        {
            rhs.row(2 * n).array() = 0;
            rhs.row(2 * n + 1).array() = 0;
        }
    }
}

template <class Mesh>
void impose_homogeneous_condition(
    const Mesh &mesh, StiffnessType &K, size_t which, double scale = 1.0)
{
    std::vector<size_t> adjacent;
    adjacent.reserve(2 * max_node_adjacencies);
    const auto &boundary = mesh.boundary(which);

    for (auto n : boundary.nodes)
    {
        adjacent.clear();
        // Get all of the adjacent DOFs to this one; since there are two
        // components of displacement there are 2 degrees of freedom (2*n,
        // 2*n+1) corresponding to each node.
        for (auto n2 : mesh.adjacent_nodes(n))
        {
            adjacent.push_back(2 * n2);
            adjacent.push_back(2 * n2 + 1);
        }
        K.eliminate_dof(2 * n, 0.0, scale, adjacent);
        K.eliminate_dof(2 * n + 1, 0.0, scale, adjacent);
    }
}

template <>
void impose_homogeneous_condition(const MeshVariant &, StiffnessType &, size_t, double);

struct OutOfBoundsIndex : public std::exception
{
    const char *msg;
    OutOfBoundsIndex(const char *m) : msg(m) {}
    const char *what() const noexcept { return msg; }
};

template <class Mesh, class RHS>
void impose_dirichlet_condition(
    const Mesh &mesh, StiffnessType &K, RHS &rhs, size_t which, const Eigen::Matrix2Xd &value,
    double scale = 1.0)
{
    if constexpr (std::is_same_v<Mesh, MeshVariant>)
    {
        std::visit(
            [&, which, scale](const auto &mesh)
            { impose_dirichlet_condition(mesh, K, rhs, which, value, scale); },
            mesh);
    }
    else
    {
        std::vector<size_t> adjacent;
        adjacent.reserve(2 * max_node_adjacencies);
        const auto &boundary = mesh.boundary(which);

        if ((size_t)value.cols() != boundary.nodes.size())
        {
            throw OutOfBoundsIndex(
                "Matrix given for Dirichlet condition has wrong number of values");
        }
        else if ((size_t)rhs.rows() != mesh.num_nodes() * 2)
        {
            throw OutOfBoundsIndex("RHS passed has wrong dimension - should be 2 * num_nodes");
        }

        size_t i = 0;
        for (auto n : boundary.nodes)
        {
            adjacent.clear();
            for (auto n2 : mesh.adjacent_nodes(n))
            {
                adjacent.push_back(2 * n2);
                adjacent.push_back(2 * n2 + 1);
            }
            K.eliminate_dof(2 * n, value(0, i), scale, rhs, adjacent);
            K.eliminate_dof(2 * n + 1, value(1, i), scale, rhs, adjacent);
            i += 1;
        }
    }
}

template <class Force, class RHS>
void add_point_force(const Force &force, size_t node, RHS &F)
{
    if (2 * node + 1 > F.size())
    {
        throw OutOfBoundsIndex("Node is out of bounds for given forcing vector");
    }
    F[2 * node] += force[0];
    F[2 * node + 1] += force[1];
}

template <class T, class RHS, int N, class IndexContainer, int... Options>
void add_point_forces(
    const Eigen::Matrix<T, 2, N, Options...> &force, const IndexContainer &nodes, RHS &F)
{
    static_assert(sizeof...(Options) == 3);
    size_t col = 0;
    for (size_t n : nodes)
    {
        add_point_force(force.col(col), n, F);
        col += 1;
    }
}

/*
 * Where fc is a matrix containing the coefficients of the X and Y components of
 * the volume forcing in the first and second rows, respectively, integrate this
 * volume force to obtain its contribution to the linear form vector.
 */
template <class Mesh>
Eigen::Matrix2Xd integrate_volume_force(const Mesh &mesh, const Eigen::Matrix2Xd &fc);

extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh1 &, const Eigen::Matrix2Xd &);
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh2 &, const Eigen::Matrix2Xd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh3 &, const Eigen::Matrix2Xd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix2Xd integrate_volume_force(const Mesh4 &, const Eigen::Matrix2Xd &);
#endif

template <>
inline Eigen::Matrix2Xd integrate_volume_force(const MeshVariant &mv, const Eigen::Matrix2Xd &fc)
{
    return std::visit([&fc](const auto &mesh) { return integrate_volume_force(mesh, fc); }, mv);
}

/*
 * Where fc is a matrix containing coefficients of the X and Y parts of a
 * traction force on boundary bound_index, integrates this traction force to
 * find its contribution to the linear form vector.
 */
template <class Mesh>
Eigen::Matrix2Xd
integrate_traction_force(const Mesh &mesh, size_t bound_index, const Eigen::Matrix2Xd &fc);

extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh1 &, size_t, const Eigen::Matrix2Xd &);
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh2 &, size_t, const Eigen::Matrix2Xd &);

#if ELASTICITY_MAX_ELEMENT_ORDER > 2
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh3 &, size_t, const Eigen::Matrix2Xd &);
#endif

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
extern template Eigen::Matrix2Xd
integrate_traction_force(const Mesh4 &, size_t, const Eigen::Matrix2Xd &);
#endif

template <>
inline Eigen::Matrix2Xd
integrate_traction_force(const MeshVariant &mv, size_t bound_index, const Eigen::Matrix2Xd &fc)
{
    return std::visit(
        [&fc, bound_index](const auto &mesh)
        { return integrate_traction_force(mesh, bound_index, fc); },
        mv);
}

/*
 * Where 'traction' is an integrated traction force on boundary 'bindex',
 * as returned from 'integrate_traction_force', adds this force to the
 * appropriate columns of 'forcing' (which, for example, might have come from
 * 'integrate_volume_force'.)
 */
void add_traction_force(
    const MeshVariant &mv, Eigen::Matrix2Xd &forcing, size_t bindex,
    const Eigen::Matrix2Xd &traction);

/*
 * Where u_true is a function taking X and Y coordinates and u is a matrix with
 * coefficients of X and Y parts of a FEM solution in its columns, compute the
 * L^2 norm of u - u_true and of u_true. Used for manufactured solution
 * verification.
 */
std::pair<double, double> integrate_error(
    const MeshVariant &mesh, const Eigen::Matrix2Xd &u,
    Eigen::Vector2d (*u_true)(std::array<double, 2>));

} // namespace Elasticity

#endif // LINEAR_ELASTICITY_HPP