#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <boost/dynamic_bitset.hpp>
#include <boost/container/vector.hpp>
#include <boost/unordered_map.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/niederreiter_base2.hpp>
#include <./Timer.cpp>

typedef boost::dynamic_bitset<> Vetor;
typedef boost::container::vector<Vetor> Matriz;

/*
*
* Ht: Matriz de verificação de paridade original
*
* Ht_long: Referência de onde guardar o array em que cada elemento é uma linha de Ht
*
* Erros_1: Referência de onde guardar o
    array em que cada elemento é um erro de peso=1 (mas só a porção de informação desse erro)
*   exemplo: se há 5 bits na palavra-código e os 2 primeiros são de informação, então um elemento
*   de Erros_1 pode ser 3=0b11, significando erro em cada um dos 2 bits de informação
*
* n_linhas, n_colunas, n_informacao: Quantas linhas e colunas tem Ht, e quantos bits são de informação
*   na palavra código (assume-se que são correspondem às primeiras linhas de Ht)
*/
void __gera_auxiliares(
    const Matriz& Ht,
    unsigned long* Ht_long,
    unsigned long* Erros_1,
    int n_linhas,
    int n_colunas,
    int n_informacao
) {
    /**
    * Só necessários os ulongs correspondentes a cada síndrome
    */
    for (int i = 0; i < n_linhas; ++i)
    {
        Ht_long[i] = Ht[i].to_ulong();
    }

    /**
    * Porção de informação dos erros associados a cada síndrome
    * (por isso só importam os erros em bits de informação)
    */
    for (int i = 0; i < n_linhas; ++i)
    {
        // Erros depois de n_informacao linhas correspondem somente a erros de bit
        // de paridade, portanto aparecem como 0 já que só se coletam os erros de informação
        Erros_1[i] = i < n_informacao ? ((unsigned long)1) << (n_informacao - i - 1) : 0;
    }
}

/**
* input_csv: Espera-se formato de valores 0 ou 1 separados por vírgula e \n, sem espaços
*
* M: Matriz onde armazenar a leitura.
*   Restrição: linha/coluna pode ter, no máximo (inclusive), 64 bits (cabe num unsigned long)
*/
void carrega_matriz(
    std::ifstream& input_csv,
    Matriz& M
) {
    std::string numero_string;

    while (getline(input_csv, numero_string)) {

        int commaPos;

        while ((commaPos = numero_string.find(",")) != std::string::npos)
            numero_string.erase(commaPos, 1);

        Vetor numero (numero_string.length(), stoul(numero_string, 0, 2));

        // std::cout << numero << "\n";

        M.push_back(numero);

    }
}

int testa_carrega_matriz() {
    std::string Htcsv = "./dados/Ht.csv";
    std::ifstream input {Htcsv};

    // if(!input)
    //     std::error("could not open " + Htcsv);

    std::string numero_string;
    Matriz Ht;

    while (getline(input, numero_string)) {

        int commaPos;

        while ((commaPos = numero_string.find(",")) != std::string::npos)
            numero_string.erase(commaPos, 1);

        Vetor numero (numero_string.length(), stoul(numero_string, 0, 2));

        std::cout << numero << "\n";

        Ht.push_back(numero);

    }

    std::cout << "Finished loading. Now replicating:" << std::endl;

    std::for_each(Ht.begin(), Ht.end(), [](Vetor line) {
        std::cout << line << std::endl;
    });
}

/**
* Auxiliar para `popular_dict`
*
*/
void __popular_dict(
    boost::unordered_map<unsigned long, unsigned long>& dict,
    unsigned long* Ht_long,
    unsigned long* Erros_1,
    int n_linhas,
    int n_colunas,
    int n_informacao,
    unsigned long sindrome_parcial,
    unsigned long erro_parcial,
    int linha_inicio,
    int niveis_restantes
) {
    if (niveis_restantes == 1) {
        unsigned long sindrome, erro;
        for (int i = linha_inicio; i < n_linhas; ++i)
        {
            sindrome = sindrome_parcial ^ Ht_long[i];
            erro = erro_parcial ^ Erros_1[i];
            if (dict.find(sindrome) == dict.end())
                dict[sindrome] = erro;
        }
        return;
    }

    for (int i = linha_inicio; i < n_linhas - (niveis_restantes - 1) /*iterar todas as síndromes*/; ++i)
    {
        __popular_dict(
            dict,
            Ht_long,
            Erros_1,
            n_linhas,
            n_colunas,
            n_informacao,
            sindrome_parcial ^ Ht_long[i],
            erro_parcial ^ Erros_1[i],
            i+1,
            niveis_restantes-1
        );
    }

}

/**
*
* dict: Onde guardar o mapa "síndrome->erro"
*
* Ht: Matriz de verificação de paridade (transposta). Assume-se que suas últimas linhas
* correspondem aos bits de informação (identidade em cima, outras linhas embaixo)
*
* n_linhas, n_colunas, n_informacao: Quantas linhas e colunas tem Ht, e quantos bits são de informação
*   na palavra código (assume-se que são correspondem às primeiras linhas de Ht)
*
* peso_maximo: O peso dos maiores erros de informação a serem catalogados em `dict`
*
*/
void popular_dict(
    boost::unordered_map<unsigned long, unsigned long>& dict,
    const Matriz& Ht,
    int n_linhas,
    int n_colunas,
    int n_informacao,
    int peso_maximo
) {

    unsigned long *Ht_long = new unsigned long[n_linhas];
    unsigned long *Erros_1 = new unsigned long[n_linhas];
    __gera_auxiliares(
        Ht,
        Ht_long,
        Erros_1,
        n_linhas,
        n_colunas,
        n_informacao
    );

    for (int i = 1; i <= peso_maximo; ++i)
    {
        __popular_dict(
        dict,
        Ht_long,
        Erros_1,
        n_linhas,
        n_colunas,
        n_informacao,
        0,
        0,
        0,
        i
    );
    }

}

void testa_popular_dict() {
    std::ifstream Htcsv("dados/Ht.csv");
    Matriz Ht;
    boost::unordered_map<unsigned long, unsigned long> dict;

    carrega_matriz(Htcsv, Ht);
    int n_linhas = Ht.size();
    int n_colunas =  Ht[0].size();
    int n_informacao = n_linhas - n_colunas;

    popular_dict(
        dict,
        Ht,
        n_linhas,
        n_colunas,
        n_informacao,
        3
    );

    assert(dict.find(0) == dict.end());
    assert(dict.at(1099511595008) == 34359738368); // 1ª linha de Ht -> erro só no 1º bit de info
    assert(dict.at(1064615018496) == 17179869184); // 2ª linha de Ht -> erro só no 2º bit de info
    assert(dict.at(34896576512) == 51539607552); // 1ª+2ª linhas de Ht -> erros nos 2 MSB de info
    assert(dict.at(1052568944640) == 55834574848); // 1ª+2ª+4ª linhas de Ht -> erros nos 1º,2º,4º bits de info
    assert(dict.at(549755813888) == 0); // erro composto só pela 37ª linha de Ht não aparece nos bits de info
    assert(dict.at(481038172167) == 1); // mas se erro for de 36ª+37ª linhas de Ht -> erro no último bit de info
                                         // (e ignora o bit de paridade)
}

/**
* Retorna nova matriz em que cada linha é uma coluna da matriz original.
*/
Matriz& transposta(
    const Matriz& M
) {
    int antes_colunas = M[0].size();
    int antes_linhas = M.size();
    Matriz* resultado = new Matriz(antes_colunas);

    for (int i = 0; i < antes_colunas; ++i)
    {
        (*resultado)[i] = Vetor (antes_linhas);
        // Se não for Vetor&, boost copia implicitamente
        Vetor& linha = (*resultado)[i];
        for (int j = 0; j < antes_linhas; ++j)
        {
            linha[antes_linhas - j - 1] = M[j][antes_colunas - i - 1];
        };
    }
    return *resultado;
}

void testa_transposta() {
    Matriz m = Matriz(3);
    /*
    * m == [1 1; 0 1; 0 0]
    */
    m[0] = Vetor(2, 3);
    m[1] = Vetor(2, 1);
    m[2] = Vetor(2, 0);

    Matriz mt = transposta(m);

    assert(mt[0] == Vetor(3, 4));
    assert(mt[1] == Vetor(3, 6));
}

/**
* Calcula M.v
*/
Vetor& mult(const Matriz& M, const Vetor& v) {
    int n_linhas = M.size(), n_colunas = v.size();
    Vetor* resultado = new Vetor(n_linhas);

    for (int i = 0; i < n_linhas; ++i)
    {
        (*resultado)[n_linhas - i - 1] =  (M[i] & v).count() & 1;
    }

    return *resultado;
}

/**
* Calcula M.N
*/
Matriz& mult(const Matriz& M, const Matriz& N) {
    int n_linhas = M.size();
    int n_colunas = N[0].size();
    Matriz pre_resultado = Matriz(n_colunas);

    Matriz Nt = transposta(N);

    for (int i = 0; i < n_colunas; ++i)
    {
        pre_resultado[i] = mult(M, Nt[i]);
    }

    Matriz& resultado = transposta(pre_resultado);

    return resultado;
}

void testa_mult() {
    // M == [1 0 1; 0 0 1]
    Matriz M = Matriz(2);
    M[0] = Vetor(3, 5);
    M[1] = Vetor(3, 1);

    // v == [0 1 1]
    Vetor v = Vetor(3, 3);

    // resultado deve ser [1 1]
    assert(mult(M, v) == Vetor(2, 3));

    // [1 1; 0 0; 1 1]
    Matriz N = Matriz(3);
    N[0] = Vetor(2, 3);
    N[1] = Vetor(2, 0);
    N[2] = Vetor(2, 3);

    assert(mult(M, N).size() == 2);
    assert(mult(M, N)[0] == Vetor(2, 0));
    assert(mult(M, N)[1] == Vetor(2, 7));
}

/**
* Modifica `Transmitido`, com chance `p` de inverter cada bit
* de cada palavra código.
*
* Transmitido: Assume-se que cada linha é uma palavra-código
*/
void canal(Matriz& Transmitido, double p) {
    int count=0;
    int n_linhas = Transmitido.size();
    int n_colunas = Transmitido[0].size();
    Vetor* linha;
    boost::random::niederreiter_base2 gen(4);
    boost::random::uniform_01<double> random;
    for (int i = 0; i < n_linhas; ++i)
    {
        linha = &Transmitido[i];
        for (int j = 0; j < n_colunas; ++j)
        {
            if (random(gen) < p) {
                (*linha)[j].flip();
                count++;
            }
        }
    }
    // std::cout << "p ≃ " << ((double) count) / (n_linhas*n_colunas) << std::endl;
}

/**
* amostras_informacao: Lista de palavras de informacao a serem enviadas
*   Espera-se uma palavra por linha, elementos separados por vírgulas (sem espaço)
*
* Ht_csv: Matriz de verificação de paridade (transposta). Mesmo formato de `amostras_informacao_csv`
*
* Gt_csv: Matriz de geração do código (transposta). Mesmo formato de `amostras_informacao_csv`
*
* p: Lista de chances de o canal BSC inverter um bit transmitido
*
* peso_maximo_memorizado: Caso se encontre síndrome com peso maior que isto, ela não será corrigida
*
* Retorna lista de chances de erro de bit (uma para cada valor de p)
*/
boost::container::vector<double> desempenho(
    std::ifstream& amostras_informacao_csv,
    std::ifstream& Ht_csv,
    std::ifstream& Gt_csv,
    const boost::container::vector<double>& p,
    int peso_maximo_memorizado
) {
    boost::container::vector<double>* resultado = new boost::container::vector<double>(p.size());

    Matriz Ht;
    carrega_matriz(Ht_csv, Ht);

    /**
    * Exemplo de chave e valor:
    * - chave: 0b1001 , valor: 0b101 significa uma síndrome [1, 0, 0, 1] com erro associado [1, 0, 1, ...]
    *   em que as reticências indicam a parte do erro concernente aos bits de paridade (não importam)
    */
    boost::unordered_map<unsigned long, unsigned long> dict;

    int n_linhas = Ht.size();
    int n_colunas = Ht[0].size();
    int n_informacao = n_linhas - n_colunas;

    popular_dict(
        dict,
        Ht,
        n_linhas,
        n_colunas,
        n_informacao,
        peso_maximo_memorizado
    );

    Matriz Info;
    carrega_matriz(amostras_informacao_csv, Info);

    int n_amostras = Info.size();

    Matriz Gt;
    carrega_matriz(Gt_csv, Gt);
    Matriz G = transposta(Gt);

    for (int i_p = 0; i_p < p.size(); ++i_p)
    {
        Matriz Transmitido = mult(Info, G);

        canal(Transmitido, p[i_p]);

        Matriz Sindromes = mult(Transmitido, Ht);

        Matriz Transmitido_informacao = Matriz(n_amostras);
        for (int i = 0; i < n_amostras; ++i)
        {
            Transmitido_informacao[i] = Vetor(n_informacao);
            for (int j = 0; j < n_informacao; ++j)
            {
                Transmitido_informacao[i][n_informacao - j - 1] = Transmitido[i][n_linhas - j - 1];
            }
            // assert(Transmitido_informacao.size() == Transmitido.size());
            // assert(Transmitido_informacao[0].size() == Info[0].size());
            // assert(Transmitido_informacao[i] == Info[i]);
        }

        int n_erros = 0;
        int incr=0;
        unsigned long sindrome;
        Vetor correcao_nula = Vetor(n_informacao, 0);
        Vetor correcao;
        for (int i = 0; i < n_amostras; ++i)
        {
            sindrome = Sindromes[i].to_ulong();
            if (dict.find(sindrome) == dict.end())
                correcao = correcao_nula;
            else
                correcao = Vetor(n_informacao, dict.at(sindrome));
            incr = (Transmitido_informacao[i] ^ correcao ^ Info[i]).count();
            // if (incr != 0) {
            //     std::cout << correcao << ": " << Transmitido_informacao[i] << " versus " << Info[i] << std::endl;
            //     std::cout << "diferença " << (Transmitido_informacao[i] ^ Info[i]) << std::endl;
            //     std::cout << "Originais: " << Transmitido[i] << " versus " << mult(Gt, Info[i]) << std::endl;
            //     std::cout << "diferença: " << (Transmitido[i] ^ mult(Gt, Info[i])) << std::endl;
            //     std::cout << "síndrome: " << Sindromes[i] << std::endl;
            //     std::cout << std::endl;
            // }

            n_erros += incr;
        }

        (*resultado)[i_p] = n_erros / ((double) n_amostras * n_informacao);
        std::cout << "parcial(" << i_p << "): " << (*resultado)[i_p] << std::endl;
    }
    return *resultado;
}

int main(int argc, char** argv) {

    int arg_peso_maximo = std::stoi(argv[1]);
    std::string arg_resultados = argv[2];

    std::ifstream Htcsv ("dados/Ht.csv");
    std::ifstream amostrasInput("dados/amostra-informacao.csv");
    std::ifstream GtInput("dados/Gt.csv");
    std::ofstream Resultados(arg_resultados);
    std::ofstream P("dados/lista-de-p.csv");

    boost::container::vector<double> p = boost::container::vector<double>(0);
    double p0 = 0.5;
    while(p0 > 1 /((double) 1000000)) {
        p.push_back(p0);
        p0 *= 0.5;
    }

    boost::container::vector<double> des = desempenho(amostrasInput, Htcsv, GtInput, p, arg_peso_maximo);
    Resultados << des[0];
    P << p[0];
    for (int i = 1; i < des.size(); ++i)
    {
        Resultados << ",";
        Resultados << des[i];
        P << ",";
        P << p[i];
    }
    Resultados << std::endl;
    P << std::endl;

    return 0;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include "print_values.hpp"

#include <mpllibs/metamonad/lambda_c.hpp>
#include <mpllibs/metamonad/name.hpp>
#include <mpllibs/metamonad/metafunction.hpp>
#include <mpllibs/metamonad/lazy_metafunction.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/lazy_protect_args.hpp>
#include <mpllibs/metamonad/if_.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/apply.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/equal_to.hpp>

#include <iostream>

using boost::mpl::int_;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::list_c;
using boost::mpl::insert_range;
using boost::mpl::end;
using boost::mpl::equal_to;

using mpllibs::metamonad::lambda_c;
using mpllibs::metamonad::lazy;
using mpllibs::metamonad::lazy_protect_args;
using mpllibs::metamonad::if_;
using mpllibs::metamonad::tmp_value;
using mpllibs::metamonad::apply;

using namespace mpllibs::metamonad::name;

/*
 * The typeclass
 */
template <class Tag>
struct addable;
// Requires:
//   struct add { template <class, class> struct apply; };

MPLLIBS_METAFUNCTION(add, (Tag)(A)(B))
((apply<typename addable<Tag>::add, A, B>));

MPLLIBS_METAFUNCTION(double_, (Tag)(X))
((apply<typename addable<Tag>::double_, X>));

template <class Tag>
struct addable_defaults
{
  typedef lambda_c<n, add<Tag, n, n> > double_;
};

/*
 * Integers
 */
typedef int_<0>::tag int_tag;

template <>
struct addable<int_tag> : addable_defaults<int_tag>
{
  struct add : tmp_value<add>
  {
    MPLLIBS_LAZY_METAFUNCTION(apply, (A)(B)) ((int_<A::value + B::value>));
  };
};

/*
 * Lists
 */
typedef list_c<int>::tag list_tag;

template <>
struct addable<list_tag> : addable_defaults<list_tag>
{
  typedef
    lambda_c<a, b,
      lazy<
        insert_range<
          lazy_protect_args<a>,
          end<lazy_protect_args<a> >,
          lazy_protect_args<b>
        >
      >
    >
    add;
};

/*
 * Generic function
 */
MPLLIBS_LAZY_METAFUNCTION(mult, (T)(N))
((
  if_<
    equal_to<int_<1>, N>,
    T,
    add<typename T::tag, mult<T, minus<N, int_<1> > >, T>
  >
));

int main()
{
  using std::cout;
  using std::endl;

  cout
    << "add<int_tag, 11, 2> = " << add<int_tag, int_<11>, int_<2> >::type::value
    << endl;

  cout
    << "double_<int_tag, 3> = " << double_<int_tag, int_<3> >::type::value
    << endl;

  cout
    << "mult<3, 4> = " << mult<int_<3>, int_<4> >::type::value
    << endl;

  cout << "add<list_tag, [1, 2], [3, 4]> = ";
  print_values<add<list_tag, list_c<int, 1, 2>, list_c<int, 3, 4> > >();
  cout << endl;

  cout << "double_<list_tag, [1, 2]> = ";
  print_values<double_<list_tag, list_c<int, 1, 2> > >();
  cout << endl;

  cout << "mult<[1, 2], 4> = ";
  print_values<mult<list_c<int, 1, 2>, int_<4> > >();
  cout << endl;
}


/*
 * ***** BEGIN GPL LICENSE BLOCK *****
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2018 by Blender Foundation.
 * All rights reserved.
 *
 * Contributor(s): MARUI-PlugIn
 *
 * ***** END GPL LICENSE BLOCK *****
 */

/** \file blender/vr/intern/vr_math.cpp
 *   \ingroup vr
 *
 * Collection of VR math-related utility functions.
 */

#include "vr_types.h"

#include "vr_math.h"

#include <Eigen/Dense>

/* Externed in vr_types.h */
bool mat44_inverse(float inv[4][4], const float m[4][4])
{
  Eigen::Map<Eigen::Matrix4f> mat = Eigen::Map<Eigen::Matrix4f>((float *)m);
  Eigen::Matrix4f out;
  bool invertible = true;
  mat.computeInverseWithCheck(out, invertible, 0.0f);
  if (!invertible) {
    out = out.Zero();
  }
  memcpy(inv, out.data(), sizeof(float) * 4 * 4);
  return invertible;
}

/* Externed in vr_types.h */
bool mat44_inverse(double inv[4][4], const double m[4][4])
{
  Eigen::Map<Eigen::Matrix4d> mat = Eigen::Map<Eigen::Matrix4d>((double *)m);
  Eigen::Matrix4d out;
  bool invertible = true;
  mat.computeInverseWithCheck(out, invertible, 0.0f);
  if (!invertible) {
    out = out.Zero();
  }
  memcpy(inv, out.data(), sizeof(double) * 4 * 4);
  return invertible;
}

static void mat44_multiply_unique(float R[4][4], const float A[4][4], const float B[4][4])
{
  /* matrix product: R[j][k] = A[j][i] . B[i][k] */
  __m128 A0 = _mm_loadu_ps(A[0]);
  __m128 A1 = _mm_loadu_ps(A[1]);
  __m128 A2 = _mm_loadu_ps(A[2]);
  __m128 A3 = _mm_loadu_ps(A[3]);

  for (int i = 0; i < 4; i++) {
    __m128 B0 = _mm_set1_ps(B[i][0]);
    __m128 B1 = _mm_set1_ps(B[i][1]);
    __m128 B2 = _mm_set1_ps(B[i][2]);
    __m128 B3 = _mm_set1_ps(B[i][3]);

    __m128 sum = _mm_add_ps(_mm_add_ps(_mm_mul_ps(B0, A0), _mm_mul_ps(B1, A1)),
                            _mm_add_ps(_mm_mul_ps(B2, A2), _mm_mul_ps(B3, A3)));

    _mm_storeu_ps(R[i], sum);
  }
}
static void mat44_pre_multiply(float R[4][4], const float A[4][4])
{
  float B[4][4];
  std::memcpy(B, R, sizeof(float) * 4 * 4);
  mat44_multiply_unique(R, A, B);
}
static void mat44_post_multiply(float R[4][4], const float B[4][4])
{
  float A[4][4];
  std::memcpy(A, R, sizeof(float) * 4 * 4);
  mat44_multiply_unique(R, A, B);
}
/* Externed in vr_types.h */
void mat44_multiply(float R[4][4], const float A[4][4], const float B[4][4])
{
  if (A == R) {
    mat44_post_multiply(R, B);
  }
  else if (B == R) {
    mat44_pre_multiply(R, A);
  }
  else {
    mat44_multiply_unique(R, A, B);
  }
}

static void mat44_multiply_unique(double R[4][4], const double A[4][4], const double B[4][4])
{
  /* matrix product: R[j][k] = A[j][i] . B[i][k] */
  __m128d A0 = _mm_loadu_pd(A[0]);
  __m128d A1 = _mm_loadu_pd(A[1]);
  __m128d A2 = _mm_loadu_pd(A[2]);
  __m128d A3 = _mm_loadu_pd(A[3]);

  for (int i = 0; i < 4; i++) {
    __m128d B0 = _mm_set1_pd(B[i][0]);
    __m128d B1 = _mm_set1_pd(B[i][1]);
    __m128d B2 = _mm_set1_pd(B[i][2]);
    __m128d B3 = _mm_set1_pd(B[i][3]);

    __m128d sum = _mm_add_pd(_mm_add_pd(_mm_mul_pd(B0, A0), _mm_mul_pd(B1, A1)),
                             _mm_add_pd(_mm_mul_pd(B2, A2), _mm_mul_pd(B3, A3)));

    _mm_storeu_pd(R[i], sum);
  }
}
static void mat44_pre_multiply(double R[4][4], const double A[4][4])
{
  double B[4][4];
  std::memcpy(B, R, sizeof(double) * 4 * 4);
  mat44_multiply_unique(R, A, B);
}
static void mat44_post_multiply(double R[4][4], const double B[4][4])
{
  double A[4][4];
  std::memcpy(A, R, sizeof(double) * 4 * 4);
  mat44_multiply_unique(R, A, B);
}
/* Externed in vr_types.h */
void mat44_multiply(double R[4][4], const double A[4][4], const double B[4][4])
{
  if (A == R) {
    mat44_post_multiply(R, B);
  }
  else if (B == R) {
    mat44_pre_multiply(R, A);
  }
  else {
    mat44_multiply_unique(R, A, B);
  }
}

float ident_f[4][4] = {1.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       1.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       1.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       0.0f,
                       1.0f};
const Mat44f VR_Math::identity_f = ident_f;

double ident_d[4][4] = {
    1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0};
const Mat44d VR_Math::identity_d = ident_d;

void VR_Math::multiply_mat44_coord3D(Coord3Df &r, const Mat44f &m, const Coord3Df &v)
{
  const float &x = v.x;
  const float &y = v.y;
  const float &z = v.z;

  r.x = x * m.m[0][0] + y * m.m[1][0] + z * m.m[2][0] + m.m[3][0];
  r.y = x * m.m[0][1] + y * m.m[1][1] + z * m.m[2][1] + m.m[3][1];
  r.z = x * m.m[0][2] + y * m.m[1][2] + z * m.m[2][2] + m.m[3][2];
}

float VR_Math::matrix_distance(const Mat44f &a, const Mat44f &b)
{
  float dx = a.m[3][0] - b.m[3][0];
  float dy = a.m[3][1] - b.m[3][1];
  float dz = a.m[3][2] - b.m[3][2];
  return sqrt(dx * dx + dy * dy + dz * dz);
}

float VR_Math::matrix_rotation(const Mat44f &a, const Mat44f &b, Coord3Df *axis)
{
  Coord3Df _axis;
  float angle = 0.0f;
  Quatf a_rotation(a);
  Quatf b_rotation(b);
  (a_rotation.inverse() * b_rotation).to_axis_angle(_axis, angle);
  angle = (float)RADTODEG(angle);
  float counter_angle = 360.0f - angle; /* rotation in opposite direction */
  if (angle > counter_angle) {
    angle = counter_angle;
  }
  if (axis) {
    *axis = _axis;
  }
  return angle;
}

void VR_Math::orient_matrix_z(Mat44f &m, Coord3Df z)
{
  z.normalize_in_place();
  Coord3Df x(m.m[0][0], m.m[0][1], m.m[0][2]); /* x-axis */
  Coord3Df y(m.m[1][0], m.m[1][1], m.m[1][2]); /* y-axis */
  float scale = x.length();
  y = (z ^ x).normalize() * scale; /* rectify y */
  x = (y ^ z).normalize() * scale; /* rectify x */
  z *= scale;                      /* give z the correct length */
  m.m[0][0] = x.x;
  m.m[0][1] = x.y;
  m.m[0][2] = x.z;
  m.m[1][0] = y.x;
  m.m[1][1] = y.y;
  m.m[1][2] = y.z;
  m.m[2][0] = z.x;
  m.m[2][1] = z.y;
  m.m[2][2] = z.z;
}


// test_alg1.cpp - original U(N)->U(3) reduction implemented by LSU3shell
//
// License: BSD 2-Clause (https://opensource.org/licenses/BSD-2-Clause)
//
// Copyright (c) 2019, Daniel Langr
// All rights reserved.
//
// Program implements the U(N) to U(3) the input irrep [f] specified by the HO level n, N=(n+1)*(n+2)/2,
// and its number of twos, ones, and zeros read from the standard input.
// For instance, for the input U(21) irrep [f] = [2,2,2,2,2,2,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0],
// the user should provide the following numbers: 5 6 1 14.
//
// The program performs the U(N) to U(3) reduction and calculates the sum of the dimensions
// of resulting U(3) irrpes multiplied by their level dimensionalities, and print it to the
// standard output. For instance, for the input irrep specified above, the output should read:
// U(3) irreps total dim = 2168999910
//
// This sum should be equal to dim[f], which can be calculated analytically with the support
// of rational numbers. The program performs this calculcation as well if the Boost library
// is available and uses its Boost.Rational sublibrary. Availabitliy of Boost is indicated by
// users by definition of HAVE_BOOST preprocessor symbol.
// For the input irrep [f] specified above, the program should first print out:
// U(N) irrep dim = 2168999910

#include <algorithm>
#include <functional>
#include <iostream>
#include <iterator>
#include <map>
#include <stdexcept>
#include <vector>

#ifdef HAVE_BOOST
#include <boost/rational.hpp>
#endif

namespace U3 {
   using LABELS = std::array<uint32_t, 3>;
   using SPS = std::array<std::vector<uint32_t>, 3>;
   enum { NZ, NX, NY };
};

namespace UN {
   using LABELS = std::vector<uint8_t>;
   using BASIS_STATE_WEIGHT_VECTOR = std::vector<uint8_t>;
   using U3MULT_LIST = std::map<U3::LABELS, uint32_t>;
}

enum { MULT, LM, MU, S2 };

void Weight2U3Label(const UN::BASIS_STATE_WEIGHT_VECTOR& vWeights, const U3::SPS& ShellSPS,
                    U3::LABELS& vU3Labels) {
   vU3Labels[U3::NZ] =
       std::inner_product(ShellSPS[U3::NZ].begin(), ShellSPS[U3::NZ].end(), vWeights.begin(), 0);
   vU3Labels[U3::NX] =
       std::inner_product(ShellSPS[U3::NX].begin(), ShellSPS[U3::NX].end(), vWeights.begin(), 0);
   vU3Labels[U3::NY] =
       std::inner_product(ShellSPS[U3::NY].begin(), ShellSPS[U3::NY].end(), vWeights.begin(), 0);
}

void GenerateU3Labels(const UN::LABELS& vGelfandParentRow, uint32_t uSumGelfandParentRow,
                      const U3::SPS& ShellSPS, UN::BASIS_STATE_WEIGHT_VECTOR& vWeights,
                      UN::U3MULT_LIST& mU3LabelsOccurance) {
   size_t N = vGelfandParentRow.size() - 1;

   std::vector<UN::LABELS> vvAllowedLabels;
   UN::LABELS vGelfandRow(N);
   std::vector<uint32_t> vElemsPerChange(N, 1);  // Fill with 1 cause vElemsPerChange[0] = 1;
   std::vector<size_t> vNLabels(N);

   // evaluate all allowed Gelfand patterns based on a parent Gelfand row
   // (vGelfandParentRow) and store them in vvAllowedLabels
   // iNAllowedCombinations is equal to the number of allowed Gelfand patterns
   uint32_t iNAllowedCombinations = 1;
   for (size_t i = 0; i < N; i++) {
      uint32_t uLabelMin = std::min(vGelfandParentRow[i + 1], vGelfandParentRow[i]);
      uint32_t uLabelMax = std::max(vGelfandParentRow[i + 1], vGelfandParentRow[i]);
      UN::LABELS vLabels(uLabelMax - uLabelMin + 1);
      std::iota(vLabels.begin(), vLabels.end(), uLabelMin);
      vNLabels[i] = vLabels.size();
      iNAllowedCombinations *= vNLabels[i];
      vvAllowedLabels.push_back(std::move(vLabels));
   }

   for (size_t i = 1; i < N; i++) {
      vElemsPerChange[i] = vElemsPerChange[i - 1] *
                           vNLabels[i - 1];  // if i == 0 => vElemsPerChange[0]=1 since constructor
   }

   for (uint32_t index = 0; index < iNAllowedCombinations; index++) {
      for (size_t i = 0; i < N; i++) {
         size_t iElement = (index / vElemsPerChange[i]) % vNLabels[i];
         vGelfandRow[i] = vvAllowedLabels[i][iElement];
      }
      uint32_t uSumGelfandRow = std::accumulate(vGelfandRow.begin(), vGelfandRow.end(), 0);
      vWeights[N] = uSumGelfandParentRow - uSumGelfandRow;
      if (N > 1) {  // this condition is due to n = 0 case when N = 0 and hence one wants to keep
                    // vWeights[0] = uSumGelfandRow;
         GenerateU3Labels(vGelfandRow, uSumGelfandRow, ShellSPS, vWeights, mU3LabelsOccurance);
      } else {
         if (N == 1) {
            vWeights[0] = uSumGelfandRow;
         }
         U3::LABELS vU3Labels = {0, 0, 0};
         Weight2U3Label(vWeights, ShellSPS, vU3Labels);
         mU3LabelsOccurance[vU3Labels] += 1;
      }
   }
}

uint32_t GetMultiplicity(const U3::LABELS u3_labels, const UN::U3MULT_LIST& u3_mult_map) {
   auto u3_mult = u3_mult_map.find(u3_labels);
   assert(u3_mult != u3_mult_map.end());

   uint32_t f1 = u3_labels[0], f2 = u3_labels[1], f3 = u3_labels[2];
   uint32_t mult = u3_mult->second;

   u3_mult = u3_mult_map.find({f1 + 1, f2 + 1, f3 - 2});
   mult += (u3_mult == u3_mult_map.end()) ? 0 : u3_mult->second;

   u3_mult = u3_mult_map.find({f1 + 2, f2 - 1, f3 - 1});
   mult += (u3_mult == u3_mult_map.end()) ? 0 : u3_mult->second;

   u3_mult = u3_mult_map.find({f1 + 2, f2, f3 - 2});
   mult -= (u3_mult == u3_mult_map.end()) ? 0 : u3_mult->second;

   u3_mult = u3_mult_map.find({f1 + 1, f2 - 1, f3});
   mult -= (u3_mult == u3_mult_map.end()) ? 0 : u3_mult->second;

   u3_mult = u3_mult_map.find({f1, f2 + 1, f3 - 1});
   mult -= (u3_mult == u3_mult_map.end()) ? 0 : u3_mult->second;

   return mult;
}

void GenerateU3SPS(int n, U3::SPS& ShellSPS) {
   for (int k = 0; k <= n; k++) {
      uint32_t nz = n - k;
      for (int nx = k; nx >= 0; nx--) {
         ShellSPS[U3::NX].push_back(nx);
         ShellSPS[U3::NY].push_back(n - nz - nx);
         ShellSPS[U3::NZ].push_back(nz);
      }
   }
}

#ifdef HAVE_BOOST
template <typename T>
unsigned long dim(const T & irrep) {
   const auto N = irrep.size();
   boost::rational<unsigned long> result{1};
   for (uint32_t l = 2; l <= N; l++)
      for (uint32_t k = 1; k <= l - 1; k++)
         result *= { irrep[k - 1] - irrep[l - 1] + l - k, l - k };

   assert(result.denominator() == 1);
   return result.numerator();
}
#endif

// special case for U(3) irreps (does not require Boost rational numbers)
unsigned long dim(const U3::LABELS & irrep) {
   return (irrep[0] - irrep[1] + 1) * (irrep[0] - irrep[2] + 2) * (irrep[1] - irrep[2] + 1) / 2;
}

int main() {
   unsigned long n;
   unsigned short n2, n1, n0;
   std::cin >> n >> n2 >> n1 >> n0;
   unsigned long N = (n + 1) * (n + 2) / 2;

   if ((n2 + n1 + n0) != N)
      throw std::invalid_argument("Arguments mismatch!");

   U3::SPS ShellSPS;
   GenerateU3SPS(n, ShellSPS);

   UN::LABELS UNLabels(n2, 2);
   std::fill_n(std::back_inserter(UNLabels), n1, 1);
   std::fill_n(std::back_inserter(UNLabels), n0, 0);

#ifdef HAVE_BOOST
   std::cout << "U(N) irrep dim = " << dim(UNLabels) << std::endl;
#endif

   uint32_t sumUNLabels = std::accumulate(UNLabels.begin(), UNLabels.end(), 0);

   UN::U3MULT_LIST mU3_mult;
   UN::BASIS_STATE_WEIGHT_VECTOR Weight(UNLabels.size());

   GenerateU3Labels(UNLabels, sumUNLabels, ShellSPS, Weight, mU3_mult);

   unsigned long sum = 0;
   for (const auto& u3labels_mult : mU3_mult) {
      U3::LABELS U3Labels(u3labels_mult.first);

      if (U3Labels[U3::NZ] >= U3Labels[U3::NX] && U3Labels[U3::NX] >= U3Labels[U3::NY])
         if (uint32_t u3_mult = GetMultiplicity(U3Labels, mU3_mult))
            sum += u3_mult * dim(U3Labels);
   }
   std::cout << "U(3) irreps total dim = " << sum << std::endl;
}


#ifndef STAN_MATH_REV_MAT_FUN_VARIANCE_HPP
#define STAN_MATH_REV_MAT_FUN_VARIANCE_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/mean.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <vector>

namespace stan {
namespace math {

namespace {

inline var calc_variance(size_t size, const var* dtrs) {
  vari** varis = reinterpret_cast<vari**>(
      ChainableStack::instance().memalloc_.alloc(size * sizeof(vari*)));
  for (size_t i = 0; i < size; ++i)
    varis[i] = dtrs[i].vi_;
  double sum = 0.0;
  for (size_t i = 0; i < size; ++i)
    sum += dtrs[i].vi_->val_;
  double mean = sum / size;
  double sum_of_squares = 0;
  for (size_t i = 0; i < size; ++i) {
    double diff = dtrs[i].vi_->val_ - mean;
    sum_of_squares += diff * diff;
  }
  double variance = sum_of_squares / (size - 1);
  double* partials = reinterpret_cast<double*>(
      ChainableStack::instance().memalloc_.alloc(size * sizeof(double)));
  double two_over_size_m1 = 2 / (size - 1);
  for (size_t i = 0; i < size; ++i)
    partials[i] = two_over_size_m1 * (dtrs[i].vi_->val_ - mean);
  return var(new stored_gradient_vari(variance, size, varis, partials));
}

}  // namespace

/**
 * Return the sample variance of the specified standard
 * vector.  Raise domain error if size is not greater than zero.
 *
 * @param[in] v a vector
 * @return sample variance of specified vector
 */
inline var variance(const std::vector<var>& v) {
  check_nonzero_size("variance", "v", v);
  if (v.size() == 1)
    return 0;
  return calc_variance(v.size(), &v[0]);
}

/*
 * Return the sample variance of the specified vector, row vector,
 * or matrix.  Raise domain error if size is not greater than
 * zero.
 *
 * @tparam R number of rows
 * @tparam C number of columns
 * @param[in] m input matrix
 * @return sample variance of specified matrix
 */
template <int R, int C>
var variance(const Eigen::Matrix<var, R, C>& m) {
  check_nonzero_size("variance", "m", m);
  if (m.size() == 1)
    return 0;
  return calc_variance(m.size(), &m(0));
}

}  // namespace math
}  // namespace stan
#endif


/*
 * @Description: ceres sliding window optimizer, interface
 * @Author: Ge Yao
 * @Date: 2021-01-03 14:53:21
 */

#ifndef LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_CERES_SLIDING_WINDOW_HPP_
#define LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_CERES_SLIDING_WINDOW_HPP_

#include <memory>

#include <string>

#include <vector>
#include <deque>

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

#include "lidar_localization/sensor_data/key_frame.hpp"

#include "lidar_localization/models/pre_integrator/imu_pre_integrator.hpp"

#include <ceres/ceres.h>

#include "lidar_localization/models/sliding_window/params/param_prvag.hpp"

#include "lidar_localization/models/sliding_window/factors/factor_prvag_marginalization.hpp"
#include "lidar_localization/models/sliding_window/factors/factor_prvag_relative_pose.hpp"
#include "lidar_localization/models/sliding_window/factors/factor_prvag_map_matching_pose.hpp"
#include "lidar_localization/models/sliding_window/factors/factor_prvag_imu_pre_integration.hpp"

namespace lidar_localization {

class CeresSlidingWindow {
public:
    static const int INDEX_P = 0;
    static const int INDEX_R = 3;
    static const int INDEX_V = 6;
    static const int INDEX_A = 9;
    static const int INDEX_G = 12;

    struct OptimizedKeyFrame {
      double time;
      double prvag[15];
      bool fixed = false;
    };

    struct ResidualMapMatchingPose {
      int param_index;

      Eigen::VectorXd m;
      Eigen::MatrixXd I;
    };

    struct ResidualRelativePose {
      int param_index_i;
      int param_index_j;

      Eigen::VectorXd m;
      Eigen::MatrixXd I;
    };

    struct ResidualIMUPreIntegration {
      int param_index_i;
      int param_index_j;

      double T;
      Eigen::Vector3d g;
      Eigen::VectorXd m;
      Eigen::MatrixXd I;
      Eigen::MatrixXd J;
    };

    CeresSlidingWindow(const int N);
    ~CeresSlidingWindow();

    /**
     * @brief  add parameter block for LIO key frame
     * @param  lio_key_frame, LIO key frame with (pos, ori, vel, b_a and b_g)
     * @param  fixed, shall the param block be fixed to eliminate trajectory estimation ambiguity
     * @return true if success false otherwise
     */
    void AddPRVAGParam(
      const KeyFrame &lio_key_frame, const bool fixed
    );

    /**
     * @brief  add residual block for relative pose constraint from lidar frontend
     * @param  param_index_i, param block ID of previous key frame
     * @param  param_index_j, param block ID of current key frame
     * @param  relative_pose, relative pose measurement
     * @param  noise, relative pose measurement noise
     * @return void
     */
    void AddPRVAGRelativePoseFactor(
      const int param_index_i, const int param_index_j,
      const Eigen::Matrix4d &relative_pose, const Eigen::VectorXd &noise
    );

    /**
     * @brief  add residual block for prior pose constraint from map matching
     * @param  param_index, param block ID of current key frame
     * @param  prior_pose, prior pose measurement
     * @param  noise, prior pose measurement noise
     * @return void
     */
    void AddPRVAGMapMatchingPoseFactor(
      const int param_index,
      const Eigen::Matrix4d &prior_pose, const Eigen::VectorXd &noise
    );

    /**
     * @brief  add residual block for IMU pre-integration constraint from IMU measurement
     * @param  param_index_i, param block ID of previous key frame
     * @param  param_index_j, param block ID of current key frame
     * @param  imu_pre_integration, IMU pre-integration measurement
     * @return void
     */
    void AddPRVAGIMUPreIntegrationFactor(
      const int param_index_i, const int param_index_j,
      const IMUPreIntegrator::IMUPreIntegration &imu_pre_integration
    );

    // do optimization
    bool Optimize();

    // get num. of parameter blocks:
    int GetNumParamBlocks();

    /**
     * @brief  get optimized odometry estimation
     * @param  optimized_key_frame, output latest optimized key frame
     * @return true if success false otherwise
     */
    bool GetLatestOptimizedKeyFrame(KeyFrame &optimized_key_frame);

    /**
     * @brief  get optimized LIO key frame state estimation
     * @param  optimized_key_frames, output optimized LIO key frames
     * @return true if success false otherwise
     */
    bool GetOptimizedKeyFrames(std::deque<KeyFrame> &optimized_key_frames);

private:
    /**
     * @brief  create information matrix from measurement noise specification
     * @param  noise, measurement noise covariances
     * @return information matrix as square Eigen::MatrixXd
     */
    Eigen::MatrixXd GetInformationMatrix(Eigen::VectorXd noise);

    // a. sliding window config:
    const int kWindowSize;

    // b. optimizer config:
    struct {
      // 1. loss function:
      std::unique_ptr<ceres::LossFunction> loss_function_ptr;
      // 2. solver:
      ceres::Solver::Options options;
    } config_;

    // c. data buffer:
    // c.1. param blocks:
    std::vector<OptimizedKeyFrame> optimized_key_frames_;

    // c.2. residual blocks:
    sliding_window::FactorPRVAGMapMatchingPose *GetResMapMatchingPose(const ResidualMapMatchingPose &res_map_matching_pose);
    sliding_window::FactorPRVAGRelativePose *GetResRelativePose(const ResidualRelativePose &res_relative_pose);
    sliding_window::FactorPRVAGIMUPreIntegration *GetResIMUPreIntegration(const ResidualIMUPreIntegration &res_imu_pre_integration);

    struct {
      std::deque<ResidualMapMatchingPose> map_matching_pose;
      std::deque<ResidualRelativePose> relative_pose;
      std::deque<ResidualIMUPreIntegration> imu_pre_integration;
    } residual_blocks_;
};

} // namespace lidar_localization

#endif // LIDAR_LOCALIZATION_MODELS_SLIDING_WINDOW_CERES_SLIDING_WINDOW_HPP_


#include <cassert>

// Before doing anything else, grab in the definitions of the test
// space and trial spaces that make up the finite element system.
#include <qdove/base/test_space.h>
#include <qdove/base/trial_space.h>
#include <qdove/materials/constants.h>

// Use the *solution* to a fick equation as a material function
#include <qdove/models/fick.h>

// Start by solving Schroedinger's problem
#include <qdove/models/schroedinger.h>

// and next by solving Poissons's problem
#include <qdove/models/poisson.h>

// Let's use a function from the qdove library for a psudo potential.
#include <qdove/psuedopotentials/function_library.h>

// Also make use of the predefined generalised eigenspectrum system -
// we need this for the Schroedinger problem.
#include <qdove/generic_linear_algebra/eigenspectrum_system.h>

// Also make use of the predefined linear algebra system - we need
// this for the Poisson problem.
#include <qdove/generic_linear_algebra/linear_algebra_system.h>

// Next up are some deal.II objects that have not been generalised
// away yet...
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/numerics/data_out.h>

// The purpose of this example is to have a working (dimensionless)
// Schroedinger-Poisson solver that can be generalised piecewise to
// the qdove library.
//
template<int dim>
class SelfConsistentProblem
{
public:
  SelfConsistentProblem (dealii::Triangulation<dim> &triangulation);
  ~SelfConsistentProblem ();

  void write_gnuplot (const dealii::PETScWrappers::Vector &vector,
          const std::string                   &name,
          const unsigned int                   cycle);
  void run ();

private:
  // The description of the finite element basis
  qdove::TrialSpace<dim> trial_space;

  // Geometry description and boundary constraints
  qdove::TestSpace<dim> test_space;

  // Fick's problem, which is used to obtain a material profile.
  qdove::Fick::Solution<1> fick_solution;

  // The eigenspectrum system that will be solved by SLEPc
  qdove::Schroedinger::Problem<dim> schroedinger_problem;

  // The system of equations that will be solved by PETSc
  qdove::Poisson::Problem<dim> poisson_problem;

  // The eigenpairs from schroedinger's problem
  std::vector<dealii::PETScWrappers::Vector> eigenvectors;
  std::vector<double>                        eigenvalues;

  // The solution from poisson's problem
  dealii::PETScWrappers::Vector solution;

};

template<int dim>
SelfConsistentProblem<dim>::SelfConsistentProblem (dealii::Triangulation<dim> &triangulation)
  :
  trial_space (triangulation),
  test_space (trial_space),
  fick_solution (trial_space, test_space),
  schroedinger_problem (trial_space, test_space, 10),
  poisson_problem (trial_space, test_space)
{}

template<int dim>
SelfConsistentProblem<dim>::~SelfConsistentProblem ()
{}

template<int dim>
void
SelfConsistentProblem<dim>::write_gnuplot (const dealii::PETScWrappers::Vector &vector,
             const std::string                   &name,
             const unsigned int                   cycle)
{
  // Output a vector to gnuplot style file.
  std::ostringstream filename;
  filename << "solution-" << name << "-" << cycle << ".gpl";
  std::ofstream output (filename.str ().c_str ());

  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler (test_space.dofs ());
  data_out.add_data_vector (vector, name);

  // generate default patches and output.
  data_out.build_patches ();
  data_out.write_gnuplot (output);
}

template<int dim>
void
SelfConsistentProblem<dim>::run ()
{
  std::cout << "Test space:" << std::endl
      << "   Finite element type:          "
      << test_space.fe ().get_name ()
      << std::endl
      << "   Number of degrees of freedom: "
      << test_space.n_dofs ()
      << std::endl;

  const double energy_height = 0.5 * qdove::E0;
  const double doping = 1e25;

  //
  // Setup a material function
  //
  fick_solution.reinit ();
  fick_solution.set_initial_height (1.);
  fick_solution.set_initial_rate(2e-10);
  fick_solution.set_symmetric_profile(false);

  dealii::PETScWrappers::Vector transition_1 (test_space.n_dofs ());
  fick_solution.set_initial_length (50e-10);
  fick_solution.interpolate_analytic_solution (transition_1);

  dealii::PETScWrappers::Vector transition_2 (test_space.n_dofs ());
  fick_solution.set_initial_length (100e-10);
  fick_solution.interpolate_analytic_solution (transition_2);

  dealii::PETScWrappers::Vector transition_3 (test_space.n_dofs ());
  fick_solution.set_initial_length (150e-10);
  fick_solution.interpolate_analytic_solution (transition_3);

  dealii::PETScWrappers::Vector material_function (test_space.n_dofs ());
  for (std::size_t i=0; i<material_function.size(); ++i)
    material_function[i] =  (1.0 - transition_1[i])
                           + transition_2[i] * 0.20/0.35  // for Al_{0.20}Ga_{0.80}As
                           + transition_3[i] * 0.15/0.35;      // for Al_{0.35}Ga_{0.65}As
  write_gnuplot (material_function, "material_function", 0);

  // set up the effective mass
  dealii::PETScWrappers::Vector kinetic_energy_prefactor (test_space.n_dofs ());
  for (std::size_t i=0; i<kinetic_energy_prefactor.size(); ++i)
    kinetic_energy_prefactor[i] = (qdove::HBAR*qdove::HBAR) / (2.*(qdove::mstar_GaAs + 0.35*material_function[i]*0.083)*qdove::M0);
  write_gnuplot (kinetic_energy_prefactor, "kinetic_energy_prefactor", 0);

  // set up the initial guess for the electron potential (i.e. electrostatic potential multiplied by elementary charge)
  dealii::PETScWrappers::Vector potential (test_space.n_dofs ());
  potential = material_function;
  potential *= 0.9 * energy_height; //start with no space charge region by selecting the potential right below the the Fer

  double E_bonding = 0.005 * qdove::E0; //5meV bonding energy of the ions
  double E_fermi = energy_height - E_bonding;

  //
  // Main iteration Schroedinger <-> Poisson
  //
  for (std::size_t cycle = 0; cycle < 100; ++cycle)
  {
    write_gnuplot (potential, "potential", cycle);

    // Get started on Schroedinger's problem
    std::cout << "Schroedinger's problem:" << std::endl;
    schroedinger_problem.reinit ();

    schroedinger_problem.assemble (kinetic_energy_prefactor, potential);
    schroedinger_problem.solve ();
    schroedinger_problem.get_solution_eigenpairs (eigenvalues, eigenvectors);
    write_gnuplot (eigenvectors[0], "electron_function", cycle);

    // output
    std::cout << "   Eigenvalues:                  ";
    for (unsigned int i=0; i<eigenvalues.size (); ++i)
      std::cout << eigenvalues[i] << " ";
    std::cout << std::endl;

    std::cout << "   Scaled values (meV):           ";
    for (unsigned int i=0; i<eigenvalues.size (); ++i)
      std::cout << eigenvalues[i]/(1e-03*qdove::E0) << " ";
    std::cout << std::endl;

    // Compute density from the eigenfunctions and eigenvalues:
    double kBT = 300.0 * qdove::KB;
    dealii::PETScWrappers::Vector density(test_space.n_dofs());
    for (unsigned int i=0; i<eigenvectors.size (); ++i)
    {
      double E_wavefunction = eigenvalues[i];
      double occupancy      =  kBT * std::log(1.0+std::exp((E_fermi - E_wavefunction)/kBT)); //integrated Fermi-Dirac statistic
      for (unsigned int j=0; j<eigenvectors[0].size (); ++j)
      {
        double density_of_states = (qdove::mstar_GaAs + 0.35*material_function[i]*0.083) * qdove::M0 / (qdove::HBAR*qdove::HBAR*qdove::PI);
        density[j] += density_of_states * eigenvectors[i][j] * eigenvectors[i][j] * occupancy;
      }
    }
    write_gnuplot (density, "density", cycle);

    // Set up RHS for Poisson's equation
    dealii::PETScWrappers::Vector rho(density.size());
    for (std::size_t i=0; i<solution.size(); ++i)
      if (potential[i] > E_fermi)
        rho[i] = -doping;
    rho += density;
    rho *= qdove::E0 * qdove::E0 / (qdove::permittivity_GaAs);
    write_gnuplot (rho, "rho", cycle);

    // Solve Poisson:
    std::cout << "Poisson's problem:" << std::endl;
    poisson_problem.reinit ();
    poisson_problem.assemble (rho);
    poisson_problem.solve ();
    poisson_problem.get_solution_vector (solution);
    write_gnuplot (solution, "poisson_solution", cycle);

    // Update potential:
    double max_update = 0;
    for (std::size_t i=0; i<solution.size(); ++i)
    {
      double old_value = potential[i] - 0.9 * energy_height * material_function[i];
      max_update = std::max(max_update, std::fabs(solution[i] - old_value));
    }

    double cutoff_update_value = 0.02 * qdove::E0;
    double alpha = (max_update > cutoff_update_value) ? cutoff_update_value / max_update : 1.0; //update by at most 10 mV for stability reasons

    alpha = 0.2;
    std::cout << "alpha: " << alpha << std::endl;

    for (std::size_t i=0; i<potential.size(); ++i)
    {
      double old_value = potential[i] - 0.9 * energy_height * material_function[i];
      double update = solution[i] - old_value;
      potential[i] += alpha * update;
    }
  }
}

int main (int argc, char **argv)
{
  try
    {
      dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);
      {
  // Create a grid
  dealii::Triangulation<1> triangulation;
  dealii::GridGenerator::hyper_cube (triangulation, 0, 200e-10);
  triangulation.refine_global (8);

  // Run Schroedinger's problem on that grid
  SelfConsistentProblem<1> self_consistent_problem (triangulation);
  self_consistent_problem.run ();
      }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


//  Copyright (c) 2006, Stephan Diederich
//
//  This code may be used under either of the following two licences:
//
//    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. OF SUCH DAMAGE.
//
//  Or:
//
//    Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//    http://www.boost.org/LICENSE_1_0.txt)

#include <boost/config.hpp>
#include <iostream>
#include <string>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/graph_utility.hpp>

// Use a DIMACS network flow file as stdin.
// boykov_kolmogorov-eg < max_flow.dat
//
// Sample output:
// c  The total flow:
// s 13
//
// c flow values:
// f 0 6 3
// f 0 1 6
// f 0 2 4
// f 1 5 1
// f 1 0 0
// f 1 3 5
// f 2 4 4
// f 2 3 0
// f 2 0 0
// f 3 7 5
// f 3 2 0
// f 3 1 0
// f 4 5 0
// f 4 6 4
// f 5 4 0
// f 5 7 1
// f 6 7 7
// f 6 4 0
// f 7 6 0
// f 7 5 0

int main()
{
    using namespace boost;

    typedef adjacency_list_traits< vecS, vecS, directedS > Traits;
    typedef adjacency_list< vecS, vecS, directedS,
        property< vertex_name_t, std::string,
            property< vertex_index_t, long,
                property< vertex_color_t, boost::default_color_type,
                    property< vertex_distance_t, long,
                        property< vertex_predecessor_t,
                            Traits::edge_descriptor > > > > >,

        property< edge_capacity_t, long,
            property< edge_residual_capacity_t, long,
                property< edge_reverse_t, Traits::edge_descriptor > > > >
        Graph;

    Graph g;
    property_map< Graph, edge_capacity_t >::type capacity
        = get(edge_capacity, g);
    property_map< Graph, edge_residual_capacity_t >::type residual_capacity
        = get(edge_residual_capacity, g);
    property_map< Graph, edge_reverse_t >::type rev = get(edge_reverse, g);
    Traits::vertex_descriptor s, t;
    read_dimacs_max_flow(g, capacity, rev, s, t);

    std::vector< default_color_type > color(num_vertices(g));
    std::vector< long > distance(num_vertices(g));
    long flow = boykov_kolmogorov_max_flow(g, s, t);

    std::cout << "c  The total flow:" << std::endl;
    std::cout << "s " << flow << std::endl << std::endl;

    std::cout << "c flow values:" << std::endl;
    graph_traits< Graph >::vertex_iterator u_iter, u_end;
    graph_traits< Graph >::out_edge_iterator ei, e_end;
    for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
        for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
            if (capacity[*ei] > 0)
                std::cout << "f " << *u_iter << " " << target(*ei, g) << " "
                          << (capacity[*ei] - residual_capacity[*ei])
                          << std::endl;

    return EXIT_SUCCESS;
}


#include "scenario.h"
#include <iostream>
#include <math.h>
#include <boost/math/distributions/normal.hpp>

double IndustryScenario::calcIndustryDraw(NormalRandomNumberGenerator& randGen, IndustryEntry& industryEntry)
{
	double random1 = randGen.rand();
	double random2 = randGen.rand();
	return (industryEntry.correl * random1) + (sqrt(1 - (industryEntry.correl*industryEntry.correl)) * random2);
}
IndustryScenario::IndustryScenario(NormalRandomNumberGenerator& randGen, IndustryEntry& industryEntry) :
	industry(industryEntry.industry),
	industryDraw(calcIndustryDraw(randGen, industryEntry))
{}
IndustryScenarioData::IndustryScenarioData(NormalRandomNumberGenerator& randGen, IndustryData& industryData)
{
	for (size_t i = 0, n = industryData.size(); i < n; i++)
	{
		IndustryEntry& entry = industryData[i];
		push_back(IndustryScenario(randGen, entry));
	}
}
IndustryScenario* IndustryScenarioData::getByName(string industry)
{
	for (size_t i = 0, n = size(); i < n; i++)
	{
		IndustryScenario& entry = at(i);
		if (entry.industry == industry)
			return &entry;
	}
	return nullptr;
}
ScenarioEntry::ScenarioEntry(NormalRandomNumberGenerator& randGen, IssuerEntry& issuerEntry, IndustryScenario& industryScenario, TransitionMatrix& transitionMatrix) :
	name(issuerEntry.name),
	percentile(calculatePercentile(randGen, issuerEntry, industryScenario)),
	rating(calculateRating(randGen, issuerEntry, industryScenario, transitionMatrix)) { }
const size_t ScenarioEntry::calculateRating(NormalRandomNumberGenerator& randGen, IssuerEntry& issuerEntry, IndustryScenario& industryScenario, TransitionMatrix& transitionMatrix)
{
	size_t rating = convertRating(issuerEntry.rating);
	//Assumes rating >= 0 && <= 7
	MatrixRow& row = transitionMatrix.cumSumMatrix[rating];
	size_t i = row.size() - 1;
	for(size_t j = 0, n = row.size(); j<n; j++)
	{
		if(percentile <= row[i])
			return i;
		i--;
	}
	cout << "Failed to find the rating. Using original rating: " << to_string(rating) << endl;
	throw new runtime_error("Bad rating");
}
const double ScenarioEntry::calculatePercentile(NormalRandomNumberGenerator& randGen, IssuerEntry& issuerEntry, IndustryScenario& industryScenario)
{
	double random3 = randGen.rand();
	double assetReturn =  (issuerEntry.correl * industryScenario.industryDraw) + (sqrt(1 - (issuerEntry.correl*issuerEntry.correl)) * random3);
	boost::math::normal norm;
	double percentile = boost::math::cdf(norm, assetReturn);
	return percentile;
}
const size_t ScenarioEntry::convertRating(string rating)
{
	size_t convertedRating;
	if (rating == "AAA")
		convertedRating = 0;
	else if (rating == "AA")
		convertedRating = 1;
	else if (rating == "A")
		convertedRating = 2;
	else if (rating == "BBB")
		convertedRating = 3;
	else if (rating == "BB")
		convertedRating = 4;
	else if (rating == "B")
		convertedRating = 5;
	else if (rating == "CCC")
		convertedRating = 6;
	else if (rating == "C")
		convertedRating = 6;
	else if (rating == "D")
		convertedRating = 7;
	return convertedRating;
}
Scenario::Scenario(NormalRandomNumberGenerator& randGen, IssuerData& issuerData, IndustryData& industryData, TransitionMatrix& transitionMatrix)
{
	IndustryScenarioData industryScenarioData(randGen, industryData);
	for (size_t i = 0, n = issuerData.size(); i < n; i++)
	{
		IssuerEntry& issuerEntry = issuerData.at(i);
		IndustryScenario* industryScenario = industryScenarioData.getByName(issuerEntry.industry);
		if (!industryScenario)
			throw runtime_error("No known industry entry for \"" + issuerEntry.industry + "\"");
		push_back(ScenarioEntry(randGen, issuerEntry, *industryScenario, transitionMatrix));
	}
}
ScenarioEntry* Scenario::getByName(string name)
{
	for (size_t i = 0, n = size(); i < n; i++)
	{
		ScenarioEntry& entry = at(i);
		if (entry.name == name)
			return &entry;
	}
	return nullptr;
}


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

#ifndef MATRIX_HPP_
#define MATRIX_HPP_

#include <boost/numeric/ublas/matrix.hpp>

#include "matrix_transposed.hpp"
#include <iomanip>

namespace unimod
{

  /**
   * Exception to indicate a pivot on a zero element.
   */

  class matrix_binary_pivot_exception: public std::exception
  {
  public:
    const char* what() const throw ()
    {
      return "Cannot pivot on a zero entry!";
    }
  };

  /**
   * Free function to set a matrix value of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param row Row index
   * @param column Column index
   * @param value New value
   */

  template <typename MatrixType>
  inline void matrix_set_value(MatrixType& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {
    matrix(row, column) = value;
  }

  /**
   * Dummy function for the version with writable orignal matrix.
   */

  template <typename MatrixType>
  inline void matrix_set_value(const MatrixType& matrix, size_t row, size_t column, typename MatrixType::value_type value)
  {
    assert (false);
  }

  /**
   * Free function to permute two rows of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute1(MatrixType& matrix, size_t index1, size_t index2)
  {
    for (size_t index = 0; index < matrix.size2(); ++index)
    {
      std::swap(matrix(index1, index), matrix(index2, index));
    }
  }

  /**
   * Free function to permute two columns of a permuted matrix.
   *
   * @param matrix The permuted matrix
   * @param index1 First index
   * @param index2 Second index
   */

  template <typename MatrixType>
  inline void matrix_permute2(MatrixType& matrix, size_t index1, size_t index2)
  {
    for (size_t index = 0; index < matrix.size1(); ++index)
    {
      std::swap(matrix(index, index1), matrix(index, index2));
    }
  }

  /**
   * Free function to perform a binary pivot on a matrix.
   *
   * @param matrix The matrix
   * @param i Row index
   * @param j Column index
   */

  template <typename MatrixType>
  void matrix_binary_pivot(MatrixType& matrix, size_t i, size_t j)
  {
    typedef typename MatrixType::value_type value_type;
    const value_type& base_value = matrix(i, j);

    if (base_value == 0)
    {
      throw matrix_binary_pivot_exception();
    }

    for (size_t row = 0; row < matrix.size1(); ++row)
    {
      if (row == i)
      {
        continue;
      }
      const value_type& first = matrix(row, j);
      if (first == 0)
      {
        continue;
      }

      for (size_t column = 0; column < matrix.size2(); ++column)
      {
        if (column == j)
        {
          continue;
        }
        const value_type& second = matrix(i, column);
        if (second == 0)
        {
          continue;
        }
        matrix(row, column) = 1 - matrix(row, column);
      }
    }
  }

  /**
   * Free function to perform a ternary pivot on a matrix.
   *
   * @param matrix The matrix
   * @param i Row index
   * @param j Column index
   */

  template <typename MatrixType>
  void matrix_ternary_pivot(MatrixType& matrix, size_t i, size_t j)
  {
    typedef typename MatrixType::value_type value_type;
    const value_type& base_value = matrix(i, j);

    if (base_value == 0)
    {
      throw matrix_binary_pivot_exception();
    }

    for (size_t row = 0; row < matrix.size1(); ++row)
    {
      if (row == i)
      {
        continue;
      }
      const value_type& first = matrix(row, j);
      if (first == 0)
      {
        continue;
      }

      for (size_t column = 0; column < matrix.size2(); ++column)
      {
        if (column == j)
        {
          continue;
        }
        const value_type& second = matrix(i, column);
        if (second == 0)
        {
          continue;
        }
        value_type value = matrix(row, column) - first * second / base_value;
        while (value > 1)
          value -= 3;
        while (value < -1)
          value += 3;
        matrix(row, column) = value;
      }
    }
  }

  /**
   * Counts how many of the rows in the specified range have a property.
   *
   * @param matrix The given matrix
   * @param row_first First row in range
   * @param row_beyond Beyond row in range
   * @param column_first First column for property check
   * @param column_beyond Beyond column for property check
   * @param check Property checking routine
   * @return Number of rows with the property
   */

  template <typename MatrixType, typename PropertyCheck>
  inline size_t matrix_count_property_row_series(const MatrixType& matrix, size_t row_first, size_t row_beyond, size_t column_first,
      size_t column_beyond, PropertyCheck check)
  {
    for (size_t row = row_first; row < row_beyond; ++row)
    {
      for (size_t column = column_first; column < column_beyond; ++column)
        check(matrix(row, column));
      if (!check())
        return row - row_first;
    }
    return row_beyond - row_first;
  }

  /**
   * Counts how many of the columns in the specified range have a property.
   *
   * @param matrix The given matrix
   * @param row_first First column for property check
   * @param row_beyond Beyond column for property check
   * @param column_first First row in range
   * @param column_beyond Beyond row in range
   * @param check Property checking routine
   * @return Number of rows with the property
   */

  template <typename MatrixType, typename PropertyCheck>
  inline size_t matrix_count_property_column_series(const MatrixType& matrix, size_t row_first, size_t row_beyond, size_t column_first,
      size_t column_beyond, PropertyCheck check)
  {
    matrix_transposed <const MatrixType> transposed(matrix);
    return matrix_count_property_row_series(transposed, column_first, column_beyond, row_first, row_beyond, check);
  }

  /**
   * Prints a matrix
   *
   * @param matrix The matrix to be printed
   */

  template <typename MatrixType>
  inline void matrix_print(const MatrixType& matrix)
  {
    for (size_t row = 0; row < matrix.size1(); ++row)
    {
      for (size_t column = 0; column < matrix.size2(); ++column)
      {
        std::cout << " " << std::setw(2) << matrix(row, column);
      }
      std::cout << "\n";
    }
    std::cout << std::flush;
  }

  /**
   * Tests two matrices for exact equality.
   *
   * @param matrix1 First matrix
   * @param matrix2 Second matrix
   * @return true if and only if all entries match
   */

  template <typename MatrixType1, typename MatrixType2>
  inline bool matrix_equals(const MatrixType1& matrix1, const MatrixType2& matrix2)
  {
    if (matrix1.size1() != matrix2.size1())
      return false;
    if (matrix1.size2() != matrix2.size2())
      return false;

    for (size_t row = 0; row < matrix1.size1(); ++row)
    {
      for (size_t column = 0; column < matrix1.size2(); ++column)
      {
        if (matrix1(row, column) != matrix2(row, column))
          return false;
      }
    }
    return true;
  }

  template <typename Matrix>
  bool matrix_row_zero(const Matrix& matrix, size_t row, size_t column_first, size_t column_beyond)
  {
    for (size_t c = column_first; c != column_beyond; ++c)
      if (matrix(row, c) != 0)
        return false;
    return true;
  }

  template <typename Matrix>
  bool matrix_column_zero(const Matrix& matrix, size_t column, size_t row_first, size_t row_beyond)
  {
    return matrix_row_zero(make_transposed_matrix(matrix), column, row_first, row_beyond);
  }

  template <typename Matrix>
  bool find_smallest_nonzero_matrix_entry(const Matrix& matrix, size_t row_first, size_t row_beyond, size_t column_first, size_t column_beyond,
      size_t& row, size_t& column)
  {
    bool result = false;
    int current_value = 0;
    for (size_t r = row_first; r != row_beyond; ++r)
    {
      for (size_t c = column_first; c != column_beyond; ++c)
      {
        int value = matrix(r, c);
        if (value == 0)
          continue;

        value = value >= 0 ? value : -value;

        if (!result || value < current_value)
        {
          result = true;
          row = r;
          column = c;
          current_value = value;
        }
      }
    }
    return result;
  }

  template <typename Matrix1, typename Matrix2>
  bool equals(const Matrix1& first, const Matrix2& second)
  {
    if (first.size1() != second.size1())
      return false;
    if (first.size2() != second.size2())
      return false;
    for (size_t r = 0; r < first.size1(); ++r)
    {
      for (size_t c = 0; c < first.size2(); ++c)
      {
        if (first(r, c) != second(r, c))
          return false;
      }
    }
    return true;
  }
}

#endif /* MATRIX_HPP_ */


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

using namespace std;
using namespace Eigen;

void LinearElasticEnergy::precompute(const TriMesh& mesh)
{
    A_.resize(mesh.f.rows());
    DmInverse_.resize(mesh.f.rows());

    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);

        DmInverse_[idx] = Dm.inverse();

        A_[idx] = 0.5 * (Vec3d() << Dm.col(0), 0.0).finished().cross(
                        (Vec3d() << Dm.col(1), 0.0).finished()).norm();
    }

    lambda_ = VecXd::Zero(3*mesh.f.rows());
}

void LinearElasticEnergy::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++) {
    }
}

void LinearElasticEnergy::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));
    }
}

void LinearElasticEnergy::perVertexCount(const TriMesh& mesh, std::vector<int>& counts) const {
    for (int idx=0; idx<mesh.f.rows(); idx++) {
        for (int j=0; j<3; j++) {
            counts[mesh.f(idx,j)]++;
        }
    }
}

void LinearElasticEnergy::update(const TriMesh& mesh, const VecXd& x, double dt, VecXd& dx) {
    const double a = (1.0 / ksx_) / (dt * dt);

    for (int idx=0; idx<mesh.f.rows(); idx++) {
        const 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]);// +
                            //dx.segment<3>(3*idxs[i+1]) - dx.segment<3>(3*idxs[0]); // Jacobi
        }

        // Deformation gradient
        Mat3x2d F = Dw * DmInverse_[idx];

        const double wu_mag = F.col(0).norm();
        const double wv_mag = F.col(1).norm();

        Vec3d wu = F.col(0) / wu_mag;
        Vec3d wv = F.col(1) / wv_mag;

        // C(x) = A ( || wu || - bu  || wv || - bv    wu . wv )
        Vec3d C = A_[idx] * Vec3d(wu_mag - 1.0, wv_mag - 1.0, F.col(0).dot(F.col(1)));

        // grad C is 3, 3x3 matrices, where each is w.r.t a different vertex
        // Each column is the derivative of the respective C
        Mat3d gradC[3];

        gradC[1].col(0) = A_[idx] * DmInverse_[idx](0,0) * wu;
        gradC[1].col(1) = A_[idx] * DmInverse_[idx](0,1) * wv;
        gradC[1].col(2) = A_[idx] * (DmInverse_[idx](0,0) * F.col(1) + DmInverse_[idx](0,1) * F.col(0));
        gradC[2].col(0) = A_[idx] * DmInverse_[idx](1,0) * wu;
        gradC[2].col(1) = A_[idx] * DmInverse_[idx](1,1) * wv;
        gradC[2].col(2) = A_[idx] * (DmInverse_[idx](1,0) * F.col(1) + DmInverse_[idx](1,1) * F.col(0));
        gradC[0] = -(gradC[1] + gradC[2]);

	double den1 = (gradC[0].col(0).squaredNorm() / mesh.m[3*idxs[0]] +
	               gradC[1].col(0).squaredNorm() / mesh.m[3*idxs[1]] +
	               gradC[2].col(0).squaredNorm() / mesh.m[3*idxs[2]] + a);
	double den2 = (gradC[0].col(1).squaredNorm() / mesh.m[3*idxs[0]] +
	               gradC[1].col(1).squaredNorm() / mesh.m[3*idxs[1]] +
	               gradC[2].col(1).squaredNorm() / mesh.m[3*idxs[2]] + a);
	double den3 = (gradC[0].col(2).squaredNorm() / mesh.m[3*idxs[0]] +
	               gradC[1].col(2).squaredNorm() / mesh.m[3*idxs[1]] +
	               gradC[2].col(2).squaredNorm() / mesh.m[3*idxs[2]] + a);

	Vec3d dl = (-C - lambda_.segment<3>(3*idx) * a).array() / Eigen::Array3d(den1, den2, den3);

	dx.segment<3>(3*idxs[0]) += gradC[0] * dl / mesh.m[3*idxs[0]];
	dx.segment<3>(3*idxs[1]) += gradC[1] * dl / mesh.m[3*idxs[1]];
	dx.segment<3>(3*idxs[2]) += gradC[2] * dl / mesh.m[3*idxs[2]];

	lambda_.segment<3>(3*idx) += dl;
    }
}