#include "per_face_prin_curvature.h"
#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_vertex_normals.h>
#include <igl/unique_edge_map.h>
#include <igl/edge_flaps.h>
#include <igl/barycenter.h>
#include <igl/parallel_for.h>
#include <igl/pinv.h>
#include <Eigen/SparseCore>
#include <iostream>
using namespace std;

void per_vertex_signed_prin_curvature(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::MatrixXd & PD1, Eigen::MatrixXd & PD2, Eigen::VectorXd & PC1, Eigen::VectorXd & PC2){

    int n = V.rows();
    int m = F.rows();
    Eigen::MatrixXi E,uE,EF,EI;
    std::vector<std::vector<int>> A,uE2E;
    Eigen::VectorXi EMAP;
  Eigen::MatrixXd N;
    std::vector<int> b;
    Eigen::MatrixXd bary;
    igl::barycenter(V,F,bary);
    igl::adjacency_list(F,A);
   igl::per_vertex_normals(V,F,N);

   igl::unique_edge_map(F,E,uE,EMAP,uE2E);
   igl::edge_flaps(F,uE,EMAP,EF,EI);

    PC1.resize(n);
    PC2.resize(n);
    PD1.resize(n,3);
    PD2.resize(n,3);

    //std::cout << "Starting iteration" << std::endl;

    //for (int i=0; i<m; i++) { // For each face
    igl::parallel_for(V.rows(),[&] (const int i){

        // Step 1: Find unique combined indeces of all vertices of face
        // uu = V(unique(u{F(i,1)},u{F(i,2)},u{F(i,3)}),:)
        // vv = uu-bary(i,:) what is this notation in c++...
        // Triangle i has edges E.row(i), E.row(i+m), E.row(i+2m)
        // So incident (repeated) faces are EF(i,0), EF(i,1), EF(i+m,0),
        // EF(i+m,1), EF(i+2m,0) and EF(i+2m,1);
        // std::cout << "entered loop" << std::endl;
        std::vector<int> vertex_indeces,v1,v2,v3;
        std::vector<std::vector<int>> v123;
        // v123.resize(3);
        //for(int j = 0; j < 3; j++){
        //    v123[j].resize(3);
            // std::cout << "entered loop" << std::endl;
            //std::cout << EF(EMAP(i+(j*m)),0) << std::endl;
           // std::cout << EF(EMAP(i+(j*m)),1) << std::endl;

         //   for(int orient = 0; orient < 2; orient++){
         //       if(EF(EMAP(i+(j*m)),orient) != i){
                 //   std::cout << "found one" << std::endl;
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),0));
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),1));
         //       v123[j].push_back(F(EF(EMAP(i+(j*m)),orient),2));
         //       }
         //       }
          //  }
        // std::cout << "exited loop" << std::endl;
        //v1 = v123[0];
        //v2 = v123[1];
        //v3 = v123[2];
        //assert(v1.size() == 3);
        //assert(v2.size() == 3);
        //assert(v3.size() == 3);
       //  std::cout << "survived assertions" << std::endl;

        Eigen::MatrixXd P;
        Eigen::Vector3d w,uu,vv;
        Eigen::VectorXd u,v,b,a;

        double E,FF,G,e,f,g,det;
        v1 = A[i];
        vertex_indeces.insert(vertex_indeces.end(),v1.begin(),v1.end());
        for (int j = 0; j < v1.size(); j++) {
            v2 = A[v1[j]];
            vertex_indeces.insert(vertex_indeces.end(),v2.begin(),v2.end());
        }
        sort( vertex_indeces.begin(), vertex_indeces.end() );
        vertex_indeces.erase( unique( vertex_indeces.begin(), vertex_indeces.end() ), vertex_indeces.end() );
        int k = vertex_indeces.size();
        for (int j=0; j<k; j++) {
            P.conservativeResize(j+1,3);
            P.row(j) = V.row(vertex_indeces[j]) - bary.row(i);
        }
        // END OF STEP 1

        // Step 2: Build orthonormal basis from N
        w = N.row(i);

        if (w(2)==-1) {
            vv(0) = 0;
            vv(1) = -1;
            vv(2) = 0;
            uu(0) = -1;
            uu(1) = 0;
            uu(2) = 0;
        }
        else{
            vv(0) = 1-(pow(w(0),2)/(1+w(2)));
            vv(1) = -w(0)*w(1)/(1+w(2));
            vv(2) = -w(0);
            uu(0) = -w(0)*w(1)/(1+w(2));
            uu(1) = 1-(pow(w(1),2)/(1+w(2)));
            uu(2) = -w(1);
        }
        // END OF STEP 2
        // STEP 3: PROJECT
        Eigen::MatrixXd S(3,2);
        S.col(0) = uu;
        S.col(1) = vv;
        u = P*uu;
        v = P*vv;
        b = P*w;
        // END OF STEP 3
        // STEP 4: FIT QUADRIC
        Eigen::MatrixXd A2(k,6);
        Eigen::MatrixXd A2_pseudo(6,k);
        A2.col(0) = u;
        A2.col(1) = v;
        A2.col(2) = u.cwiseProduct(u);
        A2.col(3) = u.cwiseProduct(v);
        A2.col(4) = v.cwiseProduct(v);
        A2.col(5).setOnes();
        igl::pinv(A2,A2_pseudo);
        a = A2_pseudo*b;
        // END OF STEP 4
        // STEP 5: BUILD SS
        E = 1+(pow(a(0),2));
        FF = a(0)*a(1);
        G = 1+(pow(a(1),2));
        e = (2*a(2))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        f = (a(3))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        g = (2*a(4))/sqrt((pow(a(0),2))+1+(pow(a(1),2)));
        Eigen::Matrix2d S1,S2,SS;
        S1 << e,f,f,g;
        det = (G*E)-(pow(FF,2));
        if (det==0) {
            Eigen::Matrix2d S2_pseudo;
            S2 << E,FF,FF,G;
            igl::pinv(S2,S2_pseudo);
            SS = S1*S2_pseudo;
            //std::cout << "Det zero" << std::endl;
        }else{
            S2 << G,-FF,-FF,E;
            S2 /= det;
            SS = S1*S2;
        }
        // END OF STEP 5
        // STEP 6: EIGENALALYSIS OF SS
        // std::cout << "Before eigensolver" << std::endl;
        Eigen::EigenSolver<Eigen::MatrixXd> es(SS);
        PC1(i) = - es.eigenvalues().real().coeff(0);
        PC2(i) = - es.eigenvalues().real().coeff(1);
        if (PC1(i)>PC2(i)) {
            std::swap(PC1(i),PC2(i));
            PD1.row(i) = es.eigenvectors().real().coeff(0,1)*uu + es.eigenvectors().real().coeff(1,1)*vv;
            PD2.row(i) = es.eigenvectors().real().coeff(0,0)*uu + es.eigenvectors().real().coeff(1,0)*vv;
        }else{
            PD1.row(i) = es.eigenvectors().real().coeff(0,0)*uu + es.eigenvectors().real().coeff(1,0)*vv;
            PD2.row(i) = es.eigenvectors().real().coeff(0,1)*uu + es.eigenvectors().real().coeff(1,1)*vv;
        }
       // PD1.row(i) = uu;
       // PD2.row(i) = w;
        //std::cout << "Ending iteration" << std::endl;
    },0);
        //
    //}

}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp principal_curvatures_silvia.cpp -o main


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#pragma once

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

#include "energy.hpp"

class ModifiedConjugateGradient {
public:
    bool compute(const SparseMatrixd& A) {
        if (A.rows() != A.cols()) {
            std::cerr << "Non-square matrix A!" << std::endl;
            return false;
        }

        if (P_.size() != A.rows() / 3) {
            P_.resize(A.rows() / 3);
        }

        for (int i=0; i<A.rows(); i+=3) {
            Mat3d Ai = A.block(i,i,3,3);
            P_[i/3] = Ai.inverse();
        }

        return true;
    }

    void resize(int nbr) {
        S_.resize(nbr, Eigen::Matrix3d::Identity());
    }

    void reset() {
        for (Eigen::Matrix3d& M : S_) {
            M.setIdentity();
        }
    }

    void setFilter(int idx, const Eigen::Matrix3d& C) {
        S_[idx] = C;
    }

    void filterInPlace(Eigen::VectorXd& v) {
        #pragma omp parallel for
        for (size_t i=0; i<S_.size(); i++) {
            v.segment<3>(3*i) = S_[i] * v.segment<3>(3*i);
        }
    }

    Eigen::VectorXd filter(const Eigen::VectorXd& v) {
        Eigen::VectorXd out(v.size());
        #pragma omp parallel for
        for (size_t i=0; i<S_.size(); i++) {
            out.segment<3>(3*i) = S_[i] * v.segment<3>(3*i);
        }
        return out;
    }

    template <typename F, typename Derived>
    void solve(F multiply, const Eigen::MatrixBase<Derived>& b, Eigen::VectorXd& x) {
        static double eps = 2.5e-3;
        static const size_t maxNbrItrs = 1000;

        VecXd r = b - multiply(x); filterInPlace(r);
        VecXd c(b.size());

        for (int i=0; i<b.size()/3; i++) {
            c.segment<3>(3*i) = S_[i] * P_[i] * r.segment<3>(3*i);
        }

        double dNew = r.dot(c);
        double d0 = dNew;

        double tol = eps * eps * d0;

        size_t nbrItrs = 0;
        size_t minNbr = 10;
        while ((dNew > tol || nbrItrs < minNbr) && nbrItrs < maxNbrItrs)
        {
            VecXd q = multiply(c); filterInPlace(q);
            double a = dNew / c.dot(q);

            x.noalias() += a * c;
            r.noalias() -= a * q;

            VecXd s(r.size());
            for (int i=0; i<b.size()/3; i++) {
                s.segment<3>(3*i) = P_[i] * r.segment<3>(3*i);
            }

            double dOld = dNew;
            dNew = r.dot(s);

            c *= dNew / dOld;
            c.noalias() += s;
	    filterInPlace(c);

            nbrItrs++;
        }

        std::cout << "CG finished in " << nbrItrs
                  << " iterations with error " << dNew << " (" << tol << ")" << std::endl;
    }

    const Eigen::Matrix3d& S(int idx) {
        return S_[idx];
    }

protected:
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> P_;
    std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> S_;
};


/*
This file is a part of Raman-Scattering-Code-Conversion.
<https://github.com/Kirbologist/Raman-Scattering-Code-Conversion>

Written by Siwan Li for the UQ School of Maths and Physics.
Based on the SMARTIES MATLAB package by W.R.C. Somerville, B. Auguié, E.C. Le Ru
Copyright (C) 2021-2022 Siwan Li

This source code form is subject to the terms of the MIT License.
If a copy of the MIT License was not distributed with this file,
you can obtain one at <https://opensource.org/licenses/MIT>.

This code contains all 'rvh' SMARTIES functions that are used in Raman scattering calculations,
i.e. T-matrix related functions specific to particles with mirror-reflection symmetry.
Henceforth, such symmetry shall be called rvh symmetry.
*/

#ifndef RVH_HPP
#define RVH_HPP

#include "core.hpp"
#include "sph.hpp"
#include "vsh.hpp"
#include "misc.hpp"
#include <Eigen/SVD>

using namespace Eigen;
using namespace std;

namespace Smarties {

  /* Struct containing vectors of expansion coefficients of the multipole expansions of a set of electric fields */
  template <class Real>
  struct stAbcdnm {
    // Scattering field coefficients
    ArrayXc<Real> p_nm;
    ArrayXc<Real> q_nm;
    // Incident field coefficients
    ArrayXc<Real> a_nm;
    ArrayXc<Real> b_nm;
    // Internal field coefficients
    ArrayXc<Real> c_nm;
    ArrayXc<Real> d_nm;
  };

  /* Struct containing orientation-averaged cross-sections for various wavelengths*/
  template <class Real>
  struct stCrossSection {
    ArrayXr<Real> C_ext; // Wavelength-dependent extinction coefficients
    ArrayXr<Real> C_sca; // Wavelength-dependent scattering coefficients
    ArrayXr<Real> C_abs; // Wavelength-dependent absorption coefficients
  };

  /*
  Calculate T (and possibly R) matrices from P, Q matrices, for scatterers with a plane of symmetry.
  This makes use of block inversion. See sec. 4.5 and eq. 70 in JQSRT2013 for details.
  Inputs:
    st_PQ_list - std::vector of size M, each element of which contains a unique pointer to an stPQ struct
                 (one entry for each m in abs_m_vec), and which makes use of the rvh symmetry
    get_R - if true, R is computed and returned
  Output:
    A std::vector of size M of unique pointers to stTR structs.
  Dependencies:
    InvertLUcol
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhGetTRfromPQ(vector<unique_ptr<stPQ<Real>>>& st_PQ_list, bool get_R = false) {
    // Note that all matrix inversions uses LU decomposition with partial pivoting of the columns
    int num_entries = st_PQ_list.size();
    vector<unique_ptr<stTR<Real>>> output(num_entries);
    for (int i = 0; i < num_entries; i++) {
      output[i] = make_unique<stTR<Real>>();
      // Get blocks of P and Q matrices
      unique_ptr<stPQ<Real>>& st_PQ = st_PQ_list[i];
      unique_ptr<stTR<Real>>& st_TR = output[i];
      // Get blocks of P_eo and Q_eo matrices
      MatrixXc<Real> P11_ee = st_PQ->st_4M_P_eo().M11;
      MatrixXc<Real> P12_eo = st_PQ->st_4M_P_eo().M12;
      MatrixXc<Real> P21_oe = st_PQ->st_4M_P_eo().M21;
      MatrixXc<Real> P22_oo = st_PQ->st_4M_P_eo().M22;
      MatrixXc<Real> Q11_ee = st_PQ->st_4M_Q_eo().M11;
      MatrixXc<Real> Q12_eo = st_PQ->st_4M_Q_eo().M12;
      MatrixXc<Real> Q21_oe = st_PQ->st_4M_Q_eo().M21;
      MatrixXc<Real> Q22_oo = st_PQ->st_4M_Q_eo().M22;

      // Get blocks of P_oe and Q_oe matrices
      MatrixXc<Real> P11_oo = st_PQ->st_4M_P_oe().M11;
      MatrixXc<Real> P12_oe = st_PQ->st_4M_P_oe().M12;
      MatrixXc<Real> P21_eo = st_PQ->st_4M_P_oe().M21;
      MatrixXc<Real> P22_ee = st_PQ->st_4M_P_oe().M22;
      MatrixXc<Real> Q11_oo = st_PQ->st_4M_Q_oe().M11;
      MatrixXc<Real> Q12_oe = st_PQ->st_4M_Q_oe().M12;
      MatrixXc<Real> Q21_eo = st_PQ->st_4M_Q_oe().M21;
      MatrixXc<Real> Q22_ee = st_PQ->st_4M_Q_oe().M22;

      int num_even = Q11_ee.rows();
      int num_odd = Q11_oo.rows();
      ArrayXi ind1_eo = st_PQ->st_4M_Q_eo().ind1;
      ArrayXi ind2_eo = st_PQ->st_4M_Q_eo().ind2;
      ArrayXi ind1_oe = st_PQ->st_4M_Q_oe().ind1;
      ArrayXi ind2_oe = st_PQ->st_4M_Q_oe().ind2;

      int m = st_PQ->st_4M_P_eo().m;

      // m == 0 case is done separately, since M12 and M21 are empty.
      if (!m) {
        st_TR->st_4M_R_oe().M11 = InvertLUcol(Q11_oo);
        st_TR->st_4M_T_oe().M11 = -P11_oo * st_TR->st_4M_R_oe().M11.matrix();
        st_TR->st_4M_R_oe().M22 = InvertLUcol(Q22_ee);
        st_TR->st_4M_T_oe().M22 = -P22_ee * st_TR->st_4M_R_oe().M22.matrix();
        st_TR->st_4M_R_eo().M11 = InvertLUcol(Q11_ee);
        st_TR->st_4M_T_eo().M11 = -P11_ee * st_TR->st_4M_R_eo().M11.matrix();
        st_TR->st_4M_R_eo().M22 = InvertLUcol(Q22_oo);
        st_TR->st_4M_T_eo().M22 = -P22_oo * st_TR->st_4M_R_eo().M22.matrix();

        st_TR->st_4M_T_eo().M12 = ArrayXXc<Real>::Zero(num_even, num_odd);
        st_TR->st_4M_T_eo().M21 = ArrayXXc<Real>::Zero(num_odd, num_even);
        st_TR->st_4M_T_oe().M12 = ArrayXXc<Real>::Zero(num_odd, num_even);
        st_TR->st_4M_T_oe().M21 = ArrayXXc<Real>::Zero(num_even, num_odd);

        if (get_R) {
          st_TR->st_4M_R_eo().M12 = ArrayXXc<Real>::Zero(num_even, num_odd);
          st_TR->st_4M_R_eo().M21 = ArrayXXc<Real>::Zero(num_odd, num_even);
          st_TR->st_4M_R_oe().M12 = ArrayXXc<Real>::Zero(num_odd, num_even);
          st_TR->st_4M_R_oe().M21 = ArrayXXc<Real>::Zero(num_even, num_odd);
        }
      } else { // Do inversion using JQSRT2013 eq. 70

        // For M_eo matrices
        MatrixXc<Real> Q11_inv = InvertLUcol(Q11_ee);

        MatrixXc<Real> G1 = P11_ee * Q11_inv;
        MatrixXc<Real> G3 = P21_oe * Q11_inv;
        MatrixXc<Real> G5 = Q21_oe * Q11_inv;
        MatrixXc<Real> F2_m1 = Q22_oo - G5 * Q12_eo;
        MatrixXc<Real> F2 = InvertLUcol(F2_m1);

        MatrixXc<Real> G2 = P22_oo * F2;
        MatrixXc<Real> G4 = P12_eo * F2;
        MatrixXc<Real> G6 = Q12_eo * F2;

        st_TR->st_4M_T_eo().M12 = G1 * G6 - G4;
        st_TR->st_4M_T_eo().M22 = G3 * G6 - G2;
        st_TR->st_4M_T_eo().M11 = -G1 - st_TR->st_4M_T_eo().M12.matrix() * G5;
        st_TR->st_4M_T_eo().M21 = -G3 - st_TR->st_4M_T_eo().M22.matrix() * G5;

        if (get_R) {
          st_TR->st_4M_R_eo().M12 = -Q11_inv * G6;
          st_TR->st_4M_R_eo().M22 = F2;
          st_TR->st_4M_R_eo().M11 = Q11_inv - st_TR->st_4M_R_eo().M12.matrix() * G5;
          st_TR->st_4M_R_eo().M21 = -st_TR->st_4M_R_eo().M22.matrix() * G5;
        }

        // For M_oe matrices
        Q11_inv = InvertLUcol(Q11_oo);

        G1 = P11_oo * Q11_inv;
        G3 = P21_eo * Q11_inv;
        G5 = Q21_eo * Q11_inv;
        F2_m1 = Q22_ee - G5 * Q12_oe;
        F2 = InvertLUcol(F2_m1);

        G2 = P22_ee * F2;
        G4 = P12_oe * F2;
        G6 = Q12_oe * F2;

        st_TR->st_4M_T_oe().M12 = G1 * G6 - G4;
        st_TR->st_4M_T_oe().M22 = G3 * G6 - G2;
        st_TR->st_4M_T_oe().M11 = -G1 - st_TR->st_4M_T_oe().M12.matrix() * G5;
        st_TR->st_4M_T_oe().M21 = -G3 - st_TR->st_4M_T_oe().M22.matrix() * G5;

        if (get_R) {
          st_TR->st_4M_R_oe().M12 = -Q11_inv * G6;
          st_TR->st_4M_R_oe().M22 = F2;
          st_TR->st_4M_R_oe().M11 = Q11_inv - st_TR->st_4M_R_oe().M12.matrix() * G5;
          st_TR->st_4M_R_oe().M21 = -st_TR->st_4M_R_oe().M22.matrix() * G5;
        }
      }

      st_TR->st_4M_T_eo().m = st_TR->st_4M_T_oe().m = m;
      st_TR->st_4M_T_eo().ind1 = ind1_eo;
      st_TR->st_4M_T_eo().ind2 = ind2_eo;
      st_TR->st_4M_T_oe().ind1 = ind1_oe;
      st_TR->st_4M_T_oe().ind2 = ind2_oe;
      st_TR->mat_list.push_back("st_4M_T");

      if (get_R) {
        st_TR->st_4M_R_eo().m = st_TR->st_4M_R_oe().m = m;
        st_TR->st_4M_R_eo().ind1 = ind1_eo;
        st_TR->st_4M_R_eo().ind2 = ind2_eo;
        st_TR->st_4M_R_oe().ind1 = ind1_oe;
        st_TR->st_4M_R_oe().ind2 = ind2_oe;
        st_TR->mat_list.push_back("st_4M_R");
      }
    }
    return output;
  }

  /*
  Truncate P and Q matrices in st_mat_list such that the maximum number of multipoles N is now N_max.
  This also removes values of m that are larger than N_max.
  This function works on st_mat_list which makes use of rvh symmetry.
  Inputs:
    st_mat_list - a vector of unique pointers to stPQ structs, with one entry for each m of abs_m_vec.
    N_max - The maximum value of N (and m) that is desired in the output.
  Output:
    returns a deep copy of the same st_mat_list vector, but where all M11, M12, M21 and M22 members
    have been truncated to N = N_max and values of m > N_max are removed.
  Dependencies:
    LogicalIndices
  */
  template <class Real>
  vector<unique_ptr<stPQ<Real>>> rvhTruncateMatrices(const vector<unique_ptr<stPQ<Real>>>& st_mat_list, int N_max) {
    vector<unique_ptr<stPQ<Real>>> output;
    int num_entries = st_mat_list.size();
    for (int i = 0 ; i < num_entries; i++) {
      int num_st_4M = st_mat_list[i]->mat_list.size() * 2; // Count how many matrices in the struct are defined
      int m;
      if (num_st_4M > 0 && (m = st_mat_list[i]->st_4M_list[0].m) <= N_max) {
        auto output_st_TR = make_unique<stPQ<Real>>();
        output_st_TR->mat_list = st_mat_list[i]->mat_list;
        for (int j = 0; j < num_st_4M; j++) {
          int new_size = N_max - max(1, m);
          ArrayXb ind1_valid = st_mat_list[i]->st_4M_list[j].ind1 <= new_size;
          ArrayXb ind2_valid = st_mat_list[i]->st_4M_list[j].ind2 <= new_size;

          ArrayXi new_ind1 = LogicalIndices(ind1_valid);
          ArrayXi new_ind2 = LogicalIndices(ind2_valid);

          output_st_TR->st_4M_list[j].ind1 = st_mat_list[i]->st_4M_list[j].ind1(new_ind1);
          output_st_TR->st_4M_list[j].ind2 = st_mat_list[i]->st_4M_list[j].ind2(new_ind2);
          output_st_TR->st_4M_list[j].m = m;

          ArrayXXc<Real> current_matrix = st_mat_list[i]->st_4M_list[j].M11;
          output_st_TR->st_4M_list[j].M11 = current_matrix(new_ind1, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M12;
          output_st_TR->st_4M_list[j].M12 = current_matrix(new_ind1, new_ind2);
          current_matrix = st_mat_list[i]->st_4M_list[j].M21;
          output_st_TR->st_4M_list[j].M21 = current_matrix(new_ind2, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M22;
          output_st_TR->st_4M_list[j].M22 = current_matrix(new_ind2, new_ind2);
        }
        output.push_back(move(output_st_TR));
      }
    }
    return output;
  }

  /*
  Truncate T and R matrices in st_mat_list such that the maximum number of multipoles N is now N_max.
  This also removes values of m that are larger than N_max.
  This function works on st_mat_list which makes use of rvh symmetry.
  Inputs:
    st_mat_list - a vector of unique pointers to stTR structs, with one entry for each m of abs_m_vec.
    N_max - The maximum value of N (and m) that is desired in the output.
  Output:
    returns a deep copy of the same st_mat_list vector, but where all M11, M12, M21 and M22 members
    have been truncated to N = N_max and values of m > N_max are removed.
  Dependencies:
    LogicalIndices
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhTruncateMatrices(const vector<unique_ptr<stTR<Real>>>& st_mat_list, int N_max) {
    vector<unique_ptr<stTR<Real>>> output;
    int num_entries = st_mat_list.size();
    for (int i = 0 ; i < num_entries; i++) {
      int num_st_4M = st_mat_list[i]->mat_list.size() * 2; // Count how many matrices in the struct are defined
      int m;
      if (num_st_4M > 0 && (m = st_mat_list[i]->st_4M_list[0].m) <= N_max) {
        auto output_st_TR = make_unique<stTR<Real>>();
        output_st_TR->mat_list = st_mat_list[i]->mat_list;
        for (int j = 0; j < num_st_4M; j++) {
          int new_size = N_max - max(1, m);
          ArrayXb ind1_valid = st_mat_list[i]->st_4M_list[j].ind1 <= new_size;
          ArrayXb ind2_valid = st_mat_list[i]->st_4M_list[j].ind2 <= new_size;

          ArrayXi new_ind1 = LogicalIndices(ind1_valid);
          ArrayXi new_ind2 = LogicalIndices(ind2_valid);

          output_st_TR->st_4M_list[j].ind1 = st_mat_list[i]->st_4M_list[j].ind1(new_ind1);
          output_st_TR->st_4M_list[j].ind2 = st_mat_list[i]->st_4M_list[j].ind2(new_ind2);
          output_st_TR->st_4M_list[j].m = m;

          ArrayXXc<Real> current_matrix = st_mat_list[i]->st_4M_list[j].M11;
          output_st_TR->st_4M_list[j].M11 = current_matrix(new_ind1, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M12;
          output_st_TR->st_4M_list[j].M12 = current_matrix(new_ind1, new_ind2);
          current_matrix = st_mat_list[i]->st_4M_list[j].M21;
          output_st_TR->st_4M_list[j].M21 = current_matrix(new_ind2, new_ind1);
          current_matrix = st_mat_list[i]->st_4M_list[j].M22;
          output_st_TR->st_4M_list[j].M22 = current_matrix(new_ind2, new_ind2);
        }
        output.push_back(move(output_st_TR));
      }
    }
    return output;
  }

  /*
  Symmetrises the matrices given in mat_list, and leaves other matrices present in st_mat_list unchanged.
  Uses the upper triangular matrices to deduce the lower triangular parts from the symmetry relations
  of the T-matrix obtained from eqs. 5.34 and 5.37 of [Mishchenko 2002], i.e.
  T11 = T11.transpose(), T22 = T22.transpose(), T12 = -T21.transpose(), T21 = -T12.transpose()
  This function assumes rvh symmetry. While the original MATLAB code could theoretically work with P and Q matrices,
  the implementation here only works with T and R matrices, although it's intended only for T and R matrices anyway.
  Inputs:
    st_mat_list - a std::vector containing unique pointers to stTR structs containing
                  T (and possibly R) matrices to symmetrise
    mat_list - a std::vector of strings specifying which structs are to be symmetrised
  Outputs:
    A deep copy of st_mat_list, but with all the desired matrices symmetrised.
  */
  template <class Real>
  vector<unique_ptr<stTR<Real>>> rvhGetSymmetricMat(const vector<unique_ptr<stTR<Real>>>& st_mat_list,
      vector<string> mat_list = {"st_4M_T"}) {
    enum parity {EO, OE, END};
    int num_entries = st_mat_list.size();
    vector<unique_ptr<stTR<Real>>> output(num_entries);

    for (int i = 0; i < num_entries; i++) {
      output[i] = make_unique<stTR<Real>>(*(st_mat_list[i])); // Make a deep copy at the ith entry of st_mat_list
      int num_st_4M = mat_list.size();
      for (int j = 0; j < num_st_4M; j++) {
        for (int k = EO; k != END; k++) { // Symmetrise eo matrix, then oe matrix
          st4M<Real>* st_4M;
          if (mat_list[j] == "st_4M_T") {
            if (k == EO)
              st_4M = &(output[i]->st_4M_T_eo());
            else if (k == OE)
              st_4M = &(output[i]->st_4M_T_oe());
          } else if (mat_list[j] == "st_4M_R") {
            if (k == EO)
              st_4M = &(output[i]->st_4M_R_eo());
            else if (k == OE)
              st_4M = &(output[i]->st_4M_R_oe());
          } else
            continue;
          ArrayXi ind1 = st_4M->ind1;
          ArrayXi ind2 = st_4M->ind2;

          if (ind1.size() && ind2.size()) { // Check that neither two are empty
            int offset12 = (ind2(0) - ind1(0) + 1)/2;
            // int offset21 = 1 - offset12;

            // Upper triangular without diagonal
            MatrixXc<Real> upper = st_4M->M11.matrix().template triangularView<StrictlyUpper>();
            // Diagonal only
            MatrixXc<Real> diag = static_cast<VectorXc<Real>>(st_4M->M11.matrix().diagonal()).asDiagonal();
            st_4M->M11 = (upper + diag + upper.transpose()).array(); // Symmtrised matrix

            upper = st_4M->M22.matrix().template triangularView<StrictlyUpper>();
            diag = static_cast<VectorXc<Real>>(st_4M->M22.matrix().diagonal()).asDiagonal();
            st_4M->M22 = (upper + diag + upper.transpose()).array();

            // In either case, the transpose of upper2 complements upper1
            MatrixXc<Real> upper1, upper2;
            if (offset12) {
              upper1 = st_4M->M12.matrix().template triangularView<StrictlyUpper>();
              upper2 = st_4M->M21.matrix().template triangularView<Upper>();
            } else {
              upper1 = st_4M->M12.matrix().template triangularView<Upper>();
              upper2 = st_4M->M21.matrix().template triangularView<StrictlyUpper>();
            }
            st_4M->M12 = (upper1 - upper2.transpose()).array(); // Symmetrised matrix
            st_4M->M21 = -st_4M->M12.transpose(); // Symmetrised matrix
          }
        }
      }
    }

    return output;
  }

  /*
  Calculates the field expansion coefficients from the T/R-matrices for a given incident plane wave
  (for one wavelength only). If R is not defined in st_TR_list, then the internal fields are not computed.
  If stIncEabnm is given, the expansion coefficients for the incident wave are not recalculated.
  This method is valid for scatterers with rvh symmetry.

  If st_TR_list contains m values that are not required, then they are ignored.
  Inputs:
    N_max - the maximum multipole order to use in the expansions
            (should be less than or equal to the T-matrix size)
    st_TR_list - std::vector of size M containing unique pointers to stTR structs for each m.
                 This should contain all m present in st_inc_par.abs_m_vec, i.e. all m where |m| <= N,
                 and should make use of the rvh symmetry.
    st_inc_par - A struct containing information about the incident field, as from vshMakeIncidentParams.
    st_inc_E_abmm - A struct containing the expansion coefficients a_nm and b_nm of the incident wave.
                    These are recalculated if not provided.
  Output:
    returns a stAbcdnm struct containing all the calculated expansion coefficients.
    The case where n = 0 and m = 0 is included for padding, so every vector in the struct has size
    P = (N_max + 1)^2. The padding exists to simplify the indexing.
    c_nm and d_nm are only defined if R is defined in st_TR_list.
  Dependencies:
    Seq2Array, LogicalSlice, vshGetIncidentCoefficients
  */
  template <class Real>
  unique_ptr<stAbcdnm<Real>> rvhGetFieldCoefficients(int N_max,
      const vector<unique_ptr<stTR<Real>>>& st_TR_list, const unique_ptr<stIncPar<Real>>& st_inc_par,
      unique_ptr<stIncEabnm<Real>> st_inc_E_abnm = unique_ptr<stIncEabnm<Real>>()) {

    // Coeff of incident wave
    if (!st_inc_E_abnm)
      st_inc_E_abnm = vshGetIncidentCoeffs(N_max, st_inc_par);

    bool get_R = false;
    if (find(st_TR_list[0]->mat_list.begin(), st_TR_list[0]->mat_list.end(), "st_4M_R")
        != st_TR_list[0]->mat_list.end())
      get_R = true;

    // Truncate to relevant m's
    ArrayXb abs_m_vec_valid = st_inc_par->abs_m_vec <= N_max;
    ArrayXi abs_m_vec = LogicalSlice(st_inc_par->abs_m_vec, abs_m_vec_valid);

    int num_M = st_TR_list.size(); // number of m-values in T, not all of which may be needed
    int M = abs_m_vec.size();
    int P_max = (N_max + 1)*(N_max + 1); // number of (n,m) coupled indices (with (0,0) included as padding)

    ArrayXc<Real> a_nm = st_inc_E_abnm->a_nm; // [P X 1]
    ArrayXc<Real> b_nm = st_inc_E_abnm->b_nm; // [P X 1]
    ArrayXc<Real> p_nm = ArrayXc<Real>::Zero(P_max); // [P X 1]
    ArrayXc<Real> q_nm = ArrayXc<Real>::Zero(P_max); // [P X 1]
    ArrayXc<Real> c_nm = ArrayXc<Real>::Zero(get_R ? P_max : 1); // if get_R, [P X 1]
    ArrayXc<Real> d_nm = ArrayXc<Real>::Zero(get_R ? P_max : 1); // if get_R, [P X 1]
    ArrayXi m_ind_for_T(M);

    // Find the corresponding m-indices in st_TR_list (in case they are not in standard order)
    for (int m_ind11 = 0; m_ind11 < M; m_ind11++) {
      for (int m_ind = 0; m_ind < num_M; m_ind++) {
        if (st_TR_list[m_ind]->st_4M_T_eo().m == abs_m_vec(m_ind11)) {
          m_ind_for_T(m_ind11) = m_ind;
          break;
        }
      }
    }

    for (int m_ind11 = 0; m_ind11 < M; m_ind11++) {
      int m_ind = m_ind_for_T(m_ind11);
      int m = abs_m_vec(m_ind11);
      int N_min = max(m, 1);

      ArrayXi n_vec_e = Seq2Array(N_min + (N_min % 2), N_max, 2); // indices for even n [N_e X 1]
      ArrayXi n_vec_o = Seq2Array(N_min + 1 - (N_min % 2), N_max, 2); // indices for odd n [N_o X 1]

      ArrayXi p_vec_e = n_vec_e * (n_vec_e + 1) + m; // [N_e X 1]
      ArrayXi p_vec_o = n_vec_o * (n_vec_o + 1) + m; // [N_o X 1]
      ArrayXi n_vec_T_e = ArrayXi::LinSpaced(n_vec_e.size(), 0, n_vec_e.size() - 1);
      ArrayXi n_vec_T_o = ArrayXi::LinSpaced(n_vec_o.size(), 0, n_vec_o.size() - 1);

      VectorXc<Real> p_nm_e = st_TR_list[m_ind]->st_4M_T_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
          a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
          b_nm(p_vec_o).matrix();
      VectorXc<Real> q_nm_o = st_TR_list[m_ind]->st_4M_T_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
          a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
          b_nm(p_vec_o).matrix();
      VectorXc<Real> p_nm_o = st_TR_list[m_ind]->st_4M_T_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
          a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
          b_nm(p_vec_e).matrix();
      VectorXc<Real> q_nm_e = st_TR_list[m_ind]->st_4M_T_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
          a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
          b_nm(p_vec_e).matrix();

      for (int i = 0; i < n_vec_e.size(); i++) {
        p_nm(p_vec_e(i)) = p_nm_e(i);
        q_nm(p_vec_e(i)) = q_nm_e(i);
      }
      for (int i = 0; i < n_vec_o.size(); i++) {
        p_nm(p_vec_o(i)) = p_nm_o(i);
        q_nm(p_vec_o(i)) = q_nm_o(i);
      }

      if (get_R) {
        VectorXc<Real> c_nm_e = st_TR_list[m_ind]->st_4M_R_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
            a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
            b_nm(p_vec_o).matrix();
        VectorXc<Real> d_nm_o = st_TR_list[m_ind]->st_4M_R_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
            a_nm(p_vec_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
            b_nm(p_vec_o).matrix();
        VectorXc<Real> c_nm_o = st_TR_list[m_ind]->st_4M_R_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
            a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
            b_nm(p_vec_e).matrix();
        VectorXc<Real> d_nm_e = st_TR_list[m_ind]->st_4M_R_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
            a_nm(p_vec_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
            b_nm(p_vec_e).matrix();

        for (int i = 0; i < n_vec_e.size(); i++) {
          c_nm(p_vec_e(i)) = c_nm_e(i);
          d_nm(p_vec_e(i)) = d_nm_e(i);
        }
        for (int i = 0; i < n_vec_o.size(); i++) {
          c_nm(p_vec_o(i)) = c_nm_o(i);
          d_nm(p_vec_o(i)) = d_nm_o(i);
        }
      }

      // Calculate for m < 0
      if (m) {
        ArrayXi p_vec_n_e = n_vec_e * (n_vec_e + 1) - m; // [N_e X 1]
        ArrayXi p_vec_n_o = n_vec_o * (n_vec_o + 1) - m; // [N_e X 1]

        // For negative m, using eq. 5.37 of [Mishchenko 2002]
        VectorXc<Real> p_nm_n_e = st_TR_list[m_ind]->st_4M_T_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
            a_nm(p_vec_n_e).matrix() - st_TR_list[m_ind]->st_4M_T_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
            b_nm(p_vec_n_o).matrix();
        VectorXc<Real> q_nm_n_o = -st_TR_list[m_ind]->st_4M_T_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
            a_nm(p_vec_n_e).matrix() + st_TR_list[m_ind]->st_4M_T_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
            b_nm(p_vec_n_o).matrix();
        VectorXc<Real> p_nm_n_o = st_TR_list[m_ind]->st_4M_T_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
            a_nm(p_vec_n_o).matrix() - st_TR_list[m_ind]->st_4M_T_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
            b_nm(p_vec_n_e).matrix();
        VectorXc<Real> q_nm_n_e = -st_TR_list[m_ind]->st_4M_T_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
            a_nm(p_vec_n_o).matrix() + st_TR_list[m_ind]->st_4M_T_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
            b_nm(p_vec_n_e).matrix();

        for (int i = 0; i < n_vec_e.size(); i++) {
          p_nm(p_vec_n_e(i)) = p_nm_n_e(i);
          q_nm(p_vec_n_e(i)) = q_nm_n_e(i);
        }
        for (int i = 0; i < n_vec_o.size(); i++) {
          p_nm(p_vec_n_o(i)) = p_nm_n_o(i);
          q_nm(p_vec_n_o(i)) = q_nm_n_o(i);
        }

        if (get_R) {
          VectorXc<Real> c_nm_n_e = st_TR_list[m_ind]->st_4M_R_eo().M11(n_vec_T_e, n_vec_T_e).matrix() *
              a_nm(p_vec_n_e).matrix() - st_TR_list[m_ind]->st_4M_R_eo().M12(n_vec_T_e, n_vec_T_o).matrix() *
              b_nm(p_vec_n_o).matrix();
          VectorXc<Real> d_nm_n_o = -st_TR_list[m_ind]->st_4M_R_eo().M21(n_vec_T_o, n_vec_T_e).matrix() *
              a_nm(p_vec_n_e).matrix() + st_TR_list[m_ind]->st_4M_R_eo().M22(n_vec_T_o, n_vec_T_o).matrix() *
              b_nm(p_vec_n_o).matrix();
          VectorXc<Real> c_nm_n_o = st_TR_list[m_ind]->st_4M_R_oe().M11(n_vec_T_o, n_vec_T_o).matrix() *
              a_nm(p_vec_n_o).matrix() - st_TR_list[m_ind]->st_4M_R_oe().M12(n_vec_T_o, n_vec_T_e).matrix() *
              b_nm(p_vec_n_e).matrix();
          VectorXc<Real> d_nm_n_e = -st_TR_list[m_ind]->st_4M_R_oe().M21(n_vec_T_e, n_vec_T_o).matrix() *
              a_nm(p_vec_n_o).matrix() + st_TR_list[m_ind]->st_4M_R_oe().M22(n_vec_T_e, n_vec_T_e).matrix() *
              b_nm(p_vec_n_e).matrix();

          for (int i = 0; i < n_vec_e.size(); i++) {
            c_nm(p_vec_n_e(i)) = c_nm_n_e(i);
            d_nm(p_vec_n_e(i)) = d_nm_n_e(i);
          }
          for (int i = 0; i < n_vec_o.size(); i++) {
            c_nm(p_vec_n_o(i)) = c_nm_n_o(i);
            d_nm(p_vec_n_o(i)) = d_nm_n_o(i);
          }
        }
      }
    }

    auto output = make_unique<stAbcdnm<Real>>();
    output->p_nm = p_nm; // [P X 1]
    output->q_nm = q_nm; // [P X 1]
    output->a_nm = a_nm; // [P X 1]
    output->b_nm = b_nm; // [P X 1]
    output->c_nm = c_nm; // if get_R, [P X 1]
    output->d_nm = d_nm; // if get_R, [P X 1]
    return output;
  }

  /*
  Calculate absorption, scattering and extinction cross sections, for the case of orientationally-averaged incidence.
  This is for matrices which make use of rvh symmetry.
  Inputs:
    k1 - wavevector [L X 1]
    st_TR_list - std::vector containing unique pointers to stTR structs (in rvh-block form).
                 Should be [L X M] and should contain all m where 0 <= m <= N
  Output:
    Returns a stCrossSection struct, where each member is of size [L X 1] and
    contains information for each wavelength.
  Dependencies:
    mp_pi
  */
  template <class Real>
  unique_ptr<stCrossSection<Real>> rvhGetAverageCrossSections(
      const ArrayXr<Real>& k1, const vector<vector<unique_ptr<stTR<Real>>>>& st_TR_list) {
    int L = st_TR_list.size();
    int M = st_TR_list[0].size();

    ArrayXc<Real> ext_sum = ArrayXc<Real>::Zero(L);
    ArrayXc<Real> sca_sum = ArrayXc<Real>::Zero(L);

    for (int l = 0; l < L; l++) { // Loop on lambda
      if (st_TR_list[l][0]->st_4M_T_eo().ind1.size() + st_TR_list[l][0]->st_4M_T_eo().ind2.size() + 1 != M)
        cout << "Warning in rvhGetAverageCrossSections: " <<
            "st_TR_list does not seem to contain T-matrices for all m" << endl;
      for (int m = 0; m < M; m++) {
        if (st_TR_list[l][m]->st_4M_T_eo().m != m || st_TR_list[l][m]->st_4M_T_oe().m != m)
          cout << "Warning in rvhGetAverageCrossSections: " <<
              "st_TR_list does not seem to contain T-matrices for all m" << endl;
        Real m_factor = m ? 1 : 0.5;

        // From eq. 5.107 of Mishchenko 2002
        ext_sum(l) += m_factor * (
            st_TR_list[l][m]->st_4M_T_eo().M11.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_oe().M11.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_eo().M22.matrix().diagonal().sum() +
            st_TR_list[l][m]->st_4M_T_oe().M22.matrix().diagonal().sum());

        // From eq. 5.141 of Mishchenko 2002
        sca_sum(l) += m_factor * (
            st_TR_list[l][m]->st_4M_T_eo().M11.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M12.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M21.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_eo().M22.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M11.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M12.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M21.abs().pow(2).sum() +
            st_TR_list[l][m]->st_4M_T_oe().M22.abs().pow(2).sum());
      }
    }

    auto output = make_unique<stCrossSection<Real>>();
    output->C_ext = -4*mp_pi<Real>()/k1.pow(2) * ext_sum.real(); // [L X 1]
    output->C_sca = 4*mp_pi<Real>()/k1.pow(2) * sca_sum.real(); // [L X 1]
    output->C_abs = -4*mp_pi<Real>()/k1.pow(2) * (ext_sum.real() + sca_sum.real()); // [L X 1]

    return output;
  }

  /*
  Finds an estimate for delta by studying the convergence of the T^{22,m=1}_{11} matrix element.
  Delta + 1 is the NQ for which this element has reached convergence.
  See JQSRT 160, 29 *2015) for further details. Also returns the estimated converged precision.
  Inputs:
    st_geometry - unique pointer to struct containing geometric information, as from sphMakeGeometry
    params - unique pointer to struct containing simulation parameters (only the first lambda is considered)
    NQ_max - maximum number of multipoles
  Output:
    Returns a struct containing an estimate for delta and estimated converged precision.
    If delta still could not be found, it returns output->delta = 0 and output->err = NaN.
  Dependencies:
    rvhGetTRfromPQ, rvhTruncateMatrices, sphCalculatePQ
  */
  template <class Real>
  stDelta<Real> sphEstimateDelta(const unique_ptr<stRtfunc<Real>>& st_geometry,
      const unique_ptr<stParams<Real>>& params, int NQ_max = 80) {
    stDelta<Real> output;
    Real min_acc = 1e-4;
    ArrayXi abs_m_vec = ArrayXi::Ones(1); // only one m

    // This works on only one wavelength, so we choose the largest k1 * s as representative of the worst case.
    // Find max and min relative refractive index
    int max_ind;
    (params->k1 * params->s).maxCoeff(&max_ind);
    auto param1 = make_unique<stParams<Real>>();
    param1->s = ArrayXr<Real>::Constant(1, params->s(max_ind));
    param1->k1 = ArrayXr<Real>::Constant(1, params->k1(max_ind));

    // Number of pointer used for linear fit to check if error has reached a plateau
    int N_for_conv = 5;
    MatrixXr<Real> A_fit(N_for_conv, 2);
    A_fit.col(0) = VectorXr<Real>::Ones(N_for_conv);
    A_fit.col(1) = VectorXr<Real>::LinSpaced(N_for_conv, 1, N_for_conv);
    // Store errors
    ArrayXr<Real> T_2211_err = ArrayXr<Real>::Zero((NQ_max + 1)/2);
    // Calculate P,Q matrices
    vector<unique_ptr<stPQ<Real>>> st_PQ_list = sphCalculatePQ(NQ_max, abs_m_vec, st_geometry, param1, NQ_max);
    complex<Real> T_2211 = complex<Real>(0.0, 0.0);

    for (int N = 1; N <= NQ_max; N += 2) { // Loop over truncation (only odd numbers)
      // Truncate P,Q to N and get corresponding T
      int n_count = (N - 1)/2; // Expression is always mathematically an integer
      vector<unique_ptr<stPQ<Real>>> trunc_st_PQ_list = rvhTruncateMatrices(st_PQ_list, N);
      vector<unique_ptr<stTR<Real>>> st_TR_list = rvhGetTRfromPQ(trunc_st_PQ_list, false);
      complex<Real> T_2211_new = st_TR_list[0]->st_4M_T_eo().M22(0, 0);

      // Relative error
      T_2211_err(n_count) = abs(T_2211/T_2211_new - static_cast<complex<Real>>(1));

      T_2211 = T_2211_new;
      // Minimum requirement for convergence
      if (n_count >= N_for_conv && T_2211_err(n_count) < min_acc) {
        // Then test if the last N_for_conv errors have flattened out by a
        // linear regression to the last N_for_conv points
        VectorXr<Real> b_vec = log10(abs(T_2211_err(seq(n_count - N_for_conv + 1, n_count))));
        MatrixXr<Real> coeff = A_fit.bdcSvd(ComputeThinU | ComputeThinV).solve(b_vec);
        Real slope = coeff(1);
        // if slope < 0, then still converging
        if (slope > -0.5) { // this means less than one digit better over 2*N_for_conv steps
          output.delta = N - 2*N_for_conv + 2; // N = 2*n_count - 1 > 2*N_for_conv - 1
          output.err = (T_2211_err(seq(n_count - N_for_conv + 1, n_count))).mean();
          return output;
        }
      }
    }
    output.delta = -1;
    output.err = 0;
    return output;
  }
}

#endif


#define CATCH_CONFIG_MAIN

#include "CALPHADConcSolverTernary.h"
#include "CALPHADFreeEnergyFunctionsTernary.h"
#include "CALPHADFunctions.h"
#include "CALPHADSpeciesPhaseGibbsEnergy.h"
#include "PhysicalConstants.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <iostream>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD ternary solver", "[ternary solver]")
{
    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadMoNbTa.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    double temperature = 2923.;

    CalphadDataType LmixABPhaseL[4][2];
    CalphadDataType LmixABPhaseA[4][2];

    CalphadDataType LmixACPhaseL[4][2];
    CalphadDataType LmixACPhaseA[4][2];

    CalphadDataType LmixBCPhaseL[4][2];
    CalphadDataType LmixBCPhaseA[4][2];

    CalphadDataType LmixABCPhaseL[3][2];
    CalphadDataType LmixABCPhaseA[3][2];

    {
        std::string dbnamemixL("LmixABCPhaseL");
        if (calphad_db.get_child_optional(dbnamemixL))
        {
            pt::ptree& Lmix0_db = calphad_db.get_child(dbnamemixL);
            Thermo4PFM::readLmixTernaryParameters(Lmix0_db, LmixABCPhaseL);
        }
        else
        {
            for (int j = 0; j < 3; j++)
                for (int i = 0; i < 2; i++)
                {
                    LmixABCPhaseL[j][i] = 0.;
                }
        }
    }
    {
        std::string dbnamemixL("LmixABCPhaseA");
        if (calphad_db.get_child_optional(dbnamemixL))
        {
            pt::ptree& Lmix0_db = calphad_db.get_child(dbnamemixL);
            Thermo4PFM::readLmixTernaryParameters(Lmix0_db, LmixABCPhaseA);
        }
        else
        {
            for (int j = 0; j < 3; j++)
                for (int i = 0; i < 2; i++)
                {
                    LmixABCPhaseA[j][i] = 0.;
                }
        }
    }

    {
        std::string dbnamemixL("LmixABPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixABPhaseL);
    }
    {
        std::string dbnamemixA("LmixABPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixABPhaseA);
    }
    {
        std::string dbnamemixL("LmixACPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixACPhaseL);
    }
    {
        std::string dbnamemixA("LmixACPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixACPhaseA);
    }
    {
        std::string dbnamemixL("LmixBCPhaseL");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixL);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixBCPhaseL);
    }
    {
        std::string dbnamemixA("LmixBCPhaseA");
        pt::ptree Lmix_db = calphad_db.get_child(dbnamemixA);
        Thermo4PFM::readLmixBinary(Lmix_db, LmixBCPhaseA);
    }

    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseL[3];
    Thermo4PFM::CALPHADSpeciesPhaseGibbsEnergy g_species_phaseA[3];
    {
        std::string dbnameL("PhaseL");
        std::string dbnameA("PhaseA");

        pt::ptree& speciesA_db = calphad_db.get_child("SpeciesA");
        g_species_phaseL[0].initialize("L0", speciesA_db.get_child(dbnameL));
        g_species_phaseA[0].initialize("A0", speciesA_db.get_child(dbnameA));

        pt::ptree& speciesB_db = calphad_db.get_child("SpeciesB");
        g_species_phaseL[1].initialize("L1", speciesB_db.get_child(dbnameL));
        g_species_phaseA[1].initialize("A1", speciesB_db.get_child(dbnameA));

        pt::ptree& speciesC_db = calphad_db.get_child("SpeciesC");
        g_species_phaseL[2].initialize("L2", speciesC_db.get_child(dbnameL));
        g_species_phaseA[2].initialize("A2", speciesC_db.get_child(dbnameA));
    }

    CalphadDataType fA[2];
    fA[0] = g_species_phaseL[0].fenergy(temperature);
    fA[1] = g_species_phaseA[0].fenergy(temperature);
    // std::cout<<"fA[0]="<<fA[0]<<", fA[1]="<<fA[1]<<std::endl;

    CalphadDataType fB[2];
    fB[0] = g_species_phaseL[1].fenergy(temperature);
    fB[1] = g_species_phaseA[1].fenergy(temperature);
    // std::cout<<"fB[0]="<<fB[0]<<", fB[1]="<<fB[1]<<std::endl;

    CalphadDataType fC[2];
    fC[0] = g_species_phaseL[2].fenergy(temperature);
    fC[1] = g_species_phaseA[2].fenergy(temperature);

    CalphadDataType L_AB_L[4];
    for (int i = 0; i < 4; i++)
        L_AB_L[i] = LmixABPhaseL[i][0] + temperature * LmixABPhaseL[i][1];
    CalphadDataType L_AB_S[4];
    for (int i = 0; i < 4; i++)
        L_AB_S[i] = LmixABPhaseA[i][0] + temperature * LmixABPhaseA[i][1];

    CalphadDataType L_AC_L[4];
    for (int i = 0; i < 4; i++)
        L_AC_L[i] = LmixACPhaseL[i][0] + temperature * LmixACPhaseL[i][1];

    CalphadDataType L_AC_S[4];
    for (int i = 0; i < 4; i++)
        L_AC_S[i] = LmixACPhaseA[i][0] + temperature * LmixACPhaseA[i][1];

    CalphadDataType L_BC_L[4];
    for (int i = 0; i < 4; i++)
        L_BC_L[i] = LmixBCPhaseL[i][0] + temperature * LmixBCPhaseL[i][1];

    CalphadDataType L_BC_S[4];
    for (int i = 0; i < 4; i++)
        L_BC_S[i] = LmixBCPhaseA[i][0] + temperature * LmixBCPhaseA[i][1];

    CalphadDataType L_ABC_L[3];
    for (int i = 0; i < 3; i++)
        L_ABC_L[i] = LmixABCPhaseL[i][0] + temperature * LmixABCPhaseL[i][1];

    CalphadDataType L_ABC_S[3];
    for (int i = 0; i < 3; i++)
        L_ABC_S[i] = LmixABCPhaseA[i][0] + temperature * LmixABCPhaseA[i][1];

    const double RTinv
        = 1.0 / (Thermo4PFM::gas_constant_R_JpKpmol * temperature);

    double sol[4] = { 0.33, 0.38, 0.32, 0.33 };
    double hphi   = 0.5;
    double c0     = 0.33;
    double c1     = 0.33;

    Thermo4PFM::CALPHADConcSolverTernary solver;
    solver.setup(c0, c1, hphi, RTinv, L_AB_L, L_AC_L, L_BC_L, L_AB_S, L_AC_S,
        L_BC_S, L_ABC_L, L_ABC_S, fA, fB, fC);

    int nits = solver.ComputeConcentration(sol, 1.e-8, 50);

    std::cout << "Solution = " << sol[0] << "," << sol[1] << "," << sol[2]
              << "," << sol[3] << std::endl;
    double ref_sol[4] = { 0.339215, 0.356739, 0.320785, 0.303261 };

    CHECK(sol[0] == Approx(ref_sol[0]).margin(1.e-6));
    CHECK(sol[1] == Approx(ref_sol[1]).margin(1.e-6));
    CHECK(sol[2] == Approx(ref_sol[2]).margin(1.e-6));
    CHECK(sol[3] == Approx(ref_sol[3]).margin(1.e-6));

    std::cout << "nits=" << nits << std::endl;
}


/*
 * hcost_tile_library.h
 *
 *  Created on: Feb 23, 2016
 *      Author: bscooper
 */

#ifndef HCOST_TILE_LIBRARY_HPP
#define HCOST_TILE_LIBRARY_HPP

#include <iostream>
#include <list>
#include <vector>
#include <cmath>
#include <map>
#include <algorithm>
#include <iterator>
#include <memory>
#include <functional>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SparseCore>
//#include <eigen3/Eigen/Core>
//#include <Eigen/Sparse>

namespace librav {
const double PI = 3.141592653589793;
const double T_INFINITY = 1.0e9;
const double TOL = 1.0e-9;
// Largest value of H considered
const unsigned short int MAX_H = 5;
// Minimum and Maximum speed considered
const unsigned int SPD_MIN = 1;
const unsigned int SPD_MAX = 1;

const int N_CBTA_W_SOL = 51;
const int N_CBTA_W = 101;
// const double r_min = 2;
// // double r_min = 0.0;

struct REGION_BD{
		std::vector<double> region_w_lower;    //REGION_BD(0,:)
		std::vector<double> region_w_upper;    //REGION_BD(1,:)
		std::vector<double> region_psi_lower;  //REGION_BD(2,:)
		std::vector<double> region_psi_upper;  //REGION_BD(3,:)
		std::vector<double> region_vel_lower;  //REGION_BD(4,:)
		std::vector<double> region_vel_upper;  //REGION_BD(5,:)
	};

class TileBlock; // say TileBlock exists without defining it, forward declaration
				 // so that Tile can declare it's instance of a TileBlock
// =============================== Tile ===================================
/* Each Tile with channel_data, cell_vertices, traversal_type,
 * cell_xform, traversal_faces, cell_edges, and connectivity
 */
class Tile{
public:
	Tile(int H, Eigen::Matrix<int,Eigen::Dynamic,4> tile_vertices);
	~Tile();

private:
	// Tile class Transition and transformation references
	Eigen::Matrix<int,12,5> FACE_REF;
	Eigen::Matrix<int,5,4> VERTICES_PERMUTATION;
	//VectorXi INVERSE_XFORM(5);
	Eigen::Matrix<int,5,1> INVERSE_XFORM;

public:

	Eigen::Matrix<int,Eigen::Dynamic,4> channel_data;
	Eigen::Matrix<double,4,Eigen::Dynamic> cell_vertices;
	Eigen::Matrix<int,Eigen::Dynamic,1> traversal_type;
	Eigen::Matrix<int,Eigen::Dynamic,2> cell_xform;
	Eigen::Matrix<int,Eigen::Dynamic,1> traversal_faces;
	Eigen::Matrix<double,Eigen::Dynamic,4> cell_edge;

	std::shared_ptr<TileBlock> tile_block;

	std::shared_ptr<Eigen::SparseMatrix<int,Eigen::RowMajor>> connectivity;

	void set_tile_data(int, Eigen::Matrix<int,Eigen::Dynamic,4>);

	void addTileBlock(REGION_BD &region_bd, std::shared_ptr<TileBlock> this_tile, int H);
};

// =============================== TileBlock ==============================
/* A TileBlock is associated with a Tile. TileBlock is responsible for the
 * Curvature Bounded Traversability Analysis (CBTA), and this information
 * is used in the edge costs calculations needed by the Lifted Graph
 * transition function. TileBlock needs access to it's associated Tile's
 * data: channel_data, cell_vertices, ...
 */
class TileBlock{
public:
	TileBlock();
	TileBlock(REGION_BD &region_bd, std::shared_ptr<Tile> linked_tile, int Hin);
	~TileBlock();
private:
	int H;
	Eigen::Matrix<double,Eigen::Dynamic,1> y_exit;
	Eigen::Matrix<double,Eigen::Dynamic,1> z_exit;

	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> bta_smp; //Matrix<double,Dynamic,N_CBTA_W>

	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W_SOL> alfa_sol;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> alfa_smp;

	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> w_smp;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> x_smp;
public:
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> alfa;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> bta;
	Eigen::Matrix<double,Eigen::Dynamic,1> w_lower;
	Eigen::Matrix<double,Eigen::Dynamic,1> w_upper;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> x;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W> w;
	Eigen::Matrix<double,Eigen::Dynamic,N_CBTA_W_SOL> w_sol;

	std::shared_ptr<Tile> tile;

public:
	void cbta(double r_min);
	void cbra(int idx_r_from,REGION_BD &theRegion,double r_min,
				long int N_REGION_TOTAL, Eigen::RowVectorXi& region_target_2,
	//			Matrix<int,1,Dynamic>& returnMatrix);
				Eigen::RowVectorXi& region_neighbors);
	static int find_sample(Eigen::RowVectorXd& ySmp, double y);

private:
	void cbta_s1(double w, double d,
			     //Matrix<double,1,N_CBTA_W>& xSmp,
			Eigen::RowVectorXd& xSmp,
			Eigen::Matrix<double,2,N_CBTA_W>& btaSmp,
			Eigen::Matrix<double,2,1>& returnMatrix,
			double r_min);

	void cbta_s2(double w, double d,
			     //Matrix<double,1,N_CBTA_W>& xSmp,
			Eigen::RowVectorXd& xSmp,
			Eigen::Matrix<double,2,N_CBTA_W>& btaSmp,
			Eigen::Matrix<double,2,1>& returnMatrix,
			double r_min);

	void interp_broken_seg(Eigen::RowVectorXd& x_data,
			Eigen::Matrix<double,2,Eigen::Dynamic>& y_data,
			Eigen::RowVectorXd& x_interp,
			Eigen::Matrix<double,2,Eigen::Dynamic>& y_interp);
	template <typename Derived1, typename Derived2>
	void remove_inf_values(Eigen::MatrixBase<Derived1>& v,
			Eigen::MatrixBase<Derived2>& returnMatrix);

//	int find_sample(Matrix<double,1,N_CBTA_W>& ySmp, double y);

	//double sign_func(double x);
	template <typename Derived_a, typename Derived_b>
	void find_zeros(Eigen::MatrixBase<Derived_a>& fSmp,
			Eigen::MatrixBase<Derived_b>& returnMatrix);
//	void find_zeros(Matrix<double,1,Dynamic>& fSmp,
//			Matrix<int,1,Dynamic>& returnMatrix);

	double pi2pi(double x);

};

// ================================ Hlevel =================================
/* HcostTileLibrary contains Hlevel's H1, H2, ....
 * Each Hlevel contains a map of Tiles with channel_data, cell_vertices, traversal_type,
 * cell_xform, traversal_faces, cell_edges, and connectivity
 */
class Hlevel{
public:
	Hlevel(unsigned int Hin);
	~Hlevel();

public:
	unsigned int H;
	std::map<unsigned int, std::shared_ptr<Tile>> Tiles; // tiles associated with particular Hlevel
	void get_tile_data(void);
	void add_tile(std::shared_ptr<Tile> newTile);
	unsigned int n_tiles;
	Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic> unique_tiles;

};

struct TileTraversalData{
	std::map<unsigned int,std::shared_ptr<Hlevel>> Hlevels;
	REGION_BD region_bd;
	int N_REGION_W;
	int N_REGION_PSI;
	int N_REGION_SPD;
	int N_REGION_TOTAL;
};

} /* namespace srcl */

#endif /* HCOST_TILE_LIBRARY_HPP */


/* type definitions for monte carlo simulation */

/*
 * HISTORY
 * 16-Oct-88 Jeffrey Trull (jt1j) at Carnegie-Mellon University
 *       Created
 */

/* note: in actual fact I am typing this in, 30 years later, from a paper copy,
 * and being a bit inexact about it
 */

#ifndef MONTE_HPP
#define MONTE_HPP

#include <boost/units/cmath.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/codata/universal_constants.hpp>
#include <boost/units/systems/si/codata/electron_constants.hpp>
#include <boost/units/systems/si/codata/electromagnetic_constants.hpp>

#include <boost/accumulators/numeric/functional.hpp>

// Accumulators don't like Boost.Units for various reasons
// but as usual there are customization points we can use to fix things
// I will do one as a training exercise but if writing this for others
// I would probably write a single-purpose custom accumulator of my own...

namespace boost { namespace numeric
{
// teach Accumulators how to produce a value of "one" for double/float times
// as the default weight for our time-weighted average velocity
// It shouldn't be necessary for the calculation since we will always supply a weight...

// specialize one<>
template<typename Float>
struct one<boost::units::quantity<boost::units::si::time, Float>>
{
    using type = one;
    using value_type = boost::units::quantity<boost::units::si::time, Float>;
    static value_type const value;

    operator value_type const & () const
    {
        throw std::runtime_error("we should never get here");
        return one::value;
    }
};

template<typename Float>
boost::units::quantity<boost::units::si::time, Float> const
one<boost::units::quantity<boost::units::si::time, Float>>::value =
    Float{1.0} * boost::units::si::second;

}}

namespace monte {

using namespace boost::units::si;

// constants
constexpr double two_pi   = boost::math::constants::two_pi<double>();
constexpr double     pi   = boost::math::constants::pi<double>();
constexpr auto   echarge  = constants::codata::e;
constexpr auto   dirac    = constants::codata::hbar;
constexpr auto   T        = 300.0 * kelvin;  // room temperature
constexpr auto   cm       = 0.01 * meter;
constexpr auto   angstrom = 1e-10 * meter;

/*
 * evaluate the constant of proportionality between the acoustic
 * scattering rate and the square root of the electron energy
 */
constexpr auto   eV = echarge * volt;
constexpr auto   g  = 10e-3 * kilogram;
constexpr auto   cm3 = cm*cm*cm;

// properties of GaAs
constexpr auto   E1 = 7.0 * eV;            // Acoustic deformation potential
constexpr auto   rho = 5.37 * g / cm3;     // Crystal density
constexpr auto   u = 5.2E5 * cm / second;  // speed of sound in GaAs
constexpr double meff = 0.063; // effective electron mass in gamma (000) valley (unitless)

// Shur 2-3-12 as modified by assignment (terms after gamma^1/2 removed)
constexpr auto scatter_const =
    // units for this initial constant not given in Shur :-/
    0.449E18 * ((g / cm3) * (cm / second) * (cm / second) /
                (kelvin * eV * eV * sqrt(eV) * second)) *
    std::pow(meff, 1.5) * T * (E1*E1) /
    (rho * (u*u)) ;

/*
 * evaluate the accelerations due to the constant electric field
 * between collisions
 */
constexpr auto Efield = 10000.0 * volt / cm;
constexpr auto accel_const = echarge * Efield / dirac ;

/*
 * evaluate k-to-velocity conversion constant
 */

// The instantaneous velocity (in real space) is the gradient of E with respect to k
// The Shur book seems to suggest calculating initial and final energies from a given timestep
// then assuming a straight line between. Here I am just using the instantaneous value,
// which is wrong.
constexpr auto   m_e       = constants::codata::m_e;   // electron mass
// If E(k) = (dirac^2 * k^2)/(2 * m_eff), and v = (1/dirac) * dE(k)/dt, then
// v(k) = dirac * k / m_eff
constexpr auto   vel_const = dirac/(meff * m_e) ;

// types

template<typename Float>
struct vector_str {
    vector_str() = default;
    boost::units::quantity<wavenumber, Float> x, y, z;

    // the energy associated with this k state
    boost::units::quantity<energy, Float> get_energy() const;

    // new set of k resulting from a collision
    vector_str
    collision_result(Float theta_r, Float phi_r) const;

};

template<typename Float>
using vector_ptr = vector_str<Float> *;

template<typename Float>
using vector_cptr = vector_str<Float> const *;

// utility functions

/* k to energy conversion */
template<typename Float>
boost::units::quantity<energy, Float>
vector_str<Float>::get_energy() const
/*
 * This appears to be very complicated and depends on which region
 * of reciprocal lattice space the electron currently occupies.
 * To make this tractable I'm assuming a parabolic relationship
 * given by E(k) = (dirac^2 * k^2)/(2 * m_eff)
 * See Shur p14.
 */
{
    constexpr auto C = (dirac * dirac) / (2.0 * meff * m_e) ;

    boost::units::quantity<energy> nrg = C * ((x * x) + (y * y) + (z * z));

    // returning above calculation directly gets a compile error
    // but we can explicitly convert it to another (possibly lower) precision *shrug*
    return boost::units::quantity<energy, Float>{nrg};

}

template<typename Float>
vector_str<Float>
vector_str<Float>::collision_result(Float theta_r, Float phi_r) const
{
    using namespace boost::units;

    // calculate our current spherical angles
    using angle = quantity<si::plane_angle, Float>;
    angle phi{atan(y/x)};
    angle theta{atan(sqrt(x*x + y*y)/z)};
    // and our current rho (radius)
    // total crystal momentum is preserved
    quantity<si::wavenumber, Float> total_k{sqrt(x*x + y*y + z*z)};

    // determine momentum components if original vector were Z axis aligned:
    vector_str<Float> k2prime;
    k2prime.x = total_k * sin(theta_r) * cos(phi_r);
    k2prime.y = total_k * sin(theta_r) * sin(phi_r);
    k2prime.z = total_k * cos(theta_r);

    // rotate to reflect actual initial angles and return
    vector_str<Float> lastk;
    lastk.x = cos(phi) * cos(theta) * k2prime.x
        + cos(phi) * sin(theta) * k2prime.z
        - sin(phi) * k2prime.y;

    lastk.y = sin(phi) * cos(theta) * k2prime.x
        + sin(phi) * sin(theta) * k2prime.z
        + cos(phi) * k2prime.y;

    lastk.z = cos(theta) * k2prime.z - sin(theta) * k2prime.x;

    return lastk;
}

}

#endif // MONTE_HPP


#include <armadillo>

#include "settings.h"
#include "RigidAlignment.h"

#include "landmark/LandmarkIO.h"

#include "mesh/MeshIO.h"
#include "mesh/Mesh.h"
#include "mesh/MeshNeighbors.h"
#include "mesh/MeshNeighborsIO.h"
#include "mesh/MeshOneRingNeighborsBuilder.h"
#include "mesh/MeshGeodesicNeighborsBuilder.h"
#include "mesh/MeshSphericalNeighborsBuilder.h"
#include "mesh/MeshResolution.h"

#include "mesh/MeshSmooth.h"

#include "optimization/matchtemplate/Energy.h"
#include "optimization/matchtemplate/EnergyMinimizer.h"
#include "optimization/matchtemplate/MinimizerSettings.h"

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

  Settings settings(argc, argv);

  // read input data
  Mesh source = MeshIO::read(settings.source);
  Mesh target = MeshIO::read(settings.target);

  if( settings.performRigidAlignment == true) {

    source = RigidAlignment(source, target, settings).perform();

  }

  // deal with target meshes that do not provide normals
  if( settings.fixedNeighbors == false && target.has_normals() == false ) {

    // try to estimate normals if the mesh has faces
    if( target.has_faces() == true ) {
      NormalEstimation estimation(target);
      target.set_vertex_normals(estimation.compute());
    } // end if
    else {
      // use point-to-point distance measure
      settings.energySettings.searchStrategy =
        matchTemplate::EnergySettings::SearchStrategy::BASIC;
    } // end else

  } // end if

  MeshResolution resolution(source);

  arma::vec min, max;

  source.get_bounding_box(min, max);

  const double scaleFactor = 1. / arma::norm( max - min );

  settings.energySettings.maxDistance  *= scaleFactor;
  settings.energySettings.searchRadius *= scaleFactor;

  MeshNeighbors neighbors;

  if( settings.meshNeighborhoodPresent == true ) {
    neighbors = MeshNeighborsIO::read(settings.meshNeighborhood);
  }
  else {
    MeshOneRingNeighborsBuilder builder(source);
    neighbors = neighbors + builder.get_neighbors();

    if( settings.addGeodesic == true ) {

      MeshGeodesicNeighborsBuilder builder(
        source, resolution.get_resolution() * settings.geodesicNeighborhoodSize);
      neighbors = neighbors + builder.get_neighbors();

    }

    if( settings.addSpherical == true ) {

      MeshSphericalNeighborsBuilder builder(
        source, resolution.get_resolution() * settings.sphericalNeighborhoodSize);
      neighbors = neighbors + builder.get_neighbors();

    }
  }

  for( arma::vec& vertex: source.get_vertices() ) {
    vertex *= scaleFactor;
  }

  for( arma::vec& vertex: target.get_vertices() ) {
    vertex *= scaleFactor;
  }

  matchTemplate::EnergyData data(source, neighbors, target);

  if(settings.landmarksPresent == true) {
    data.landmarks = LandmarkIO::read(settings.landmarks);

    for( Landmark& mark: data.landmarks ) {

      mark.targetPosition *= scaleFactor;

    }

  }

  settings.energySettings.weights["dataTerm"] = 1;
  settings.energySettings.weights["smoothnessTerm"] = settings.smoothnessTermWeight;
  settings.energySettings.weights["postSmoothnessTerm"] = settings.postSmoothnessTermWeight;
  settings.energySettings.changeFactors["smoothnessTerm"] = settings.smoothnessTermChange;
  settings.energySettings.weights["landmarkTerm"] = settings.landmarkTermWeight;
  settings.energySettings.changeFactors["landmarkTerm"] = settings.landmarkTermChange;

  matchTemplate::Energy energy(data, settings.energySettings);

  matchTemplate::EnergyMinimizer minimizer(energy, settings.minimizerSettings);

  minimizer.minimize();

  for( arma::vec& vertex: energy.derived_data().source.get_vertices() ) {
    vertex /= scaleFactor;
  }

  Mesh result = energy.derived_data().source;

  // remove normals
  result.get_vertex_normals().clear();

  MeshSmooth(result).apply(settings.meshSmoothIterations);

  MeshIO::write(result, settings.output);

  return 0;

}


/*
 * File:   hamiltonian.hpp
 * Copyright (C) 2013-2014  K M Masum Habib <masum.habib@gmail.com>
 *
 * Created on April 6, 2013, 5:52 PM
 *
 * Description: Tight binding calculation logic and data.
 *
 */

#ifndef HAMILTONIAN_HPP
#define	HAMILTONIAN_HPP

#include <boost/serialization/string.hpp>
#include <boost/serialization/access.hpp>

#include "maths/constants.h"
#include "maths/arma.hpp"
#include "atoms/AtomicStruct.h"
#include "utils/vout.h"
#include "utils/std.hpp"

namespace quest{
namespace hamiltonian{

using namespace utils::stds;
using utils::Printable;
using namespace maths::armadillo;
using atoms::AtomicStruct;
using atoms::PeriodicTable;
using namespace maths::spvec;
using namespace maths::constants;
using utils::stds::static_pointer_cast;

template<class T>
class HamParams: public Printable{
public:
    HamParams(const string &prefix = ""):
        Printable(" " + prefix)
    {
    }

    void   dtol(double dtol){ mdtol = dtol; update(); }
    double dtol() const { return mdtol; }

    void   orthogonal(bool orth){ mortho = orth; update(); }
    bool   orthogonal() const { return mortho; }

    void   periodicTable(const PeriodicTable &pt) { mpt = pt; update(); }
    const  PeriodicTable &periodicTable() const { return mpt; }

    void   Bz(double Bz, int gauge = coord::X){ mBz = Bz; mBzGauge = gauge; update(); };
    double Bz() const { return mBz; }

    //!< Generate Hamiltonian between two atoms.
    virtual T twoAtomHam(const AtomicStruct& atomi,
                            const AtomicStruct& atomj) const { return T(); };
    //!< Generate Overlap matrix between two atoms.
    virtual T twoAtomOvl(const AtomicStruct& atomi,
                            const AtomicStruct& atomj) const { return T(); };

protected:
    // Updates internal parameters. Call it after changing any of the
    // public parameters.
    virtual void update() {};

    // Caluclates Peierl's phase factor for magnetic field
    dcmplx calcPeierlsPhase(double xi, double yi, double xj, double yj) const;

protected:
    // Parameters required for all Hamiltonin
    double mdtol;         //!< Distance tolerance.
    bool   mortho;        //!< Is this an orthogonal basis.
    PeriodicTable mpt;    //!< The periodic table required for this system.
    double mBz;           //!< The z-component of magnetic field.
    int    mBzGauge;      //!< gauge choice for the z-component.
    //!< pre-factor for magnetic phase = i*hbar/q/2
    double mfactor = 1E-20*q/hbar/2;
    static constexpr double mBzTol = 1E-10;
};

// Caluclates Peierl's phase factor for magnetic field
template<class T>
dcmplx HamParams<T>::calcPeierlsPhase(double xi, double yi,
        double xj, double yj) const
{
    dcmplx phase = dcmplx(1,0);
    if(abs(mBz) > mBzTol) {
        dcmplx iphi = dcmplx(0,0);
        if (mBzGauge == coord::X) { // for A = (-Bz*y, 0, 0)
            iphi = dcmplx(0, mfactor*mBz*(xi - xj)*(yi + yj));
        } else if (mBzGauge == coord::Y) { // for A = (0, Bz*x, 0)
            iphi = dcmplx(0, mfactor*mBz*(yj - yi)*(xi + xj));
        }
        phase = exp(iphi);
    }
    return phase;
}

//!< Generates the hamiltonaina and overlap matrices between atomc block
//!< i and atomic block j.
template<class T>
void generateHamOvl(T &hmat, T&smat, const HamParams<T> &p,
        const AtomicStruct &bi, const AtomicStruct &bj){
    // Just for easy reference
    int nai = bi.NumOfAtoms();
    int naj = bj.NumOfAtoms();
    int noi = bi.NumOfOrbitals();
    int noj = bj.NumOfOrbitals();

    // Most of the matrix elements are zeros. So, we'll only change the
    // non zero elements below.
    hmat = zeros<T>(noi, noj);
    smat = zeros<T>(noi, noj);

    // Lets find the neighbors.
    int io = 0;
    for(int ia = 0; ia != nai; ++ia){
        AtomicStruct atomi = bi(ia);        // extract atom ia
        int ni = atomi.NumOfOrbitals();     // number of orbitals in atom ia

        int jo = 0;
        for(int ja = 0; ja != naj; ++ja){
            AtomicStruct atomj = bj(ja);        // extract atom ja
            int nj = atomj.NumOfOrbitals();     // number of orbitals in atom ja
            // generate Hamiltonian matrix between orbitals of
            // atom i and atom j
            hmat(span(io,io+ni-1), span(jo,jo+nj-1)) = p.twoAtomHam(atomi, atomj);
            smat(span(io,io+ni-1), span(jo,jo+nj-1)) = p.twoAtomOvl(atomi, atomj);
            jo += nj;
        }

        io += ni;
    }
}

typedef HamParams<cxmat>  cxhamparams;
typedef HamParams<mat>    hamparams;

}
}
#endif	/* HAMILTONIAN_HPP */


#include "window.hpp"
#include <iostream>
#include <random>
#include <boost/program_options.hpp>

#include "classes.hpp"
#include "kruskal.hpp"
#include "prim.hpp"
#include "helperFunctions.hpp"

std::once_flag algorithmEnd;

namespace po = boost::program_options;

struct MinimumSpanningTreeConfig{
    std::string algorithm{""};
	int nodes{0};
    int delay{0};
    std::string input_path{""};
    bool interactive{false};
};

struct MinimumSpanningTreeOptions
{
	po::options_description	options;
	po::variables_map vm;

	MinimumSpanningTreeConfig ca;

	MinimumSpanningTreeOptions(int argc, char** argv)
        : options()
    {
		options.add_options()
		("help,h", "display this help.")
		("algorithm,a", po::value<std::string>(&ca.algorithm)->default_value("kruskal"), "chosen algorithm name")
		("nodes,n", po::value<int>(&ca.nodes)->default_value(50), "number of nodes")
		("delay,d", po::value<int>(&ca.delay)->default_value(100), "animation delay")
		("path,p", po::value<std::string>(&ca.input_path)->default_value(""), "input path")
		("interactive,i", "enable interactive mode")
		;

		po::variables_map tmp_vm;
		po::store(po::parse_command_line(argc, argv, options), tmp_vm);
		po::notify(tmp_vm);
		if (tmp_vm.count("help")) {
			std::cout << options << '\n';
			exit(0);
		}

		if (tmp_vm.count("interactive")) {
            ca.interactive = true;
		}
		vm = tmp_vm;
	}
};

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

    std::string algorithm = opt.ca.algorithm;
    int nodes_value  = opt.ca.nodes;
    int delay = opt.ca.delay;
    std::string input_path = opt.ca.input_path;
    bool is_interactive = opt.ca.interactive;

    FastGui::Window w(800, 800);
    w.setScreenColor(0,0,0);
    w.setDelay(delay);

    std::cout << "Input algorithm option value=" << algorithm << '\n';
    std::cout << "Input nodes number option value=" << nodes_value << '\n';
    std::cout << "Delay value=" << delay << '\n';
    // ------------------------------------

    std::vector<Node*> allNodes;

    if (!is_interactive) {
        if (!input_path.empty())
            readPointsFromFile(input_path, allNodes);
        else {
            // set random number generator
            std::random_device dev;
            std::mt19937 rng(dev());
            std::uniform_int_distribution<std::mt19937::result_type> dist(100, 700);

            // create new nodes at random postions
            for (int i = 1; i <= nodes_value; ++i) {
                int x = dist(rng);
                int y = dist(rng);
                Node* t = new Node(i, x,y);
                allNodes.push_back(t);
            }
            writePointsToFile("../data/inputs/data_" + std::to_string(w.getStartTime()), allNodes);
        }
    }
    else {
        std::cout << "Interactive mode enabled" << '\n';
    }

    Kruskal k(allNodes, w, is_interactive);
    Prim p(allNodes, w, is_interactive);

    if (algorithm == "kruskal")
        w.main_loop(k);
    else if(algorithm == "prim")
        w.main_loop(p);
    else {
        std::cout << "There is no such algorithm!" << '\n';
        w.windowShouldClose();
    }

    return 0;
}


#pragma once

#include <list>
#include <vector>

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

#include "AMDiS_fwd.hpp"
#include "BasisFunction.hpp"
#include "FixVec.hpp"
#include "Flag.hpp"

namespace AMDiS
{
  /**
   * \ingroup Assembler
   *
   * \brief
   * For the assemblage of the system matrix and right hand side vector of the
   * linear system, we have to compute integrals, for example:
   * \f[ \int_{\Omega} f(x)\varphi_i(x) dx \f]
   * For general data A, b, c, and f, these integrals can not be calculated
   * exactly. Quadrature formulas have to be used in order to calculate the
   * integrals approximately. Numerical integration in finite element methods is
   * done by looping over all grid elements and using a quadrature formula on
   * each element.
   */
  class Quadrature
  {
  protected:
    /// Avoids call of default constructor
    Quadrature();

    /** \brief
     * Constructs a Quadrature with name name_ of degree degree_ for dim dim_.
     * The Quadrature has n_points_ quadrature points with barycentric
     * coordinates lambda_ and weights w_. The constructor is protected because
     * access to a Quadrature should be done via \ref provideQuadrature.
     */
    Quadrature(const char* name_,
               int degree_,
               int dim_,
               int n_points_,
               VectorOfFixVecs<DimVec<double>>* lambda_,
               double* w_)
      : name(name_),
        degree(degree_),
        dim(dim_),
        n_points(n_points_),
        lambda(lambda_),
        w(w_)
    {}

  public:
    /// Copy constructor
    Quadrature(const Quadrature&);

    /// Destructor
    virtual ~Quadrature();

    /// Returns a Quadrature for dimension dim exact for degree degree.
    static Quadrature* provideQuadrature(int dim, int degree);

    /** \brief
     * Approximates an integral by the numerical quadrature described by quad;
     * f is a pointer to an AbstractFunction to be integrated, evaluated in
     * barycentric coordinates; the return value is
     * \f[ \sum_{k = 0}^{n_points-1} w[k] * (*f)(lambda[k]) \f]
     * For the approximation of \f$ \int_S f\f$ we have to multiply this value
     * with d!|S| for a simplex S; for a parametric simplex f should be a pointer
     * to a function which calculates
     * \f$ f(\lambda)|det DF_S(\hat{x}(\lambda))| \f$.
     */
    double integrateStdSimplex(std::function<double(DimVec<double>)> f);

    /// Returns \ref name
    std::string getName() const
    {
      return name;
    }

    /// Returns \ref n_points
    int getNumPoints() const
    {
      return n_points;
    }

    /// Returns \ref w[p]
    double getWeight(int p) const
    {
      return w[p];
    }

    /// Returns \ref w.
    double* getWeight() const
    {
      return w;
    }

    /// Returns \ref dim
    int getDim() const
    {
      return dim;
    }

    /// Returns \ref degree
    int getDegree() const
    {
      return degree;
    }

    /** \brief
     * Returns a pointer to a vector storing the values of a doubled valued
     * function at all quadrature points; f is that AbstractFunction
     * , evaluated in barycentric coordinates; if vec is not NULL, the values are
     * stored in this vector, otherwise the values are stored in some static
     * local vector, which is overwritten on the next call
     */
    const double* fAtQp(const std::function<double(DimVec<double>)>& f,
                        double* vec) const ;

    /** \brief
     * Returns a pointer to a vector storing the gradient (with respect to world
     * coordinates) of a double valued function at all quadrature points;
     * grdF is a pointer to a AbstractFunction, evaluated in barycentric
     * coordinates and returning a pointer to a WorldVector storing the gradient;
     * if vec is not NULL, the values are stored in this vector, otherwise the
     * values are stored in some local static vector, which is overwritten on the
     * next call
     */
    const WorldVector<double>* grdFAtQp(const std::function<WorldVector<double>(DimVec<double>)>& grdF,
                                        WorldVector<double>* vec) const;

    /// Returns \ref lambda[a][b] which is the b-th coordinate entry of the a-th
    /// quadrature point
    double getLambda(int a, int b) const
    {
      return (lambda ? (*lambda)[a][b] : 0.0);
    }

    /// Returns \ref lambda[a] which is a DimVec<double> containing the
    /// coordiantes of the a-th quadrature point
    const DimVec<double>& getLambda(int a) const
    {
      return (*lambda)[a];
    }

    /// Returns \ref lambda which is a VectorOfFixvecs<DimVec<double> >.
    VectorOfFixVecs<DimVec<double>>* getLambda() const
    {
      return lambda;
    }

  public:
    /// Maximal number of quadrature points for the different dimensions
    static constexpr int maxNQuadPoints[4] = {0, 10, 61, 64};

  protected:
    /// Name of this Quadrature
    std::string name;

    /// Quadrature is exact of this degree
    int degree;

    /// Quadrature for dimension dim
    int dim;

    /// Number of quadrature points
    int n_points;

    /// Vector of quadrature points given in barycentric coordinates
    VectorOfFixVecs<DimVec<double>>* lambda;

    /// Vector of quadrature weights
    double* w;

  protected:
    /// Initialisation of all static Quadrature objects which will be returned
    /// by \ref provideQuadrature()
    static void initStaticQuadratures();

    /** \name static quadratures, used weights, and barycentric coords
     * \{
     */
    static Quadrature** quad_nd[4];
    static Quadrature* quad_0d[1];
    static Quadrature* quad_1d[20];
    static Quadrature* quad_2d[18];
    static Quadrature* quad_3d[8];

    static VectorOfFixVecs<DimVec<double>>* x_0d;
    static double* w_0d;

    static VectorOfFixVecs<DimVec<double>>* x0_1d;
    static VectorOfFixVecs<DimVec<double>>* x1_1d;
    static VectorOfFixVecs<DimVec<double>>* x2_1d;
    static VectorOfFixVecs<DimVec<double>>* x3_1d;
    static VectorOfFixVecs<DimVec<double>>* x4_1d;
    static VectorOfFixVecs<DimVec<double>>* x5_1d;
    static VectorOfFixVecs<DimVec<double>>* x6_1d;
    static VectorOfFixVecs<DimVec<double>>* x7_1d;
    static VectorOfFixVecs<DimVec<double>>* x8_1d;
    static VectorOfFixVecs<DimVec<double>>* x9_1d;
    static double* w0_1d;
    static double* w1_1d;
    static double* w2_1d;
    static double* w3_1d;
    static double* w4_1d;
    static double* w5_1d;
    static double* w6_1d;
    static double* w7_1d;
    static double* w8_1d;
    static double* w9_1d;

    static VectorOfFixVecs<DimVec<double>>* x1_2d;
    static VectorOfFixVecs<DimVec<double>>* x2_2d;
    static VectorOfFixVecs<DimVec<double>>* x3_2d;
    static VectorOfFixVecs<DimVec<double>>* x4_2d;
    static VectorOfFixVecs<DimVec<double>>* x5_2d;
    static VectorOfFixVecs<DimVec<double>>* x7_2d;
    static VectorOfFixVecs<DimVec<double>>* x8_2d;
    static VectorOfFixVecs<DimVec<double>>* x9_2d;
    static VectorOfFixVecs<DimVec<double>>* x10_2d;
    static VectorOfFixVecs<DimVec<double>>* x11_2d;
    static VectorOfFixVecs<DimVec<double>>* x12_2d;
    static VectorOfFixVecs<DimVec<double>>* x17_2d;
    static double* w1_2d;
    static double* w2_2d;
    static double* w3_2d;
    static double* w4_2d;
    static double* w5_2d;
    static double* w7_2d;
    static double* w8_2d;
    static double* w9_2d;
    static double* w10_2d;
    static double* w11_2d;
    static double* w12_2d;
    static double* w17_2d;

    static VectorOfFixVecs<DimVec<double>>* x1_3d;
    static VectorOfFixVecs<DimVec<double>>* x2_3d;
    static VectorOfFixVecs<DimVec<double>>* x3_3d;
    static VectorOfFixVecs<DimVec<double>>* x4_3d;
    static VectorOfFixVecs<DimVec<double>>* x5_3d;
    static VectorOfFixVecs<DimVec<double>>* x7_3d;
    static double* w1_3d;
    static double* w2_3d;
    static double* w3_3d;
    static double* w4_3d;
    static double* w5_3d;
    static double* w7_3d;

    /** \} */
  };

  /// Pre-compute the values of all basis functions at all quadrature nodes;
  const Flag INIT_PHI=1;

  /// Pre-compute the gradients (with respect to the barycentric coordinates) of
  /// all basis functions at all quadrature nodes
  const Flag INIT_GRD_PHI=2;

  /// pre-compute all 2nd derivatives (with respect to the barycentric
  /// coordinates) of all basis functions at all quadrature nodes;
  const Flag INIT_D2_PHI=4;

  /**
   * \ingroup Integration
   *
   *\brief
   * Often numerical integration involves basis functions, such as the assembling
   * of the system matrix and right hand side, or the integration of finite
   * element functions. Since numerical quadrature involves only the values at
   * the quadrature points and the values of basis functions and its derivatives
   * are the same at these points for all elements of the grid, such routines can
   * be much more efficient, if they can use pre-computed values of the basis
   * functions at the quadrature points. In this case the basis functions do not
   * have to be evaluated for each quadrature point on every element newly.
   * Information that should be pre-computed can be specified by the following
   * symbolic constants:
   * \ref INIT_PHI, \ref INIT_GRD_PHI, \ref INIT_D2_PHI
   */
  class FastQuadrature
  {
  protected:
    /// Constructs a FastQuadrature for the given Quadrature, BasisFunction, and
    /// flag.
    FastQuadrature(BasisFunction* basFcts, Quadrature* quad, Flag flag)
      : init_flag(flag),
        phi(0, 0),
        D2Phi(NULL),
        quadrature(quad),
        basisFunctions(basFcts)
    {}

    /// Copy constructor
    FastQuadrature(const FastQuadrature&);

    /// Extended copy constructor
    FastQuadrature(const FastQuadrature&, const Flag);

    /// Destructor
    virtual ~FastQuadrature();

  public:
    /// Returns a FastQuadrature for the given BasisFunction, Quadrature, and flags.
    static FastQuadrature* provideFastQuadrature(const BasisFunction*,
        const Quadrature&,
        Flag);

    /// inits FastQuadrature like speciefied in flag
    void init(Flag init_flag);

    bool initialized(Flag flag)
    {
      if (flag == INIT_PHI)
        return (num_rows(phi) > 0);

      if (flag == INIT_GRD_PHI)
        return (!grdPhi.empty());

      if (flag == INIT_D2_PHI)
        return (D2Phi != NULL);

      ERROR_EXIT("invalid flag\n");
      return false;
    }

    /// Returns \ref quadrature
    const Quadrature* getQuadrature() const
    {
      return quadrature;
    }

    /// Returns \ref max_points
    int getMaxQuadPoints() const
    {
      return max_points;
    }

    /// Returns (*\ref D2Phi)[q][i][j][m]
    double getSecDer(int q, int i, int j, int m) const;

    /// Returns (*\ref D2Phi)[q]
    const VectorOfFixVecs<DimMat<double>>* getSecDer(int q) const;

    /// Returns (*\ref grdPhi)[q][i][j]
    double getGradient(int q, int i ,int j) const
    {
      return (!grdPhi.empty()) ? grdPhi[q][i][j] : 0.0;
    }

    /// Returns (*\ref grdPhi)[q]
    const std::vector<DenseVector<double>>& getGradient(int q) const
    {
      return grdPhi[q];
    }

    const DenseVector<double>& getGradient(int q, int i) const
    {
      return grdPhi[q][i];
    }

    const DenseMatrix<double>& getPhi() const
    {
      return phi;
    }

    /// Returns \ref phi[q][i]
    double getPhi(int q, int i) const
    {
      return phi[q][i];
    }

    /// Returns \ref quadrature ->integrateStdSimplex(f)
    double integrateStdSimplex(std::function<double(DimVec<double>)> f)
    {
      return quadrature->integrateStdSimplex(f);
    }

    /// Returns \ref quadrature ->getNumPoints()
    int getNumPoints() const
    {
      return quadrature->getNumPoints();
    }

    /// Returns \ref quadrature ->getWeight(p)
    double getWeight(int p) const
    {
      return quadrature->getWeight(p);
    }

    /// Returns \ref quadrature ->getDim()
    int getDim() const
    {
      return quadrature->getDim();
    }

    /// Returns \ref quadrature ->getDegree()
    int getDegree() const
    {
      return quadrature->getDegree();
    }

    /// Returns \ref quadrature ->grdFAtQp(f, vec)
    const WorldVector<double>
    * grdFAtQp(const std::function<WorldVector<double>(DimVec<double>)>& f,
               WorldVector<double>* vec) const
    {
      return quadrature->grdFAtQp(f, vec);
    }

    /// Returns \ref quadrature ->fAtQp(f, vec)
    const double* fAtQp(const std::function<double(DimVec<double>)>& f, double* vec) const
    {
      return quadrature->fAtQp(f, vec);
    }

    /// Returns \ref quadrature ->getLambda(a,b)
    double getLambda(int a,int b) const
    {
      return quadrature->getLambda(a,b);
    }

    /// Returns \ref quadrature ->getLambda(a)
    const DimVec<double>& getLambda(int a) const
    {
      return quadrature->getLambda(a);
    }

    /// Returns \ref basisFunctions
    BasisFunction* getBasisFunctions() const
    {
      return basisFunctions;
    }

  protected:
    /// Specifies which information should be pre-computed. Can be \ref INIT_PHI,
    /// \ref INIT_GRD_PHI, or \ref INIT_D2_PHI
    Flag init_flag;

    /** \brief
     * Matrix storing function values if the flag \ref INIT_PHI is set;
     * phi[i][j] stores the value \ref basisFunctions->phi[j]
     * (quadrature->lambda[i]), 0 <= j < basisFunctions->getNumber()  and
     * 0 <= i < n_points
     */
    DenseMatrix<double> phi;

    /** \brief
     * Matrix storing all gradients (with respect to the barycentric coordinates)
     * if the flag \ref INIT_GRD_PHI is set; grdPhi[i][j][k] stores the value
     * basisFunctions->grdPhi[j](quadrature->lambda[i])[k]
     * for 0 <= j < basisFunctions->getNumber(),
     * 0 <= i < . . . , n_points, and 0 <= k < DIM
     */
    std::vector<std::vector<DenseVector<double>>> grdPhi;

    /** \brief
     * Matrix storing all second derivatives (with respect to the barycentric
     * coordinates) if the flag \ref INIT_D2_PHI is set; D2Phi[i][j][k][l] stores
     * the value basisFunctions->D2Phi[j](quadrature->lambda[i])[k][l]
     * for 0 <= j < basisFunctions->getNumber(),
     * 0 <= i < n_points, and 0 <= k,l < DIM
     */
    MatrixOfFixVecs<DimMat<double>>* D2Phi;

    /// List of all used FastQuadratures
    static std::list<FastQuadrature*> fastQuadList;

    /// Maximal number of quadrature points for all yet initialised FastQuadrature
    /// objects. This value may change after a new initialisation of a
    /// FastQuadrature
    static int max_points;

    /// This FastQuadrature stores values for Quadrature quadrature
    Quadrature* quadrature;

    /// Values stored for basis functions basisFunctions
    BasisFunction* basisFunctions;

  };

} // end namespace AMDiS


/*
 * phase_chain.cpp
 *
 * Example of MPI parallelization with odeint
 *
 * Copyright 2013 Karsten Ahnert
 * Copyright 2013 Mario Mulansky
 * Copyright 2013 Pascal Germroth
 * 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 <iostream>
#include <vector>
#include <boost/random.hpp>
#include <boost/timer/timer.hpp>
//[phase_chain_mpi_header
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/mpi/mpi.hpp>
//]

using namespace std;
using namespace boost::numeric::odeint;
using boost::timer::cpu_timer;
using boost::math::double_constants::pi;

//[phase_chain_state
typedef mpi_state< vector<double> > state_type;
//]

//[phase_chain_mpi_rhs
struct phase_chain_mpi_state
{
    phase_chain_mpi_state( double gamma = 0.5 )
    : m_gamma( gamma ) { }

    void operator()( const state_type &x , state_type &dxdt , double /* t */ ) const
    {
        const size_t M = x().size();
        const bool have_left = x.world.rank() > 0,
                   have_right = x.world.rank() < x.world.size()-1;
        double x_left, x_right;
        boost::mpi::request r_left, r_right;
        if( have_left )
        {
            x.world.isend( x.world.rank()-1 , 0 , x().front() ); // send to x_right
            r_left = x.world.irecv( x.world.rank()-1 , 0 , x_left ); // receive from x().back()
        }
        if( have_right )
        {
            x.world.isend( x.world.rank()+1 , 0 , x().back() ); // send to x_left
            r_right = x.world.irecv( x.world.rank()+1 , 0 , x_right ); // receive from x().front()
        }
        for(size_t m = 1 ; m < M-1 ; ++m)
        {
            dxdt()[m] = coupling_func( x()[m+1] - x()[m] ) +
                        coupling_func( x()[m-1] - x()[m] );
        }
        dxdt()[0] = coupling_func( x()[1] - x()[0] );
        if( have_left )
        {
            r_left.wait();
            dxdt()[0] += coupling_func( x_left - x().front() );
        }
        dxdt()[M-1] = coupling_func( x()[M-2] - x()[M-1] );
        if( have_right )
        {
            r_right.wait();
            dxdt()[M-1] += coupling_func( x_right - x().back() );
        }
    }

    double coupling_func( double x ) const
    {
        return sin( x ) - m_gamma * ( 1.0 - cos( x ) );
    }

    double m_gamma;
};
//]

int main( int argc , char **argv )
{
    //[phase_chain_mpi_init
    boost::mpi::environment env( argc , argv );
    boost::mpi::communicator world;

    const size_t N = 131101;
    vector<double> x;
    if( world.rank() == 0 )
    {
        x.resize( N );
        boost::random::uniform_real_distribution<double> distribution( 0.0 , 2.0*pi );
        boost::random::mt19937 engine( 0 );
        generate( x.begin() , x.end() , boost::bind( distribution , engine ) );
    }

    state_type x_split( world );
    split( x , x_split );
    //]

    cpu_timer timer;
    //[phase_chain_mpi_integrate
    integrate_n_steps( runge_kutta4<state_type>() , phase_chain_mpi_state( 1.2 ) ,
                       x_split , 0.0 , 0.01 , 100 );
    unsplit( x_split , x );
    //]

    if( world.rank() == 0 )
    {
        double run_time = static_cast<double>(timer.elapsed().wall) * 1.0e-9;
        std::cerr << run_time << "s" << std::endl;
        // copy(x.begin(), x.end(), ostream_iterator<double>(cout, "\n"));
    }

    return 0;
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with SGD. We first creates factors and then a data matrix
 * from these factors. THis process ensures that we know the best factorization of the input.
 * We then try to reconstruct the factors using SGD.
 */
#include <iostream>

#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>
#include <util/io.h>

#include <mf/mf.h>

using namespace std;
using namespace mf;
using namespace rg;
using namespace boost::numeric::ublas;

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

int main(int argc, char* argv[]) {
#ifndef NDEBUG
	LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 5000000;
	mf_size_type r = 10;

	// parameters for SGD
	double eps0 = 0.001;
	unsigned epochs = 10;
	mf_size_type testNnz = nnz/100;
	SgdOrder order = SGD_ORDER_WOR;
	typedef SlLoss Loss;
	Loss loss;
//	typedef UpdateAbs<UpdateNzsll> Update;
//	typedef RegularizeAbs<RegularizeSL> Regularize;
//	Update update = Update( (Update::Update()) );
//	Regularize regularize = Regularize( (Regularize::Regularize()) );
	typedef UpdateTruncate<UpdateSl> Update;
	typedef RegularizeTruncate<RegularizeSl> Regularize;
	Update update = Update( (Update::Update()), 0, 100 );
	Regularize regularize = Regularize( (Regularize::Regularize()), 0, 100 );

	// generate original factors by sampling from a uniform[0,1] distribution
	Random32 random; // note: this takes a default seed (not randomized!)
	DenseMatrix wIn(size1, r);
	DenseMatrixCM hIn(r, size2);
	generateRandom(wIn, random, boost::uniform_real<>(0,1));
	generateRandom(hIn, random, boost::uniform_real<>(0,1));

	// generate a sparse matrix by selecting random entries from the generated factors
	// and add small Gaussian noise
	SparseMatrix v;
	generateRandom(v, nnz, wIn, hIn, random);
	//addRandom(v, random, boost::normal_distribution<>(0, 0.1));
	LOG4CXX_INFO(logger, "Data matrix: "
		<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
	v.sort();
	//LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));
	SparseMatrixCM vc;
	copyCm(v, vc);

	// create a test matrix (without noise)
	SparseMatrix vTest;
	generateRandom(vTest, testNnz, wIn, hIn, random);
	LOG4CXX_INFO(logger, "Test matrix: "
		<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

	// generate initial factors by sampling from a uniform[0,1] distribution
	DenseMatrix w(size1, r);
	DenseMatrixCM h(r, size2);
	generateRandom(w, random, boost::uniform_real<>(0,1));
	LOG4CXX_INFO(logger, "Row factors: " << w.size1() << " x " << w.size2());
	generateRandom(h, random, boost::uniform_real<>(0,1));
	LOG4CXX_INFO(logger, "Column factors: " << h.size1() << " x " << h.size2());

	// rescale initial factors to match frobenius norm
//	double l2v = l2(v);
//	double l2wh = sumOfSquares(w, h);
//	double f = std::sqrt(l2wh / l2v);

	// take a small sample and remove empty rows/columns
	ProjectedSparseMatrix Vsample;
	projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
	projectFrequent(Vsample, 0);
	LOG4CXX_INFO(logger, "Sample matrix: "
		<< Vsample.data.size1() << " x " << Vsample.data.size2()
		<< ", " << Vsample.data.nnz() << " nonzeros");

	// initialize the SGD
	Timer t;
	SgdRunner sgdRunner(random);
	SgdJob<Update,Regularize> job(v, w, h, update, regularize, order);
	DecayAuto<Update,Regularize,Loss> decay(job, loss, Vsample, eps0, 8, 0.5, 1.05, false, true);
	Trace trace;

	// run SGD to try to reconstruct the original factors
	t.start();
	sgdRunner.run(job, loss, epochs, decay, trace);
	t.stop();
	LOG4CXX_INFO(logger, "Total time: " << t);

	// write trace to an R file
	LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/sgd-gnmf-trace.R");
	trace.toRfile("/tmp/sgd-gnmf-trace.R", "sgd.gnmf");

	return 0;
}


#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h> //input from range_finder
#include <nav_msgs/Odometry.h> //input from odometry
#include <geometry_msgs/Point.h>
#include <armadillo>
#include <math.h>
#include <stdio.h>

using namespace std;
using namespace arma;

class PathPlan{
private:

  ros::NodeHandle nh_;
  ros::Subscriber range_sub_;
  ros::Subscriber odom_sub_;
  ros::Publisher target_pub_;
  ros::Publisher curr_pub_;

  cube wall_map; // It's a 3d tensor (can create as row, col, slices)

  // for /odom
  double pos_x_, pos_y_, ang_z_; //current robot position and orientation in euler angle
  // for /range_pub
  double dist_north_, dist_east_, dist_south_, dist_west_;

  int goal_reached_;

  // update the map

  void initializeWall();
  void setWall(int x, int y, int direction);
  void removeWall(int x, int y, int direction);
  bool hasWall(int x, int y, int direction);

  // callback functions
  void rangeCallback(const std_msgs::Float32MultiArray& rangeMsg);
  void odomCallback(const nav_msgs::OdometryConstPtr& odomMsg);

  //Path Planning Algorithm
  Mat<int> path_map_;
  vec neighbor_value_ = vec(4, fill::zeros);
  int target_x_, target_y_, target_x_prev_, target_y_prev_;
  bool path_map_initialized_ = false;
  void initializePathMap(); // set all the cell with 1000
  void setNextDestCell(); // based on the current position, find the next heading cell
  int getPathMapValue(int x, int y);

public:

  PathPlan(ros::NodeHandle& nh);
  void spin();

  void checkWall();
  void path_plan_alg();

};


#include <boost/test/unit_test.hpp>
#include "algorithms/data_structures/array/max_subarray_sum.hpp"

BOOST_AUTO_TEST_SUITE(TestMaxSubArraySum)

BOOST_AUTO_TEST_CASE(empty_array)
{
    BOOST_CHECK(0 == Algo::DS::Array::MaxSubarraySum::Calc({}));
}

BOOST_AUTO_TEST_CASE(array_with_one_elem)
{
    BOOST_CHECK(0 == Algo::DS::Array::MaxSubarraySum::Calc({0}));
    BOOST_CHECK(1 == Algo::DS::Array::MaxSubarraySum::Calc({1}));
    BOOST_CHECK(-1 == Algo::DS::Array::MaxSubarraySum::Calc({-1}));
}

BOOST_AUTO_TEST_CASE(array_with_several_elements)
{
    BOOST_CHECK(1 == Algo::DS::Array::MaxSubarraySum::Calc({-2, 1}));
    BOOST_CHECK(-1 == Algo::DS::Array::MaxSubarraySum::Calc({-2, -1}));
    BOOST_CHECK(2 == Algo::DS::Array::MaxSubarraySum::Calc({-1, 2}));
    BOOST_CHECK(6 == Algo::DS::Array::MaxSubarraySum::Calc({0, 2, 4, -1, -1}));
    BOOST_CHECK(7 == Algo::DS::Array::MaxSubarraySum::Calc(
            {-2, -3, 4, -1, -2, 1, 5, -3}));
    BOOST_CHECK(6 == Algo::DS::Array::MaxSubarraySum::Calc(
            {-2, 1, -3, 4, -1, 2, 1, -5, 4}));
}

BOOST_AUTO_TEST_SUITE_END()