#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <exception>
#include <vector>
#include <time.h>
#include <utility>
#include <limits>
#include <tuple>
#include <list>

#include <boost/math/special_functions/prime.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>

//typedegs for graph and edges
typedef adjacency_list < listS, vecS, undirectedS,no_property,
	property < edge_weight_t, int > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef std::pair<int, int> Edge;

//edge with weight
typedef std::tuple<int,int,int> Edge_wW;

// Graph Definitions
//typedef boost::adjacency_list<boost::vecS,boost::vecS,boost::directedS> Graph;
typedef boost::graph_traits<graph_t>::vertex_descriptor Vertex;
typedef boost::graph_traits<graph_t>::edge_descriptor EdgeDes;

// Visitor that throw an exception when finishing the destination vertex
class my_visitor : boost::default_bfs_visitor{
protected:
  Vertex destination_vertex_m;
public:
  my_visitor(Vertex destination_vertex_l)
    : destination_vertex_m(destination_vertex_l) {};

  void initialize_vertex(const Vertex &s, const graph_t &g) const {}
  void discover_vertex(const Vertex &s, const graph_t &g) const {}
  void examine_vertex(const Vertex &s, const graph_t &g) const {}
  void examine_edge(const EdgeDes &e, const graph_t &g) const {}
  void edge_relaxed(const EdgeDes &e, const graph_t &g) const {}
  void edge_not_relaxed(const EdgeDes &e, const graph_t &g) const {}
  void finish_vertex(const Vertex &s, const graph_t &g) const {
    if (destination_vertex_m == s)
      throw(2);
  }
};

//find the prime vertexes
int findPrimes(std::vector<int> &primes, int vertNumb){

		std::ifstream file;
		file.open("primes1.txt");
		int searchingPrimes=true;

		int primesNumb=0;

		std::string str;
		getline(file, str,'\r');

	while(searchingPrimes){

		int numsInLine=8;
		getline(file, str,'\r');

		for(int i=0;i<numsInLine;i++){

			int nextPrime = std::stoi(str.substr(10*i,10+10*i));

			if(nextPrime<=vertNumb){
				primes.push_back(nextPrime);
				primesNumb++;
			}
			else{
				searchingPrimes=false;
				break;
			}

		}
		getline(file, str,'\r');

	}
	file.close();

	return primesNumb;
}

// fills the edge array with all the edges from the node array
void createEdgeArray(std::vector<Edge>* nodes,Edge* edge_array,int* weights, int vertNumb ){

	int count=0;
	for(int i=0;i<vertNumb;i++){
		int size=nodes[i].size();
		for(int j=0;j<size;j++){
			Edge edg=nodes[i].at(j);

			edge_array[count]=Edge(i,edg.first);
			weights[count]=edg.second;
			count++;
		}
	}
}

// reads in and initialize the edges
void initializeEdges(std::vector<Edge>* nodes,std::vector<int> primes, int vertNumb,int arcNumb,int primeNumb,std::ifstream &file){

	try {

		int count=0;

		//edges from the source node to 2 with weithg 0
		nodes[0].push_back(Edge(2,0));
		count++;

		std::string str;

		while (count<arcNumb-primeNumb+1) {

			getline(file, str, ' ');
			int from = std::stoi(str);

			getline(file, str, ' ');
			int to = std::stoi(str);

			getline(file,str);
			int dist=std::stoi(str);

			if(from>to){
				int help=from;
				from = to;
				to = help;
			}

			nodes[from].push_back(Edge(to,dist));

			count++;
		}

		//Edges from all primes except 2 to the sink
		for(int i=1;i<primeNumb;i++){

			int prime=primes.at(i);
			nodes[prime].push_back(Edge(vertNumb-1,0));
		}
	}
	catch (...) {
	}
}

// removes an Edge out of nodes from vertex to parent
void removeEdge(std::vector<Edge>* nodes, int vertex, int parent){

	if(parent<vertex){
		int help=vertex;
		vertex=parent;
		parent=help;
	}

	std::vector<Edge>::iterator it=nodes[vertex].begin();

	for(it;it!=nodes[vertex].end();it++){
		if((*it).first==parent){
			nodes[vertex].erase(it);
			break;
		}
	}
}

// uses boost dijkstra and returns the distance from the graph to the new prime
int dijkstra(graph_t &g,int vertNumb, bool* steinTree, std::vector<Edge>* nodes){

	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	std::vector<vertex_descriptor> p(num_vertices(g));
	std::vector<int> d(num_vertices(g));
	vertex_descriptor s = vertex(0, g);
	my_visitor vis(vertNumb-1);

   	//clock_t tStartd1 = clock();

   	try{
		dijkstra_shortest_paths(g, s,
	                          predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, g))).
	                          distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, g))).
	                          visitor(vis));
	}
	catch(...){
	}

	//std::cout <<"INFORM TIME DIJKSTRA: "<< (double)(clock() - tStartd1) / CLOCKS_PER_SEC << " seconds" << std::endl;

	///////////// remove edges
	//clock_t rem=clock();
	int vertex=vertNumb-1;
	int parent=p[vertex];

	remove_edge(vertex,parent,g);
	removeEdge(nodes,vertex,parent);

	while(!steinTree[parent]){

	 		steinTree[parent]=true;

	 		// create a new Edge from source node to parent node with weight 0
	 		add_edge(0,parent,0,g);

	 		//removes the Edge from vertex to parent
	 		remove_edge(vertex,parent,g);
	 		removeEdge(nodes,vertex,parent);

	 		vertex=parent;
	 		parent=p[parent];
	}

	remove_edge(vertex,parent,g);
	removeEdge(nodes,vertex,parent);

	//std::cout <<"INFORM TIME REMOVE EDGES: "<< (double)(clock() - rem) / CLOCKS_PER_SEC << " seconds" << std::endl;
	//std::cout<< "RESULT Parent  "<< p[vertNumb-1]<<std::endl;

	return d[vertNumb-1];
}

// TODO is not working
void simplifyGraph(graph_t &g,std::vector<Edge>* nodes,bool* steinTree,int vertNumb){
	bool hasEdge[vertNumb]={false};

	for(int i = 1;i<vertNumb;i++){

		std::vector<Edge>::iterator it=nodes[i].begin();
		if(steinTree[i]){

			for(it;it!=nodes[i].end();it++){

				std::cout<<i<<std::endl;
				std::cout<<((*it).first)<<std::endl;  //This gives out some strange values
				std::cout<<(nodes[i].size())<<std::endl;
				if(steinTree[(*it).first]){		//segmentation fault in this line

					std::cout<<"lalala"<<std::endl;
					remove_edge(i,(*it).first,g);
					it=nodes[i].erase(it);

				}
				else{
					std::cout<<"lululu"<<std::endl;
					hasEdge[(*it).first]=true;
				}
				std::cout<<"hehehehe"<<std::endl;
			}
		}
		else{

			std::cout<<"huhuhuhu"<<std::endl;
			for(it;it!=nodes[i].end();it++){
				hasEdge[(*it).first]=true;
			}
		}

		if(nodes[i].size()==0 && !hasEdge){
			std::cout<<"weweweew"<<std::endl;
			remove_vertex(i,g);
		}
	}
}

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

	clock_t c=clock();
	//////// read input arguments

	std::ifstream file;
	file.open(argv[1]);

	if (!file.is_open()) {
			std::cerr << "there is no file of the name \"" << argv[1] << "\" in the directory" << std::endl;
			return 0;
	}
	////////

	// get the Number of vertexes
  	std::string str;
	getline(file, str, ' ');
	int vertNumb = std::stoi(str)+2;

	// get the prime nodes
	std::vector<int> primes;
	int primeNumb=findPrimes(primes,vertNumb-2);

	//vector for the Edges to the nodes, first node is source last node is sink( this is needed for simplify graph)
	std::vector<Edge> nodes[vertNumb];

	// get the Number of arcs
	getline(file, str);
	int arcNumb=std::stoi(str)+primeNumb;

	//the vertexes in the steiner tree
	bool steinTree[vertNumb]={false};
	steinTree[0]=true;
	steinTree[2]=true;

	//////// create the graph
	initializeEdges(nodes, primes, vertNumb, arcNumb, primeNumb,file);

	Edge *edge_array;
	edge_array = (Edge*)malloc((arcNumb)* sizeof(Edge));

	int *weights;
	weights = (int*)malloc((arcNumb)* sizeof(int));

	createEdgeArray(nodes,edge_array,weights,vertNumb);

	graph_t g(edge_array, edge_array + arcNumb, weights, vertNumb);
	////////

	//clock_t cl[primeNumb];
	//cl[0]=clock();

	//weight of steinertree
	long long weightST=0;

	for(int i =1; i<primeNumb;i++){

		weightST= weightST+dijkstra(g, vertNumb, steinTree,nodes);

	/*
	 	if(i%100==0){
	 		simplifyGraph(g,nodes,steinTree,vertNumb);
	 	}

	*/

		//cl[i]=clock();
		//std::cout <<"INFORM TIME STEP: "<<  i<<"   "<< (double)(cl[i] - cl[i-1]) / CLOCKS_PER_SEC << " seconds" << std::endl;

	}

		clock_t tStart5 = clock();
		std::cout <<"INFORM WEIGHT OF STEINER TREE: "<< weightST << std::endl;
		std::cout <<"INFORM TIME: "<< (double)(tStart5 - c) / CLOCKS_PER_SEC << " seconds" << std::endl;

 	return 0;
}


//============================================================================
// Name        : minimacro.cpp
// Author      : wilfeli
// Version     :
// Copyright   : Your copyright notice
// Description : Minimacro model in C++, Ansi-style
//============================================================================

#include <iostream>
#include <fstream>
#include <string>
#include <map>
#include <Eigen/Dense>

#include "W.h"
#include "Parameters.h"
#include "H.h"
#include "F.h"
#include "Market.h"

using namespace std;

void
save_history(W& w, int N, std::string file_path){
	//saves history
	//parameters of the simulation

	std::string file_path_default;
	std::string file_name;

    if (file_path.empty()){
        file_path = file_path_default;
    };

	int i = 0;
	//read current number of iterations
	file_name = file_path + "HF_i.txt";
	std::ifstream in_file(file_name);

	std::string I_NUMBER;

    if (in_file){
        std::getline(in_file, I_NUMBER);
    }else{
        I_NUMBER = "0";
    };
	in_file.close();

	std::ofstream out_file(file_name);
	out_file << std::to_string(std::stoi(I_NUMBER) + 1) << std::endl;
    out_file.close();

	file_name = file_path + "HF_param_" + I_NUMBER + ".txt";
	out_file.open(file_name);

	if (out_file){
		//save number of H
		//save number of F
		out_file << "number of humans: "<< w.NH << std::endl;
		out_file << "number of firms: "<< w.NFGC << std::endl;
		out_file << "F wm length: " << w.param->F_wm_LENGTH << std::endl;
		out_file << "H wm length: " << w.param->H_wm_LENGTH << std::endl;
		out_file << "F type: " << w.Fs.back()->param->opt_TYPE << std::endl;
		out_file << "H type: " << w.Hs.back()->param->opt_TYPE << std::endl;
		out_file << "seed: " << w.param->W_SEED << std::endl;
		out_file << "F forecasting length: " << w.param->F_T_MAX << std::endl;
		out_file << "H forecasting length: " << w.param->H_T_MAX << std::endl;
		out_file << "F production function parameters: " << w.param->F_F_F_theta << std::endl;
		out_file << "H utility function parameters: " << w.param->H_GOAL_T_theta << std::endl;
		out_file << "F mRe Phi: " << w.param->F_mRE_PHI << std::endl;
		out_file << "H mRe Phi: " << w.param->H_mRE_PHI << std::endl;
		out_file << "F QL L: " << w.param->F_QL_L << std::endl;
		out_file << "H QL L: " << w.param->H_QL_L << std::endl;
		out_file << "F Grid: " << w.param->F_GRID << std::endl;
		out_file << "H Grid: " << w.param->H_GRID << std::endl;

		i = 0;

		for (auto a:w.Fs){
			if (a->status > 0.0){
				i++;
			};
		};

		out_file << "End number of F:" << i << std::endl;

	};

	out_file.close();

    Eigen::IOFormat CleanFmt(Eigen::StreamPrecision, 0, ", ", "\n");

	file_name = file_path + "HF_data_" + I_NUMBER + ".txt";
	out_file.open(file_name);
	//save prices of the market
	Eigen::MatrixXd data_all(N, 5);
	data_all = Eigen::MatrixXd::Zero(N, 5);
	//employment, sales on goods market, production, price on labor market,
	//price on goods market

	for (auto a:w.Hs){
		data_all.col(0) += Eigen::VectorXd::Map(a->account->asCHK_q.data(),(long)a->account->asCHK_q.size());
		data_all.col(1) += Eigen::VectorXd::Map(a->account->asGC_q.data(),(long)a->account->asGC_q.size());
	};

	for (auto a:w.Fs){
        if (a->status > 0.0){
            data_all.col(2) += Eigen::VectorXd::Map(a->account->production_q.data(),(long)a->account->production_q.size());
        };
	};

	for (int j = 0; j < N; ++j){
		data_all(j,3) = w.ml->market_price[j]->p;
		data_all(j,4) = w.mc->market_price[j]->p;

	};
	if (out_file){
		out_file << data_all.format(CleanFmt);
	};

//	cout << data_all;
	out_file.close();

	file_name = file_path + "HF_data_all_H_" + I_NUMBER + ".txt";
	Eigen::MatrixXd h_all(N, w.NH);
	i = 0;
	for (auto a:w.Hs){
		h_all.col(i) = Eigen::VectorXd::Map(a->account->g_t.data(),(long)a->account->g_t.size());
		i++;

	};

    out_file.open(file_name);
	if (out_file){
		out_file << h_all.format(CleanFmt);;
	};
	out_file.close();

	file_name = file_path + "HF_data_all_F_" + I_NUMBER + ".txt";
	Eigen::MatrixXd f_all(N, w.NFGC);
	i = 0;
	for (auto a:w.Fs){
		f_all.col(i) = Eigen::VectorXd::Map(a->account->profit.data(),(long)a->account->profit.size());
		i++;

	};
    out_file.open(file_name);
	if (out_file){
		out_file << f_all.format(CleanFmt);
	};
	out_file.close();

};

void
read_init(double& seed_, int& N_, std::map <std::string, std::string> &ini, std::string file_name){
	std::string file_path_default;
	std::string file_name_default;

	//open ini file
	file_name_default = "minimacro.ini";

    if (file_name.empty()){
        file_name = file_name_default;
    };

	std::ifstream in_file(file_name);

	std::string s, key, value;

    while (std::getline( in_file, s )){
        // Extract the key value
        std::string::size_type begin = s.find_first_not_of( " \f\t\v" );
        std::string::size_type end = s.find( '=', begin );
        key = s.substr( begin, end - begin );

        // (No leading or trailing whitespace allowed)
        key.erase( key.find_last_not_of( " \f\t\v" ) + 1 );

        // No blank keys allowed
        if (key.empty()) continue;

        // Extract the value (no leading or trailing whitespace allowed)
        begin = s.find_first_not_of( " \f\n\r\t\v", end + 1 );
        end   = s.find_last_not_of(  " \f\n\r\t\v" ) + 1;

        value = s.substr( begin, end - begin );
        ini[key] = value;

//        std::cout << s << " " << key << " " << value << std::endl;
	};
    in_file.close();

    //set ini values
    N_ = std::stoi(ini["N"]);
    seed_ = std::stoi(ini["seed"]);

};

int main(int argc, char *argv[]) {
    //path to ini file
    //argv[1] path to ini file
    //argv[2] path to save directory

    std::string file_ini_name;
    std::string file_path;
    //path to save directory
    if (argc > 2){
        file_path = argv[2];
    };

    if (argc > 1){
        file_ini_name = argv[1];
    };

	//seed
	double seed = 2013;

    //number of steps
    int N = 100;
    std::map <std::string, std::string> simulation_ini;

    read_init(seed, N, simulation_ini, file_ini_name);

	//create world
	W w(seed);

	w.NH = 10;
	w.NFGC = 3;
	w.NCB = 1;

//	w.param->F_F_F_theta << ?, ?;
//	w.param->H_GOAL_T_theta << ?, ?;

    w.param->SIMULATION_MODE = simulation_ini["SIMULATION_MODE"];
    w.param->F_T_MAX = std::stoi(simulation_ini["F_T_MAX"]);
    w.param->H_T_MAX = std::stoi(simulation_ini["H_T_MAX"]);
    w.param->F_opt_CS_N = std::stoi(simulation_ini["F_opt_CS_N"]);
    w.param->H_opt_CS_N = std::stoi(simulation_ini["H_opt_CS_N"]);

    std::string wm_length;
    wm_length = simulation_ini["F_wm_LENGTH"];
    if (wm_length == "inf"){
        w.param->F_wm_LENGTH = std::numeric_limits<double>::infinity();;
    }else{
        w.param->F_wm_LENGTH = std::stod(simulation_ini["F_wm_LENGTH"]);
    };
    wm_length = simulation_ini["H_wm_LENGTH"];
    if (wm_length == "inf"){
        w.param->H_wm_LENGTH = std::numeric_limits<double>::infinity();;
    }else{
        w.param->H_wm_LENGTH = std::stod(simulation_ini["H_wm_LENGTH"]);
    };

    w.param->F_GRID = simulation_ini["F_GRID"];
    w.param->H_GRID = simulation_ini["H_GRID"];

    //decision type
    w.param->F_opt_TYPE = simulation_ini["F_opt_TYPE"];
    w.param->H_opt_TYPE = simulation_ini["H_opt_TYPE"];

    //QL parameters
    w.param->F_QL_L = std::stod(simulation_ini["F_QL_L"]);
    w.param->H_QL_L = std::stod(simulation_ini["H_QL_L"]);

    //mRe parameters
//    w.param->F_mRE_EPSILON = std::stod(simulation_ini["F_mRE_EPSILON"]);
//    w.param->H_mRE_EPSILON = std::stod(simulation_ini["H_mRE_EPSILON"]);
//    w.param->F_mRE_T = std::stod(simulation_ini["F_mRE_T"]);
//    w.param->H_mRE_T = std::stod(simulation_ini["H_mRE_T"]);
    w.param->F_mRE_PHI = std::stod(simulation_ini["F_mRE_PHI"]);
    w.param->H_mRE_PHI = std::stod(simulation_ini["H_mRE_PHI"]);

    w.param->F_ACCOUNTING_TYPE = simulation_ini["F_ACCOUNTING_TYPE"];

    if (simulation_ini.find("NH") != simulation_ini.end()){
        w.NH = std::stoi(simulation_ini["NH"]);
    };

    if (simulation_ini.find("NFGC") != simulation_ini.end()){
        w.NFGC = std::stoi(simulation_ini["NFGC"]);
    };

    if (simulation_ini.find("NCB") != simulation_ini.end()){
        w.NCB = std::stoi(simulation_ini["NCB"]);
    };

	w.init();

	for (int i=0; i<N; ++i){
		w.step();
//        std::cout << i << " ";
//        w.print_step();
	};

//	w.print_step();

	save_history(w, N, file_path);

	return 0;
};


#pragma once

#include <Eigen/Dense>
#include <algorithm>

// own includes -------------------------------------------------
#include "base/array_buffer.hpp"
#include "h2n_1d.hpp"

namespace boltzmann {
template <typename BASIS, typename NUMERIC_T = double>
class Hermite2Nodal
{
 public:
  typedef NUMERIC_T numeric_t;
  typedef Eigen::Matrix<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> matrix_t;

 public:
  /**
   *
   *
   * @param basis  Hermite basis
   * @param K      max poly. degree
   * @param init   functor to initialize the 1d trafo matrix (initializers can
   * be found in
   * h2n_1d.hpp)
   */
  template <typename INITIALIZER>
  Hermite2Nodal(const BASIS &basis, const int K, const INITIALIZER &init);

  /**
   * @brief Transform from Nodal to Hermite basis.
   *
   * Also see @ref to_nodal ,
   *
   * @param dst Hermite coefficients ordered according to @ref basis.
   * @param src Nodal coefficients, \f$ src(i,j) = f(x_j, y_i) \f$.
   * @param transpose defaults to false, true is ``experimental''
   *
   *
   */
  template <typename MATRIX>
  void to_hermite(numeric_t *dst, const MATRIX &src, bool transpose = false) const;

  template <typename DERIVED, typename DERIVED2>
  void to_hermite(Eigen::DenseBase<DERIVED> &dst,
                  const Eigen::DenseBase<DERIVED2> &src,
                  bool transpose = false) const;

  /**
   * @brief Transform from Hermite to Nodal basis.
   *
   * @param dst Output Nodal coefficients dst(i,j) correpsonds to nodes \f$ x_i,
   * y_j\f$.
   * @param c   Hermite coefficients in @ref basis
   * @param tranpose: defaults to false, true is ``experimental''
   *
   *
   * Internally the Hermite coefficients are arranged like so:
   * @remark{
   *
   *     H[deg(y), deg(x)] = c_(iy, ix)
   * }
   *
   */
  template <typename MATRIX>
  void to_nodal(MATRIX &dst, const numeric_t *c, bool transpose = false) const;

  template <typename DERIVED, typename DERIVED2>
  void to_nodal(Eigen::DenseBase<DERIVED> &dst,
                const Eigen::DenseBase<DERIVED2> &src,
                bool transpose = false) const;

  const matrix_t &get_n2h() const { return N2H_; }
  const matrix_t &get_h2n() const { return H2N_; }

 private:
  std::vector<unsigned int> perm_;
  int K_;

  matrix_t N2H_;
  matrix_t H2N_;
  thread_local static ::ArrayBuffer<> buf_;

  BASIS hbasis_;
};

template <typename BASIS, typename NUMERIC_T>
thread_local ::ArrayBuffer<> Hermite2Nodal<BASIS, NUMERIC_T>::buf_;

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename INITIALIZER>
Hermite2Nodal<BASIS, NUMERIC_T>::Hermite2Nodal(const BASIS &basis,
                                               const int K,
                                               const INITIALIZER &init)
    : K_(K)
    , hbasis_(basis)
{
  buf_.reserve(K * K);
  // initialize the 1d transformation matrix
  init(H2N_, N2H_);

  typedef typename BASIS::elem_t elem_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 0>::type hx_t;
  typedef typename boost::mpl::at_c<typename elem_t::types_t, 1>::type hy_t;

  typename elem_t::Acc::template get<hx_t> get_hx;
  typename elem_t::Acc::template get<hy_t> get_hy;

  // max degree
  unsigned int max_kx =
      get_hx(*std::max_element(basis.begin(),
                               basis.end(),
                               [&](const elem_t &e1, const elem_t &e2) {
                                 return get_hx(e1).get_id().k < get_hx(e2).get_id().k;
                               }))
          .get_id()
          .k;
  unsigned int max_ky =
      get_hy(*std::max_element(basis.begin(),
                               basis.end(),
                               [&](const elem_t &e1, const elem_t &e2) {
                                 return get_hy(e1).get_id().k < get_hy(e2).get_id().k;
                               }))
          .get_id()
          .k;

  assert(max_kx == max_ky);

  unsigned int stride = K;
  perm_.resize(hbasis_.n_dofs());
  // build permutation vector
  unsigned int i = 0;
  for (auto elem = basis.begin(); elem < basis.end(); ++elem, ++i) {
    unsigned int kx = get_hx(*elem).get_id().k;
    unsigned int ky = get_hy(*elem).get_id().k;
    perm_[i] = kx * stride + ky;
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename MATRIX>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_nodal(MATRIX &dst, const numeric_t *c, bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);

  if (!transpose) {
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    //  const auto& T = h2n_.get_matrix();
    const auto &T = H2N_;
    dst = T * TMP * T.transpose();
  } else {
    // ATTENTION: untested!
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    const auto &T = H2N_;
    dst = T.transpose() * TMP * T;
  }
}

// -----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename DERIVED, typename DERIVED2>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_nodal(Eigen::DenseBase<DERIVED> &dst,
                                          const Eigen::DenseBase<DERIVED2> &c,
                                          bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);

  if (!transpose) {
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    //  const auto& T = h2n_.get_matrix();
    const auto &T = H2N_;
    dst = T * TMP * T.transpose();
  } else {
    // ATTENTION: untested!
    TMP.fill(0);
    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      data[perm_[i]] = c[i];
    }
    const auto &T = H2N_;
    dst = T.transpose() * TMP * T;
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename MATRIX>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_hermite(numeric_t *dst, const MATRIX &src, bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);
  if (!transpose) {
    const auto &T = N2H_;
    TMP = T * src * T.transpose();
    // undo permuation
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = TMP.data()[perm_[i]];
    }
  } else {
    // ATTENTION: untested!
    const auto &T = N2H_;
    TMP = T.transpose() * src * T;

    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = data[perm_[i]];
    }
  }
}

// ----------------------------------------------------------------------
template <typename BASIS, typename NUMERIC_T>
template <typename DERIVED, typename DERIVED2>
void
Hermite2Nodal<BASIS, NUMERIC_T>::to_hermite(Eigen::DenseBase<DERIVED> &dst,
                                            const Eigen::DenseBase<DERIVED2> &src,
                                            bool transpose) const
{
  auto TMP = buf_.get<matrix_t>(K_, K_);
  if (!transpose) {
    const auto &T = N2H_;
    TMP = T * src.derived() * T.transpose();
    // undo permuation
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = TMP.data()[perm_[i]];
    }
  } else {
    // ATTENTION: untested!
    const auto &T = N2H_;
    TMP = T.transpose() * src.derived() * T;

    numeric_t *data = TMP.data();
    for (unsigned int i = 0; i < hbasis_.n_dofs(); ++i) {
      dst[i] = data[perm_[i]];
    }
  }
}

}  // end namespace boltzmann


/*
  Copyright 2012-2014 Joshua Nathaniel Pritikin and contributors

  libifa-rpf is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <R.h>
#include <R_ext/BLAS.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "libifa-rpf.h"
#include <Eigen/Core>

#ifndef M_LN2
#define M_LN2           0.693147180559945309417232121458        /* ln(2) */
#endif

#ifndef M_LN_SQRT_PI
#define M_LN_SQRT_PI    0.572364942924700087071713675677        /* log(sqrt(pi))
                                                                   == log(pi)/2 */
#endif

static const double EXP_STABLE_DOMAIN = 35;
static const double SMALLEST_PROB = 6.305116760146989222002e-16;  // exp(-35), need constexpr

static void
irt_rpf_logprob_adapter(const double *spec,
			const double *param, const double *th,
			double *out)
{
  (*librpf_model[(int) spec[RPF_ISpecID]].prob)(spec, param, th, out);

  int numOutcomes = spec[RPF_ISpecOutcomes];
  for (int ox=0; ox < numOutcomes; ox++) {
    out[ox] = log(out[ox]);
  }
}

static double
dotprod(const double *v1, const double *v2, const int len)
{
  double dprod = 0;
  for (int dx=0; dx < len; dx++) {
    dprod += v1[dx] * v2[dx];
  }
  return dprod;
}

static int
hessianIndex(int numParam, int row, int col)
{
  return numParam + row*(row+1)/2 + col;
}

static double antilogit(const double x)
{
    if (x == INFINITY) return 1.0;
    else if(x == -INFINITY) return 0.0;
    else return 1.0 / (1.0 + exp(-x));
}

static int
irt_rpf_1dim_drm_numSpec(const double *spec)
{ return RPF_ISpecCount; }

static int
irt_rpf_1dim_drm_numParam(const double *spec)
{ return 4; }

static void
irt_rpf_1dim_drm_prob(const double *spec,
		      const double *param, const double *th,
		      double *out)
{
  double guessing = param[2];
  double upper = param[3];
  double athb = -param[0] * (th[0] - param[1]);
  if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
  else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
  double pp = guessing + (upper-guessing) / (1 + exp(athb));
  out[0] = 1-pp;
  out[1] = pp;
}

static void
set_deriv_nan(const double *spec, double *out)
{
  int numParam = (*librpf_model[(int) spec[RPF_ISpecID]].numParam)(spec);

  for (int px=0; px < numParam; px++) {
    out[px] = nan("I");
  }
}

static void
irt_rpf_1dim_drm_rescale(const double *spec, double *param, const int *paramMask,
			 const double *mean, const double *cov)
{
  double thresh = param[1] * -param[0];
  if (paramMask[0] >= 0) {
    param[0] *= cov[0];
  }
  if (paramMask[1] >= 0) {
    thresh += param[0] * mean[0];
    param[1] = thresh / -param[0];
  }
}

static int
irt_rpf_mdim_drm_numSpec(const double *spec)
{ return RPF_ISpecCount; }

static int
irt_rpf_mdim_drm_numParam(const double *spec)
{
	if (spec[RPF_ISpecDims] == 0) return 1;
	else return 3 + spec[RPF_ISpecDims];
}

static void
irt_rpf_mdim_drm_paramInfo(const double *spec, const int param,
			   const char **type, double *upper, double *lower)
{
	int numDims = spec[RPF_ISpecDims];
	*upper = nan("unset");
	*lower = nan("unset");
	if (numDims == 0) {
		*type = "intercept";
		return;
	}
	*type = NULL;
	if (param >= 0 && param < numDims) {
		*type = "slope";
		*lower = 1e-6;
	} else if (param == numDims) {
		*type = "intercept";
	} else if (param == numDims+1 || param == numDims+2) {
		*type = "bound";
	}
}

static void
irt_rpf_mdim_drm_prob(const double *spec,
		      const double *param, const double *th,
		      double *out)
{
  int numDims = spec[RPF_ISpecDims];
  double dprod = dotprod(param, th, numDims);
  double diff = param[numDims];
  double athb = -(dprod + diff);
  if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
  else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
  double tmp;
  if (numDims == 0) {
	  tmp = 1/(1+exp(athb));
  } else {
	  const double gg = antilogit(param[numDims+1]);
	  const double uu = antilogit(param[numDims+2]);
	  const double width = uu-gg;
	  if (width < 0) tmp = nan("I");
	  else {
		  tmp = gg + width / (1 + exp(athb));
	  }
  }
  out[0] = 1-tmp;
  out[1] = tmp;
}

static void
irt_rpf_mdim_drm_prob2(const double *spec,
		       const double *param, const double *th,
		       double *out1, double *out2)
{
  int numDims = spec[RPF_ISpecDims];
  double dprod = dotprod(param, th, numDims);
  double diff = param[numDims];
  double athb = -(dprod + diff);
  if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
  else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
  double tmp = 1 / (1 + exp(athb));
  out1[0] = 1-tmp;
  out1[1] = tmp;
  if (numDims) {
	  const double gg = antilogit(param[numDims+1]);
	  const double uu = antilogit(param[numDims+2]);
	  tmp = gg + (uu-gg) * tmp;
  }
  out2[0] = 1-tmp;
  out2[1] = tmp;
}

static void
irt_rpf_mdim_drm_deriv1(const double *spec,
		       const double *param,
		       const double *where,
		       const double *weight, double *out)
{
  const int numDims = spec[RPF_ISpecDims];
  double QP[2];
  double QPstar[2];
  irt_rpf_mdim_drm_prob2(spec, param, where, QPstar, QP);
  const double r1 = weight[1];
  const double r2 = weight[0];
  const double r1_P = r1/QP[1];
  const double r1_P2 = r1/(QP[1] * QP[1]);
  const double r2_Q = r2/QP[0];
  const double r2_Q2 = r2/(QP[0] * QP[0]);
  const double r1_Pr2_Q = (r1_P - r2_Q);
  const double Pstar = QPstar[1];
  const double Pstar2 = Pstar * Pstar;
  const double Pstar3 = Pstar2 * Pstar;
  const double Qstar = QPstar[0];
  if (numDims == 0) {
	  out[0] -= Pstar * Qstar * r1_Pr2_Q;
	  double chunk1 = (Pstar - Pstar2);
	  out[1] -= (r1_P * ((Pstar - 3*Pstar2 + 2*Pstar3)) -
		     r1_P2 * chunk1*chunk1 +
		     r2_Q * ((-Pstar + 3*Pstar2 - 2*Pstar3)) -
		     r2_Q2 * chunk1*chunk1);   // cc^2
	  return;
  }
  const double expgg = param[numDims+1];
  const double expuu = param[numDims+2];
  const double gg = antilogit(expgg);
  const double uu = antilogit(expuu);
  const double difexpgg = gg * (1-gg);
  const double difexpuu = uu * (1-uu);
  const double gm1 = (1.0 - gg);
  const double um1 = (1.0 - uu);
  const double u_1u = uu * um1;
  const double g_1g = gg * gm1;
  const double ugD = (uu-gg);
  for (int dx=0; dx < numDims; dx++) {
    out[dx] -= where[dx] * Pstar * Qstar * ugD * r1_Pr2_Q;
  }
  out[numDims] -= ugD * Pstar * Qstar * r1_Pr2_Q;
  out[numDims+1] -= difexpgg * QPstar[0] * r1_Pr2_Q;
  out[numDims+2] -= difexpuu * QPstar[1] * r1_Pr2_Q;

  int ox = numDims+2;

  for(int ix=0; ix < numDims; ix++) {
    for(int jx=0; jx <= ix; jx++) {
      out[++ox] -= (r1_P * (ugD * where[ix] * where[jx] *
				   (Pstar - 3*Pstar2 + 2*Pstar3)) -
			   r1_P2 * (ugD * where[ix] * (Pstar - Pstar2) *
				    (ugD * where[jx] * (Pstar - Pstar2))) +
			   r2_Q * (ugD * where[ix] * where[jx] *
				   (-Pstar + 3*Pstar2 - 2*Pstar3)) -
			   r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) *
				    (ugD * where[jx] * (-Pstar + Pstar2))));  // aa_k aa_k
    }
  }
  for(int ix=0; ix < numDims; ix++) {
    out[++ox] -= (r1_P * (ugD * where[ix] * (Pstar - 3*Pstar2 + 2*Pstar3)) -
			 r1_P2 * (ugD * where[ix] * (Pstar - Pstar2) *
				  (ugD * (Pstar - Pstar2))) +
			 r2_Q * (ugD * where[ix] * (-Pstar + 3*Pstar2 - 2*Pstar3)) -
			 r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) *
				  (ugD * (-Pstar + Pstar2))));  // cc aa_k
  }
  double chunk1 = ugD * (Pstar - Pstar2);
  out[++ox] -= (r1_P * (ugD * (Pstar - 3*Pstar2 + 2*Pstar3)) -
		r1_P2 * chunk1*chunk1 +
		r2_Q * (ugD * (-Pstar + 3*Pstar2 - 2*Pstar3)) -
		r2_Q2 * chunk1*chunk1);   // cc^2
  for(int ix=0; ix < numDims; ix++) {
	  out[++ox] -= (r1_P * (g_1g * where[ix] * (-Pstar + Pstar2)) -
		  r1_P2 * (ugD * where[ix] * (Pstar - Pstar2)) * g_1g * Qstar +
		  r2_Q * (g_1g * where[ix] * (Pstar - Pstar2)) -
		  r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) ) * g_1g * (Pstar - 1));   // gg aa_k
  }
  out[++ox] -= (r1_P * (g_1g * (-Pstar + Pstar2)) -
		r1_P2 * (ugD * (Pstar - Pstar2)) * g_1g * Qstar +
		r2_Q * (g_1g * (Pstar - Pstar2)) -
		r2_Q2 * (ugD * (-Pstar + Pstar2)) * g_1g * -Qstar);  // gg cc
  out[++ox] -= (r1_P * (g_1g * (2.0*gm1 - 1.0 - 2.0*gm1*Pstar + Pstar)) -
		r1_P2 * (g_1g * (1.0 - Pstar)) * (g_1g * (1.0 - Pstar)) +
		r2_Q * (g_1g * (-2.0*gm1 + 1.0 + 2.0*gm1*Pstar - Pstar)) -
		r2_Q2 * (g_1g * (-1.0 + Pstar)) * (g_1g * (-1.0 + Pstar)));  // gg^2

  for(int ix=0; ix < numDims; ix++) {
    out[++ox] -= (r1_P * (u_1u * where[ix] * (Pstar - Pstar2)) -
		  r1_P2 * (ugD * where[ix] * (Pstar - Pstar2)) * u_1u * Pstar +
		  r2_Q * (u_1u * where[ix] * (-Pstar + Pstar2)) +
		  r2_Q2 * (ugD * where[ix] * (-Pstar + Pstar2) ) * u_1u * Pstar);  // uu aa_k
  }

  out[++ox] -= (r1_P * (u_1u * (Pstar - Pstar2)) -
		r1_P2 * (ugD * (Pstar - Pstar2)) * u_1u * Pstar +
		r2_Q * (u_1u * (-Pstar + Pstar2)) +
		r2_Q2 * (ugD * (-Pstar + Pstar2)) * u_1u * Pstar);  // uu cc

  out[++ox] -= (-r1_P2 * (g_1g * (1.0 - Pstar)) * u_1u * Pstar +
		r2_Q2 * (g_1g * (-1.0 + Pstar)) * u_1u * Pstar);  // uu gg
  out[++ox] -=  (r1_P * (2.0*u_1u*um1*Pstar) - r1_P * (u_1u*Pstar) - r1_P2 *(u_1u*u_1u*Pstar2) -
		 r2_Q * (2.0*u_1u*um1*Pstar) + r2_Q * (u_1u*Pstar) - r2_Q2 *(u_1u*u_1u*Pstar2));  // uu^2
}

static void
irt_rpf_mdim_drm_deriv2(const double *spec,
			const double *param,
			double *out)
{
  int numDims = spec[RPF_ISpecDims];
  if (numDims == 0) return;
  const double *aa = param;
  double gg = param[numDims+1];
  double uu = param[numDims+2];

  for (int dx=0; dx < numDims; dx++) {
    if (aa[dx] < 0) {
      set_deriv_nan(spec, out);
      return;
    }
  }
  if (gg == -INFINITY) {
    out[numDims+1] = nan("I");
  }
  if (uu == INFINITY) {
    out[numDims+2] = nan("I");
  }
  if (gg > uu) {
    out[numDims+1] = nan("I");
    out[numDims+2] = nan("I");
  }
}

static void
irt_rpf_mdim_drm_rescale(const double *spec, double *param, const int *paramMask,
			 const double *mean, const double *cov)
{
  int numDims = spec[RPF_ISpecDims];

  double madj = dotprod(param, mean, numDims);

  for (int d1=0; d1 < numDims; d1++) {
    if (paramMask[d1] < 0) continue;
    param[d1] = dotprod(param+d1, cov + d1 * numDims + d1, numDims-d1);
  }

  param[numDims] += madj;
}

static void
irt_rpf_mdim_drm_dTheta(const double *spec, const double *param,
			const double *where, const double *dir,
			double *grad, double *hess)
{
  int numDims = spec[RPF_ISpecDims];
  double PQ[2];
  double PQstar[2];
  irt_rpf_mdim_drm_prob2(spec, param, where, PQstar, PQ);
  double Pstar = PQstar[0];
  double Qstar = PQstar[1];
  const double *aa = param;
  const double guess = antilogit(param[numDims + 1]);
  const double upper = antilogit(param[numDims + 2]);
  for (int ax=0; ax < numDims; ax++) {
    double piece = dir[ax] * (upper-guess) * aa[ax] * (Pstar * Qstar);
    grad[1] += piece;
    grad[0] -= piece;
    piece = dir[ax] * (2 * (upper - guess) * aa[ax]*aa[ax] * (Qstar * Qstar * Pstar) -
		       (upper - guess) * aa[ax]*aa[ax] * (Pstar * Qstar));
    hess[1] -= piece;
    hess[0] += piece;
  }
}

static void
irt_rpf_1dim_drm_dTheta(const double *spec, const double *param,
			const double *where, const double *dir,
			double *grad, double *hess)
{
  double nparam[4];
  memcpy(nparam, param, sizeof(double) * 4);
  nparam[1] = param[1] * -param[0];
  irt_rpf_mdim_drm_dTheta(spec, nparam, where, dir, grad, hess);
}

static int
irt_rpf_mdim_grm_numSpec(const double *spec)
{ return RPF_ISpecCount; }

static int
irt_rpf_mdim_grm_numParam(const double *spec)
{ return spec[RPF_ISpecOutcomes] + spec[RPF_ISpecDims] - 1; }

static void
irt_rpf_mdim_grm_paramInfo(const double *spec, const int param,
			   const char **type, double *upper, double *lower)
{
	int numDims = spec[RPF_ISpecDims];
	*upper = nan("unset");
	*lower = nan("unset");
	*type = NULL;
	if (param >= 0 && param < numDims) {
		*type = "slope";
		*lower = 1e-6;
	} else {
		*type = "intercept";
	}
}

static void _grm_fix_crazy_stuff(const double *spec, const int numOutcomes, double *out)
{
  int bigk = -1;
  double big = 0;

  for (int bx=0; bx < numOutcomes; bx++) {
    if (out[bx] > big) {
      bigk = bx;
      big = out[bx];
    }
  }

  for (int fx=0; fx < numOutcomes; fx++) {
    if (out[fx] < SMALLEST_PROB) {
      double small = SMALLEST_PROB - out[fx];
      out[bigk] -= small;
      out[fx] += small;
    }
  }
}

static void
irt_rpf_mdim_grm_prob(const double *spec,
		      const double *param, const double *th,
		      double *out)
{
  const int numDims = spec[RPF_ISpecDims];
  const int numOutcomes = spec[RPF_ISpecOutcomes];
  const double *slope = param;
  const double dprod = dotprod(slope, th, numDims);
  const double *kat = param + (int) spec[RPF_ISpecDims];

  double athb = -(dprod + kat[0]);
  if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
  else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
  double tmp = 1 / (1 + exp(athb));
  out[0] = 1-tmp;
  out[1] = tmp;

  for (int kx=2; kx < numOutcomes; kx++) {
	  if (1e-6 + kat[kx-1] >= kat[kx-2]) {
		  for (int ky=0; ky < numOutcomes; ky++) {
			  out[ky] = nan("I");
		  }
		  return;
	  }
    double athb = -(dprod + kat[kx-1]);
    if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
    else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
    double tmp = 1 / (1 + exp(athb));
    out[kx-1] -= tmp;
    out[kx] = tmp;
  }

  for (int kx=0; kx < numOutcomes; kx++) {
    if (out[kx] <= 0) {
      _grm_fix_crazy_stuff(spec, numOutcomes, out);
      return;
    }
  }
}

static void
irt_rpf_mdim_grm_rawprob(const double *spec,
			 const double *param, const double *th,
			 double *out)
{
  int numDims = spec[RPF_ISpecDims];
  const int numOutcomes = spec[RPF_ISpecOutcomes];
  const double dprod = dotprod(param, th, numDims);
  const double *kat = param + (int) spec[RPF_ISpecDims];

  out[0] = 1;
  for (int kx=0; kx < numOutcomes-1; kx++) {
    double athb = -(dprod + kat[kx]);
    if (athb < -EXP_STABLE_DOMAIN) athb = -EXP_STABLE_DOMAIN;
    else if (athb > EXP_STABLE_DOMAIN) athb = EXP_STABLE_DOMAIN;
    double tmp = 1 / (1 + exp(athb));
    out[kx+1] = tmp;
  }
  out[numOutcomes] = 0;
}

// Compare with Cai (2010, p. 54) Appendix B
static void
irt_rpf_mdim_grm_deriv1(const double *spec,
			const double *param,
			const double *where,
			const double *weight, double *out)
{
  int nfact = spec[RPF_ISpecDims];
  int outcomes = spec[RPF_ISpecOutcomes];
  int nzeta = spec[RPF_ISpecOutcomes] - 1;
  Eigen::VectorXd P(nzeta+2);
  Eigen::VectorXd PQfull(nzeta+2);
  irt_rpf_mdim_grm_rawprob(spec, param, where, P.data());
  PQfull[0] = 0;
  PQfull[outcomes] = 0;
  for (int kx=1; kx <= nzeta; kx++) PQfull[kx] = P[kx] * (1-P[kx]);
  for (int jx = 0; jx <= nzeta; jx++) {
    double Pk_1 = P[jx];
    double Pk = P[jx + 1];
    double PQ_1 = PQfull[jx];
    double PQ = PQfull[jx + 1];
    double Pk_1Pk = Pk_1 - Pk;
    if (Pk_1Pk < 1e-10) Pk_1Pk = 1e-10;
    double dif1 = weight[jx] / Pk_1Pk;
    double dif1sq = dif1 / Pk_1Pk;
    if(jx < nzeta) {
      double Pk_p1 = P[jx + 2];
      double PQ_p1 = PQfull[jx + 2];
      double Pk_Pkp1 = Pk - Pk_p1;
      if(Pk_Pkp1 < 1e-10) Pk_Pkp1 = 1e-10;
      double dif2 = weight[jx+1] / Pk_Pkp1;
      double dif2sq = dif2 / Pk_Pkp1;
      out[nfact + jx] += PQ * (dif1 - dif2);  //gradient for intercepts

      int d2base = hessianIndex(nfact + nzeta, nfact+jx, 0);
      // hessian for intercept^2
      double tmp3 = (dif1 - dif2) * (Pk * (1.0 - Pk) * (1.0 - 2.0*Pk));
      double piece1 = (PQ * PQ * (dif1sq + dif2sq) + tmp3);
      out[d2base + nfact + jx] += piece1;
      if (jx < (nzeta - 1)) {
	      // hessian for adjacent intercepts
	      int d2base1 = hessianIndex(nfact + nzeta, nfact+jx+1, nfact + jx);
	      out[d2base1] -= dif2sq * PQ_p1 * PQ;
      }
      double tmp1 = -dif2sq * PQ * (PQ - PQ_p1);
      double tmp2 = dif1sq * PQ * (PQ_1 - PQ);
      for(int kx = 0; kx < nfact; kx++){
	// hessian for slope intercept
	out[d2base + kx] -= (tmp1 + tmp2 - tmp3) * where[kx];
      }
    }
    for(int kx = 0; kx < nfact; kx++) {
      // gradient for slope
      out[kx] -= dif1 * (PQ_1 - PQ) * where[kx];
    }

    Eigen::VectorXd temp(nfact);
    for(int ix = 0; ix < nfact; ix++)
      temp[ix] = PQ_1 * where[ix] - PQ * where[ix];

    int d2x = nfact + nzeta;
    double Pk_adj = (Pk_1 * (1.0 - Pk_1) * (1.0 - 2.0 * Pk_1) -
		     Pk * (1.0 - Pk) * (1.0 - 2.0 * Pk));
    for(int i = 0; i < nfact; i++) {
      for(int j = 0; j <= i; j++) {
	double outer = where[i]*where[j];
	// hessian for slope slope
	out[d2x++] -= (- dif1sq * temp[i] * temp[j] + (dif1 * outer * Pk_adj));
      }
    }
  }
}

static void
irt_rpf_mdim_grm_deriv2(const double *spec,
			const double *param,
			double *out)
{
  int nfact = spec[RPF_ISpecDims];
  int nzeta = spec[RPF_ISpecOutcomes] - 1;
  const double *aa = param;
  for (int dx=0; dx < nfact; dx++) {
    if (aa[dx] < 0) {
      set_deriv_nan(spec, out);
      return;
    }
  }
  for (int zx=0; zx < nzeta-1; zx++) {
    if (param[nfact+zx] < param[nfact+zx+1]) {
      set_deriv_nan(spec, out);
      return;
    }
  }
}

static void
irt_rpf_mdim_grm_dTheta(const double *spec, const double *param,
			const double *where, const double *dir,
			double *grad, double *hess)
{
  int numDims = spec[RPF_ISpecDims];
  int outcomes = spec[RPF_ISpecOutcomes];
  const double *aa = param;
  Eigen::VectorXd P(outcomes+1);
  irt_rpf_mdim_grm_rawprob(spec, param, where, P.data());
  for (int jx=0; jx < numDims; jx++) {
    for (int ix=0; ix < outcomes; ix++) {
      double w1 = P[ix] * (1-P[ix]) * aa[jx];
      double w2 = P[ix+1] * (1-P[ix+1]) * aa[jx];
      grad[ix] += dir[jx] * (w1 - w2);
      hess[ix] += dir[jx] * (aa[jx]*aa[jx] * (2 * P[ix] * (1 - P[ix])*(1 - P[ix]) -
					      P[ix] * (1 - P[ix]) -
					      2 * P[ix+1] * (1 - P[ix+1])*(1 - P[ix+1]) +
					      P[ix+1] * (1 - P[ix+1])));
    }
  }
}

static void
irt_rpf_mdim_grm_rescale(const double *spec, double *param, const int *paramMask,
			 const double *mean, const double *cov)
{
  int numDims = spec[RPF_ISpecDims];
  int nzeta = spec[RPF_ISpecOutcomes] - 1;

  double madj = dotprod(param, mean, numDims);

  for (int d1=0; d1 < numDims; d1++) {
    if (paramMask[d1] < 0) continue;
    param[d1] = dotprod(param+d1, cov + d1 * numDims + d1, numDims-d1);
  }

  for (int tx=0; tx < nzeta; tx++) {
    int px = numDims + tx;
    if (paramMask[px] >= 0) param[px] += madj;
  }
}

static int
irt_rpf_nominal_numSpec(const double *spec)
{
  int outcomes = spec[RPF_ISpecOutcomes];
  int Tlen = (outcomes - 1) * (outcomes - 1);
  return RPF_ISpecCount + 4 * Tlen;
}

static int
irt_rpf_nominal_numParam(const double *spec)
{
	int dims = spec[RPF_ISpecDims];
	if (dims == 0) {
		return spec[RPF_ISpecOutcomes]-1;
	} else {
		return dims + 2 * (spec[RPF_ISpecOutcomes]-1);
	}
}

static void
irt_rpf_nominal_paramInfo(const double *spec, const int param,
			  const char **type, double *upper, double *lower)
{
	int numDims = spec[RPF_ISpecDims];
	const int numOutcomes = spec[RPF_ISpecOutcomes];
	*upper = nan("unset");
	*lower = nan("unset");
	if (numDims == 0) {
		*type = "intercept";
		return;
	}
	*type = NULL;
	if (param >= 0 && param < numDims) {
		*type = "slope";
		*lower = 1e-6;
	} else if (param < numDims + numOutcomes - 1) {
		*type = "slope";
	} else {
		*type = "intercept";
	}
}

static void
_nominal_rawprob1(const double *spec,
		 const double *param, const double *th,
		 double discr, double *ak, double *num, double *maxout)
{
  int numDims = spec[RPF_ISpecDims];
  int numOutcomes = spec[RPF_ISpecOutcomes];
  const double *alpha = param + numDims;
  const double *gamma;
  if (numDims == 0) {
	  // alpha doesn't matter
	  gamma = param + numDims;
  } else {
	  gamma = param + numDims + numOutcomes - 1;
  }
  const double *Ta = spec + RPF_ISpecCount;
  const double *Tc = spec + RPF_ISpecCount + (numOutcomes-1) * (numOutcomes-1);

  double curmax = 1;
  for (int kx=0; kx < numOutcomes; kx++) {
    ak[kx] = 0;
    double ck = 0;
    if (kx) {
      for (int tx=0; tx < numOutcomes-1; tx++) {
	int Tcell = tx * (numOutcomes-1) + kx-1;
	ak[kx] += Ta[Tcell] * alpha[tx];
	ck += Tc[Tcell] * gamma[tx];
      }
    }

    double z = discr * ak[kx] + ck;
    num[kx] = z;
    if (curmax < z) curmax = z;
  }
  *maxout = curmax;
}

static void
_nominal_rawprob2(const double *spec,
		  const double *param, const double *th,
		  double discr, double *ak, double *num)
{
  int numOutcomes = spec[RPF_ISpecOutcomes];
  double maxZ;
  _nominal_rawprob1(spec, param, th, discr, ak, num, &maxZ);

  double recenter = 0;
  if (maxZ > EXP_STABLE_DOMAIN) {
    recenter = maxZ - EXP_STABLE_DOMAIN;
  }

  int Kadj = -1;
  double adj = 0;
  double den = 0;   // not exact because adj not taken into account
  for (int kx=0; kx < numOutcomes; kx++) {
    if (num[kx] == maxZ) Kadj = kx;
    if (num[kx] - recenter < -EXP_STABLE_DOMAIN) {
      num[kx] = 0;
      adj += SMALLEST_PROB;
      continue;
    }
    num[kx] = exp(num[kx] - recenter);
    den += num[kx];
  }
  for (int kx=0; kx < numOutcomes; kx++) {
    if (kx == Kadj) {
      num[kx] = num[kx]/den - adj;
    } else if (num[kx] == 0) {
      num[kx] = SMALLEST_PROB;
    } else {
      num[kx] = num[kx]/den;
    }
  }
}

static void
irt_rpf_nominal_prob(const double *spec,
		     const double *param, const double *th,
		     double *out)
{
  int numOutcomes = spec[RPF_ISpecOutcomes];
  int numDims = spec[RPF_ISpecDims];
  Eigen::VectorXd ak(numOutcomes);
  double discr = dotprod(param, th, numDims);
  _nominal_rawprob2(spec, param, th, discr, ak.data(), out);
}

static void
irt_rpf_nominal_logprob(const double *spec,
			const double *param, const double *th,
			double *out)
{
  int numOutcomes = spec[RPF_ISpecOutcomes];
  int numDims = spec[RPF_ISpecDims];
  Eigen::VectorXd num(numOutcomes);
  Eigen::VectorXd ak(numOutcomes);
  double discr = dotprod(param, th, numDims);
  double maxZ;
  _nominal_rawprob1(spec, param, th, discr, ak.data(), num.data(), &maxZ);
  double den = 0;

  if (maxZ > EXP_STABLE_DOMAIN) {
    den = maxZ;  // not best approx
  } else {
    for (int kx=0; kx < numOutcomes; kx++) {
      if (num[kx] < -EXP_STABLE_DOMAIN) continue;
      den += exp(num[kx]);
    }
    den = log(den);
  }

  for (int kx=0; kx < numOutcomes; kx++) {
    out[kx] = num[kx] - den;
  }
}

static double makeOffterm(const double *dat, const double p, const double aTheta,
			  const int ncat, const int cat)
{
  double ret = 0;
  for (int CAT = 0; CAT < ncat; CAT++) {
    if (CAT == cat) continue;
    ret += dat[CAT] * p * aTheta;
  }
  return(ret);
}

static double makeOffterm2(const double *dat, const double p1, const double p2,
			   const double aTheta, const int ncat, const int cat)
{
  double ret = 0;
  for (int CAT = 0; CAT < ncat; CAT++) {
    if (CAT == cat) continue;
    ret += dat[CAT] * p1 * p2 * aTheta;
  }
  return(ret);
}

static void
irt_rpf_nominal_deriv1(const double *spec,
		       const double *param,
		       const double *where,
		       const double *weight, double *out)
{
  int nfact = spec[RPF_ISpecDims];
  int ncat = spec[RPF_ISpecOutcomes];
  double aTheta = dotprod(param, where, nfact);
  double aTheta2 = aTheta * aTheta;

  Eigen::VectorXd num(ncat);
  Eigen::VectorXd ak(ncat);
  _nominal_rawprob2(spec, param, where, aTheta, ak.data(), num.data());

  Eigen::VectorXd P(ncat);
  Eigen::VectorXd P2(ncat);
  Eigen::VectorXd P3(ncat);
  Eigen::VectorXd ak2(ncat);
  Eigen::VectorXd dat_num(ncat);
  double numsum = 0;
  double numakD = 0;
  double numak2D2 = 0;
  Eigen::VectorXd numakDTheta_numsum(nfact);

  for (int kx=0; kx < ncat; kx++) {
    ak2[kx] = ak[kx] * ak[kx];
    dat_num[kx] = weight[kx]/num[kx];
    numsum += num[kx];
    numakD += num[kx] * ak[kx];
    numak2D2 += num[kx] * ak2[kx];
  }
  double numsum2 = numsum * numsum;

  for (int kx=0; kx < ncat; kx++) {
    P[kx] = num[kx]/numsum;
    P2[kx] = P[kx] * P[kx];
    P3[kx] = P2[kx] * P[kx];
  }

  double sumNumak = dotprod(num.data(), ak.data(), ncat);
  for (int fx=0; fx < nfact; fx++) {
    numakDTheta_numsum[fx] = sumNumak * where[fx] / numsum;
  }

  for (int jx = 0; jx < nfact; jx++) {
    double tmpvec = 0;
    for(int i = 0; i < ncat; i++) {
      tmpvec += dat_num[i] * (ak[i] * where[jx] * P[i] -
			      P[i] * numakDTheta_numsum[jx]) * numsum;
    }
    out[jx] -= tmpvec;
  }
  int dkoffset;
  if (nfact == 0) {
	  dkoffset = 0;
  } else {
	  dkoffset = ncat - 1;
  }
  for(int i = 1; i < ncat; i++) {
	  if (nfact) {
		  double offterm = makeOffterm(weight, P[i], aTheta, ncat, i);
		  double tmpvec = dat_num[i] * (aTheta * P[i] - P2[i] * aTheta) * numsum - offterm;
		  out[nfact + i - 1] -= tmpvec;
	  }
    double offterm2 = makeOffterm(weight, P[i], 1, ncat, i);
    double tmpvec2 = dat_num[i] * (P[i] - P2[i]) * numsum - offterm2;
    out[nfact + dkoffset + i - 1] -= tmpvec2;
  }

  int hessbase = nfact + (ncat-1) + dkoffset;
  int d2ind = 0;
  //a's
  for (int j = 0; j < nfact; j++) {
    for (int k = 0; k <= j; k++) {
      double tmpvec = 0;
      for (int i = 0; i < ncat; i++) {
	tmpvec += dat_num[i] * (ak2[i] * where[j] * where[k] * P[i] -
				ak[i] * where[j] * P[i] * numakDTheta_numsum[k] -
				ak[i] * where[k] * P[i] * numakDTheta_numsum[j] +
				2 * P[i] * numakD * where[j] * numakD * where[k] / numsum2 -
				P[i] * numak2D2 * where[j] * where[k] / numsum) * numsum -
	  dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
	  numsum * ak[i] * where[k] +
	  dat_num[i] * (ak[i] * where[j] * P[i] - P[i] * numakDTheta_numsum[j]) *
	  numakD * where[k];
      }
      out[hessbase + d2ind++] -= tmpvec;
    }
  }
  //a's with ak and d
  for(int k = 1; k < ncat; k++){
    int akrow = hessbase + (nfact+k)*(nfact+k-1)/2;
    int dkrow = hessbase + (nfact+ncat+k-1)*(nfact+ncat+k-2)/2;
    for(int j = 0; j < nfact; j++){
      double tmpvec = 0;
      double tmpvec2 = 0;
      for(int i = 0; i < ncat; i++){
	if(i == k){
	  tmpvec += dat_num[i] * (ak[i]*where[j] * aTheta*P[i] -
				     aTheta*P[i]*numakDTheta_numsum[j] +
				     where[j]*P[i] - 2*ak[i]*where[j]*aTheta*P2[i] +
				     2*aTheta*P2[i]*numakDTheta_numsum[j] -
				     where[j]*P2[i])*numsum -
	    dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*numsum*ak[i]*where[j] +
	    dat_num[i]*(aTheta*P[i] - aTheta*P2[i])*(numakD*where[j]);
	  tmpvec2 += dat_num[i]*(ak[i]*where[j]*P[i] -
				      2*ak[i]*where[j]*P2[i] -
				      P[i]*numakDTheta_numsum[j] +
				      2*P2[i]*numakDTheta_numsum[j])*numsum -
	    dat_num[i]*(P[i] - P2[i])*numsum*ak[i]*where[j] +
	    dat_num[i]*(P[i] - P2[i])*(numakD*where[j]);
	} else {
	  tmpvec += -weight[i]*ak[k]*aTheta*where[j]*P[k] +
	    weight[i]*P[k]*aTheta*numakDTheta_numsum[j] -
	    weight[i]*P[k]*where[j];
	  tmpvec2 += -weight[i]*ak[k]*where[j]*P[k] +
	    weight[i]*P[k]*numakDTheta_numsum[j];
	}
      }
      out[akrow + j] -= tmpvec;
      out[dkrow + j] -= tmpvec2;
    }
  }
  //ak's and d's
  for(int j = 1; j < ncat; j++){
    int akrow = hessbase + (nfact+j)*(nfact+j-1)/2;
    int dkrow = hessbase + (nfact+dkoffset+j)*(nfact+dkoffset+j-1)/2;

    double tmpvec = makeOffterm(weight, P2[j], aTheta2, ncat, j);
    double tmpvec2 = makeOffterm(weight, P[j], aTheta2, ncat, j);
    double offterm = tmpvec - tmpvec2;
    tmpvec = makeOffterm(weight, P2[j], 1, ncat, j);
    tmpvec2 = makeOffterm(weight, P[j], 1, ncat, j);
    double offterm2 = tmpvec - tmpvec2;

    if (nfact) {
	    out[akrow + nfact + j - 1] -=
		    (dat_num[j]*(aTheta2*P[j] - 3*aTheta2*P2[j] +
				 2*aTheta2*P3[j])*numsum - weight[j]/num[j] *
		     (aTheta*P[j] - aTheta*P2[j])*numsum*aTheta + weight[j] *
		     (aTheta*P[j] - aTheta*P2[j])*aTheta + offterm);
    }

    out[dkrow + nfact + dkoffset + j - 1] -=
      (dat_num[j]*(P[j] - 3*P2[j] + 2*P3[j])*numsum - weight[j]/num[j] *
	      (P[j] - P2[j])*numsum + weight[j] *
	      (P[j] - P2[j]) + offterm2);

    for(int i = 1; i < ncat; i++) {
      if(j > i) {
	      if (nfact) {
		      offterm = makeOffterm2(weight, P[j], P[i], aTheta2, ncat, i);
		      tmpvec = dat_num[i] * (-aTheta2*P[i]*P[j] + 2*P2[i] *aTheta2*P[j])*numsum +
			      dat_num[i] * (aTheta*P[i] - P2[i] * aTheta)*aTheta*num[j]+offterm;
		      out[akrow + nfact + i - 1] -= tmpvec;
	      }
	offterm2 = makeOffterm2(weight, P[j], P[i], 1, ncat, i);
	tmpvec2 = dat_num[i] * (-P[i]*P[j] + 2*P2[i] *P[j]) * numsum +
	  dat_num[i] * (P[i] - P2[i]) * num[j] + offterm2;
	out[dkrow + nfact + dkoffset + i - 1] -= tmpvec2;
      }
      if (nfact == 0) continue;
      if (abs(j-i) == 0) {
	tmpvec = makeOffterm(weight, P2[i], aTheta, ncat, i);
	tmpvec2 = makeOffterm(weight, P[i], aTheta, ncat, i);
	offterm = tmpvec - tmpvec2;
	tmpvec = dat_num[i]*(aTheta*P[i] - 3*aTheta*P2[i] +
			     2*aTheta*P3[i]) * numsum - dat_num[i] *
	  (aTheta*P[i] - aTheta*P2[i])*numsum + weight[i] *
	  (P[i] - P2[i])*aTheta + offterm;
	out[dkrow + nfact + i - 1] -= tmpvec;
      } else {
	offterm = makeOffterm2(weight, P[j], P[i], aTheta, ncat, i);
	tmpvec = dat_num[i] * (-aTheta*P[i]*P[j] + 2*P2[i] *aTheta*P[j]) * numsum +
	  dat_num[i] * (P[i] - P2[i]) * aTheta * num[j] + offterm;
	out[dkrow + nfact + i - 1] -= tmpvec;
      }
    }
  }
}

static void
irt_rpf_nominal_deriv2(const double *spec,
		       const double *param,
		       double *out)
{
  int nfact = spec[RPF_ISpecDims];
  int nzeta = spec[RPF_ISpecOutcomes] - 1;
  const double *aa = param;

  for (int dx=0; dx < nfact; dx++) {
    if (aa[dx] < 0) {
      set_deriv_nan(spec, out);
      return;
    }
  }

  int ckoffset = nzeta;
  if (nfact == 0) ckoffset = 0;

  const double *Ta = spec + RPF_ISpecCount;
  const double *Tc = spec + RPF_ISpecCount + nzeta * nzeta;
  const int numParam = irt_rpf_nominal_numParam(spec);
  Eigen::VectorXd rawOut(numParam);
  memcpy(rawOut.data(), out, sizeof(double) * numParam);

  // gradient
  for (int tx=0; tx < nzeta; tx++) {
    double ak1=0;
    double ck1=0;
    for (int kx=0; kx < nzeta; kx++) {
      int Tcell = tx * nzeta + kx;
      ak1 += rawOut[nfact + kx] * Ta[Tcell];
      ck1 += rawOut[nfact + ckoffset + kx] * Tc[Tcell];
    }
    out[nfact + tx] = ak1;
    out[nfact + ckoffset + tx] = ck1;
  }

  // don't need to transform the main a parameters TODO
  double *dmat = Realloc(NULL, 3 * numParam * numParam, double);
  const int hsize = hessianIndex(0, numParam-1, numParam-1);
  {
	  // unpack triangular storage into a full matrix
    int row=0;
    int col=0;
    for (int dx=0; dx <= hsize; dx++) {
      dmat[numParam * col + row] = out[numParam + dx];
      if (row == col) {
	col=0; ++row;
      } else {
	dmat[numParam * row + col] = out[numParam + dx];
	++col;
      }
    }
  }

  double *tmat = dmat + numParam * numParam;
  for (int dx=0; dx < numParam * numParam; dx++) tmat[dx] = 0;
  for (int dx=0; dx < nfact; dx++) {
    tmat[dx * numParam + dx] = 1;
  }
  for (int rx=0; rx < nzeta; rx++) {
    for (int cx=0; cx < nzeta; cx++) {
      tmat[(cx + nfact)*numParam + nfact + rx] = Ta[rx * nzeta + cx];
      tmat[(cx + nfact + ckoffset)*numParam + nfact + ckoffset + rx] = Tc[rx * nzeta + cx];
    }
  }

  double *dest = dmat + 2 * numParam * numParam;

  // It is probably possible to do this more efficiently than dgemm
  // since we know that we only care about the lower triangle.
  // I'm not sure whether this is worth optimizing. TODO

  char normal = 'n';
  char transpose = 't';
  double one = 1;
  double zero = 0;
  F77_CALL(dgemm)(&normal, &normal, &numParam, &numParam, &numParam,
		  &one, tmat, &numParam, dmat, &numParam, &zero, dest, &numParam);
  F77_CALL(dgemm)(&normal, &transpose, &numParam, &numParam, &numParam,
		  &one, dest, &numParam, tmat, &numParam, &zero, dmat, &numParam);

  {
    int row=0;
    int col=0;
    for (int dx=0; dx <= hsize; dx++) {
      out[numParam + dx] = dmat[numParam * col + row];
      if (row == col) {
	col=0; ++row;
      } else {
	++col;
      }
    }
  }

  Free(dmat);
}

static void
irt_rpf_mdim_nrm_dTheta(const double *spec, const double *param,
			const double *where, const double *dir,
			double *grad, double *hess)
{
  int numDims = spec[RPF_ISpecDims];
  int outcomes = spec[RPF_ISpecOutcomes];
  const double *aa = param;
  Eigen::VectorXd num(outcomes);
  Eigen::VectorXd ak(outcomes);
  double discr = dotprod(param, where, numDims);
  _nominal_rawprob2(spec, param, where, discr, ak.data(), num.data());

  double den = 0;
  for (int kx=0; kx < outcomes; kx++) {
    den += num[kx];
  }

  Eigen::VectorXd P(outcomes);
  for (int kx=0; kx < outcomes; kx++) {
    P[kx] = num[kx]/den;
  }

  for(int jx=0; jx < numDims; jx++) {
	  Eigen::VectorXd jak(outcomes);
	  Eigen::VectorXd jak2(outcomes);
    for (int ax=0; ax < outcomes; ax++) {
      jak[ax] = ak[ax] * aa[jx];
      jak2[ax] = jak[ax] * jak[ax];
    }
    double numjak = dotprod(num.data(), jak.data(), outcomes);
    double numjakden2 = numjak / den;
    numjakden2 *= numjakden2;
    double numjak2den = dotprod(num.data(), jak2.data(), outcomes) / den;

    for(int ix=0; ix < outcomes; ix++) {
      grad[ix] += dir[jx] * (ak[ix] * aa[jx] * P[ix] - P[ix] * numjak / den);
      hess[ix] += dir[jx] * (ak[ix]*ak[ix] * aa[jx]*aa[jx] * P[ix] -
			     2 * ak[ix] * aa[jx] * P[ix] * numjak / den +
			     2 * P[ix] * numjakden2 - P[ix] * numjak2den);
    }
  }
}

static void
irt_rpf_mdim_nrm_rescale(const double *spec, double *param, const int *paramMask,
			 const double *mean, const double *cov)
{
  int numDims = spec[RPF_ISpecDims];
  int nzeta = spec[RPF_ISpecOutcomes] - 1;
  double *alpha = param + numDims;
  double *gamma = param + numDims + nzeta;
  const double *Ta  = spec + RPF_ISpecCount;
  const double *Tc  = spec + RPF_ISpecCount + nzeta * nzeta;
  const double *iTc = spec + RPF_ISpecCount + 3 * nzeta * nzeta;

  double madj = dotprod(param, mean, numDims);

  for (int d1=0; d1 < numDims; d1++) {
    if (paramMask[d1] < 0) continue;
    param[d1] = dotprod(param+d1, cov + d1 * numDims + d1, numDims-d1);
  }

  Eigen::VectorXd ak(nzeta);
  ak.setZero();
  Eigen::VectorXd ck(nzeta);
  ck.setZero();

  for (int kx=0; kx < nzeta; kx++) {
    for (int tx=0; tx < nzeta; tx++) {
      int Tcell = tx * nzeta + kx;
      ak[kx] += Ta[Tcell] * alpha[tx];
      ck[kx] += Tc[Tcell] * gamma[tx];
    }
  }

  for (int kx=0; kx < nzeta; kx++) {
    ck[kx] += madj * ak[kx];
  }

  for (int kx=0; kx < nzeta; kx++) {
    int px = numDims + nzeta + kx;
    if (paramMask[px] < 0) continue;

    param[px] = 0;

    for (int tx=0; tx < nzeta; tx++) {
      int Tcell = tx * nzeta + kx;
      param[px] += iTc[Tcell] * ck[tx];
    }
  }
}

//static void noop() {}
static void notimplemented_deriv1(const double *spec,
				  const double *param,
				  const double *where,
				  const double *weight, double *out)
{ error("Not implemented"); }

static void notimplemented_deriv2(const double *spec,
				  const double *param,
				  double *out)
{ error("Not implemented"); }

const struct rpf librpf_model[] = {
  { "drm1-",
    irt_rpf_1dim_drm_numSpec,
    irt_rpf_1dim_drm_numParam,
    irt_rpf_mdim_drm_paramInfo,
    irt_rpf_1dim_drm_prob,
    irt_rpf_logprob_adapter,
    notimplemented_deriv1,
    notimplemented_deriv2,
    irt_rpf_1dim_drm_dTheta,
    irt_rpf_1dim_drm_rescale,
  },
  { "drm",
    irt_rpf_mdim_drm_numSpec,
    irt_rpf_mdim_drm_numParam,
    irt_rpf_mdim_drm_paramInfo,
    irt_rpf_mdim_drm_prob,
    irt_rpf_logprob_adapter,
    irt_rpf_mdim_drm_deriv1,
    irt_rpf_mdim_drm_deriv2,
    irt_rpf_mdim_drm_dTheta,
    irt_rpf_mdim_drm_rescale,
  },
  { "grm",
    irt_rpf_mdim_grm_numSpec,
    irt_rpf_mdim_grm_numParam,
    irt_rpf_mdim_grm_paramInfo,
    irt_rpf_mdim_grm_prob,
    irt_rpf_logprob_adapter,
    irt_rpf_mdim_grm_deriv1,
    irt_rpf_mdim_grm_deriv2,
    irt_rpf_mdim_grm_dTheta,
    irt_rpf_mdim_grm_rescale,
  },
  { "nominal",
    irt_rpf_nominal_numSpec,
    irt_rpf_nominal_numParam,
    irt_rpf_nominal_paramInfo,
    irt_rpf_nominal_prob,
    irt_rpf_nominal_logprob,
    irt_rpf_nominal_deriv1,
    irt_rpf_nominal_deriv2,
    irt_rpf_mdim_nrm_dTheta,
    irt_rpf_mdim_nrm_rescale,
  }
};

const int librpf_numModels = (sizeof(librpf_model) / sizeof(struct rpf));


#include "matrix_product_sparse.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SparseLU>

template<typename T>
using SparseMatrixType = Eigen::SparseMatrix<T>;
template<typename T>
using MatrixType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
template<typename T>
using VectorType = Eigen::Matrix<T, Eigen::Dynamic, 1, Eigen::ColMajor>;
template<typename T>
using VectorTypeT = Eigen::Matrix<T, 1, Eigen::Dynamic, Eigen::RowMajor>;

namespace sparse {
    std::optional<Eigen::SparseMatrix<double>>                             A_real_sparse  = std::nullopt;
    std::optional<Eigen::SparseMatrix<std::complex<double>>>               A_cplx_sparse  = std::nullopt;
    std::optional<Eigen::SparseLU<SparseMatrixType<double>>>               lu_real_sparse = {};
    std::optional<Eigen::SparseLU<SparseMatrixType<std::complex<double>>>> lu_cplx_sparse = {};
    std::optional<Eigen::PartialPivLU<MatrixType<double>>>                 lu_real_dense  = std::nullopt;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>>>   lu_cplx_dense  = std::nullopt;

    void reset() {
        A_real_sparse.reset();
        A_cplx_sparse.reset();
        lu_real_sparse.reset();
        lu_cplx_sparse.reset();
        lu_real_dense.reset();
        lu_cplx_dense.reset();
    }

}

template<typename Scalar, bool sparseLU>
SparseMatrixProduct<Scalar, sparseLU>::~SparseMatrixProduct(){
    sparse::reset();
}

template<typename Scalar, bool sparseLU>
SparseMatrixProduct<Scalar, sparseLU>::SparseMatrixProduct(const Scalar *A_, const int L_, const bool copy_data, const eigutils::eigSetting::Form form_, const eigutils::eigSetting::Side side_)
    : A_ptr(A_), L(L_), form(form_), side(side_) {
    if (copy_data){
        A_stl.resize(static_cast<size_t>(L*L));
        std::copy(A_ptr,A_ptr + static_cast<size_t>(L*L), A_stl.begin());
        A_ptr = A_stl.data();
    }

    if constexpr(sparseLU) {
        Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
        if constexpr(std::is_same_v<Scalar, double>) {
            sparse::A_real_sparse = A_matrix.sparseView();
            sparse::A_real_sparse.value().makeCompressed();
        }
        if constexpr(std::is_same_v<Scalar, std::complex<double>>) {
            sparse::A_cplx_sparse = A_matrix.sparseView();
            sparse::A_cplx_sparse.value().makeCompressed();
        }

    }
    init_profiling();
}

// Function definitions

template<typename Scalar, bool sparseLU>
void SparseMatrixProduct<Scalar, sparseLU>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    std::cout << "A_matrix: \n" << A_matrix << std::endl;
}

template<typename Scalar, bool sparseLU>
void SparseMatrixProduct<Scalar, sparseLU>::FactorOP()

/*  Sparse decomposition
 *  Factors P(A-sigma*I) = LU
 */

{
    if(readyFactorOp) { return; }
    assert(readyShift and "Shift value sigma has not been set.");
    t_factorOp.tic();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);

    Scalar sigma;
    if constexpr(std::is_same_v<Scalar, double>) sigma = sigmaR;
    if constexpr(std::is_same_v<Scalar, std::complex<double>>) sigma = std::complex<double>(sigmaR, sigmaI);
    // Real
    if constexpr(std::is_same_v<Scalar, double> and not sparseLU) {
        sparse::lu_real_dense = Eigen::PartialPivLU<MatrixType<Scalar>>();
        sparse::lu_real_dense.value().compute(A_matrix - sigma * Eigen::MatrixXd::Identity(L, L));
    }
    if constexpr(std::is_same_v<Scalar, double> and sparseLU) {
        //        container::lu_real_sparse = Eigen::SparseLU<SparseMatrixType<Scalar>>();
        sparse::lu_real_sparse.value().compute(sparse::A_real_sparse.value() - sigma * Eigen::MatrixXd::Identity(L, L));
    }
    // Complex
    if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU) {
        sparse::lu_cplx_dense = Eigen::PartialPivLU<MatrixType<Scalar>>();
        sparse::lu_cplx_dense.value().compute(A_matrix - sigma * Eigen::MatrixXd::Identity(L, L));
    }
    if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU) {
        //        container::lu_cplx_sparse = Eigen::SparseLU<SparseMatrixType<Scalar>>();
        sparse::lu_cplx_sparse.value().compute(sparse::A_cplx_sparse.value() - sigma * Eigen::MatrixXcd::Identity(L, L));
    }

    t_factorOp.toc();
    readyFactorOp = true;
    std::cout << "Time Factor Op [ms]: " << std::fixed << std::setprecision(3) << t_factorOp.get_last_time_interval() * 1000 << '\n';
}

template<typename Scalar, bool sparseLU>
void SparseMatrixProduct<Scalar, sparseLU>::MultOPv(Scalar *x_in_ptr, Scalar *x_out_ptr) {
    using namespace eigutils::eigSetting;
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    Eigen::Map<VectorType> x_in(x_in_ptr, L);
    Eigen::Map<VectorType> x_out(x_out_ptr, L);

    switch(side) {
        case Side::R: {
            if constexpr(std::is_same_v<Scalar, double> and not sparseLU)
                x_out.noalias() = sparse::lu_real_dense.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU)
                x_out.noalias() = sparse::lu_cplx_dense.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, double> and sparseLU)
                x_out.noalias() = sparse::lu_real_sparse.value().solve(x_in);
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and sparseLU)
                x_out.noalias() = sparse::lu_cplx_sparse.value().solve(x_in);
            break;
        }
        case Side::L: {
            if constexpr(std::is_same_v<Scalar, double> and not sparseLU)
                x_out.noalias() = x_in * sparse::lu_real_dense.value().inverse();
            else if constexpr(std::is_same_v<Scalar, std::complex<double>> and not sparseLU)
                x_out.noalias() = x_in * sparse::lu_cplx_dense.value().inverse();
            else {
                throw std::runtime_error("Left sided sparse shift invert hasn't been implemented yet...");
            }
            break;
        }
    }
    counter++;
}

template<typename Scalar, bool sparseLU>
void SparseMatrixProduct<Scalar, sparseLU>::MultAx(Scalar *x_in, Scalar *x_out) {
    using namespace eigutils::eigSetting;
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case Form::NONSYMMETRIC:
            switch(side) {
                case Side::R: {
                    using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
                    Eigen::Map<VectorType> x_vec_in(x_in, L);
                    Eigen::Map<VectorType> x_vec_out(x_out, L);
                    if constexpr(not sparseLU) x_vec_out.noalias() = A_matrix * x_vec_in;
                    else if constexpr(std::is_same_v<Scalar,double> and sparseLU) x_vec_out.noalias() = sparse::A_real_sparse.value() * x_vec_in;
                    else if constexpr(std::is_same_v<Scalar,std::complex<double>> and sparseLU) x_vec_out.noalias() = sparse::A_cplx_sparse.value() * x_vec_in;
                    break;
                }
                case Side::L: {
                    using VectorTypeT = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
                    Eigen::Map<VectorTypeT> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT> x_vec_out(x_out, L);
                    if constexpr(not sparseLU) x_vec_out.noalias() = x_vec_in * A_matrix;
                    else if constexpr(std::is_same_v<Scalar,double> and sparseLU) x_vec_out.noalias() = x_vec_in * sparse::A_real_sparse.value();
                    else if constexpr(std::is_same_v<Scalar,std::complex<double>> and sparseLU) x_vec_out.noalias() = x_vec_in * sparse::A_cplx_sparse.value();
                    break;
                }
            }
            break;
        case Form::SYMMETRIC: {
            using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
            Eigen::Map<VectorType> x_vec_in(x_in, L);
            Eigen::Map<VectorType> x_vec_out(x_out, L);
            if constexpr(not sparseLU) x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Upper>() * x_vec_in;
            if constexpr(std::is_same_v<Scalar,double> and sparseLU) x_vec_out.noalias() = sparse::A_real_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
            if constexpr(std::is_same_v<Scalar,std::complex<double>> and sparseLU) x_vec_out.noalias() = sparse::A_cplx_sparse.value().template selfadjointView<Eigen::Upper>() * x_vec_in;
            break;
        }
    }
    counter++;
}

// Explicit instantiations
template class SparseMatrixProduct<double, true>;
template class SparseMatrixProduct<double, false>;
template class SparseMatrixProduct<std::complex<double>, true>;
template class SparseMatrixProduct<std::complex<double>, false>;


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_StandardSubshellDopplerBroadenedPhotonEnergyDistribution_def.hpp
//! \author Alex Robinson
//! \brief  The standard subshell Doppler broadened photon energy distribution
//!         def.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_STANDARD_SUBSHELL_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_DEF_HPP
#define MONTE_CARLO_STANDARD_SUBSHELL_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_DEF_HPP

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "MonteCarlo_PhotonKinematicsHelpers.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_RandomNumberGenerator.hpp"
#include "Utility_ExplicitTemplateInstantiationMacros.hpp"
#include "Utility_DesignByContract.hpp"

namespace MonteCarlo{

// Constructor
template<typename ComptonProfilePolicy>
StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::StandardSubshellDopplerBroadenedPhotonEnergyDistribution(
		 const Data::SubshellType interaction_subshell,
		 const double subshell_occupancy,
		 const double subshell_binding_energy,
		 const std::shared_ptr<const ComptonProfile>& compton_profile )
  : SubshellDopplerBroadenedPhotonEnergyDistribution(interaction_subshell,
                                                     subshell_occupancy,
                                                     subshell_binding_energy ),
    d_compton_profile( compton_profile )
{
  // Make sure the interaction subshell is valid
  testPrecondition( interaction_subshell != Data::INVALID_SUBSHELL &&
                    interaction_subshell !=Data::UNKNOWN_SUBSHELL );
  // Make sure the subshell occupancy is valid
  testPrecondition( subshell_occupancy > 0.0 );
  // Make sure the subshell binding energy is valid
  testPrecondition( subshell_binding_energy > 0.0 );
  // Make sure the Compton profile is valid
  testPrecondition( compton_profile.get() );
  testPrecondition( ComptonProfilePolicy::isValidProfile( *compton_profile ) );
}

// Evaluate the distribution
/*! \details The electron momentum projection must be in me*c units
 * (a momentum value of me*c kg*m/s is 1.0 in me*c units). The distribution
 * will have units of barns since the unitless momentum is being used.
 */
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluateWithElectronMomentumProjection(
                              const double incoming_energy,
                              const double electron_momentum_projection,
                              const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the electron momentum projection is valid
  testPrecondition( electron_momentum_projection >= -1.0 );
  // Make sure the scattering angle is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  // Calculate the max electron momentum projection
  ComptonProfile::MomentumQuantity max_electron_momentum_projection =
    calculateMaxElectronMomentumProjection( incoming_energy,
                                            this->getSubshellBindingEnergy(),
                                            scattering_angle_cosine )*
    ComptonProfile::MomentumUnit();

  // Evaluate the Compton profile
  ComptonProfile::ProfileQuantity compton_profile_quantity =
    ComptonProfilePolicy::evaluateWithPossibleLimit(
                   *d_compton_profile,
                   electron_momentum_projection*ComptonProfile::MomentumUnit(),
                   max_electron_momentum_projection );

  // Evaluate the cross section
  const double multiplier = this->evaluateMultiplier(incoming_energy,
                                                     scattering_angle_cosine );

  const double relativistic_term = this->evaluateRelativisticTerm(
                                                     incoming_energy,
                                                     scattering_angle_cosine );

  const double cross_section =
    multiplier*relativistic_term*this->getSubshellOccupancy()*
    compton_profile_quantity.value();

  // Make sure the cross section is valid
  testPostcondition( cross_section >= 0.0 );

  return cross_section;
}

// Evaluate the distribution
/*! \details The distribution has units of barns/MeV.
 */
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluateExact(
				   const double incoming_energy,
				   const double outgoing_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  // The evaluated double differential cross section
  double cross_section;

  if( outgoing_energy <= incoming_energy - this->getSubshellBindingEnergy() )
  {
    // Calculate the electron momentum projection
    const ComptonProfile::MomentumQuantity electron_momentum_projection =
      ComptonProfile::MomentumUnit()*
      calculateElectronMomentumProjection( incoming_energy,
                                           outgoing_energy,
                                           scattering_angle_cosine );

    // Evaluate the Compton profile
    ComptonProfile::ProfileQuantity compton_profile_quantity =
      ComptonProfilePolicy::evaluate( *d_compton_profile,
                                      electron_momentum_projection );

    // Evaluate the cross section
    const double multiplier = this->evaluateMultiplierExact(
                                                     incoming_energy,
                                                     outgoing_energy,
                                                     scattering_angle_cosine );

    const double relativistic_term = this->evaluateRelativisticTermExact(
                                                     incoming_energy,
                                                     outgoing_energy,
                                                     scattering_angle_cosine );

    cross_section = multiplier*relativistic_term*this->getSubshellOccupancy()*
      compton_profile_quantity.value();
  }
  else
    cross_section = 0.0;

  // Make sure the cross section is valid
  testPostcondition( cross_section >= 0.0 );

  return cross_section;
}

// Evaluate the PDF with electron momentum projection
/*! \details The electron momentum projection must be in me*c units
 * (a momentum value of me*c kg*m/s is 1.0 in me*c units). The PDF
 * will be unitless since the unitless momentum is being used.
 */
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluatePDFWithElectronMomentumProjection(
                              const double incoming_energy,
                              const double electron_momentum_projection,
                              const double scattering_angle_cosine,
                              const double precision ) const
{
  const double diff_cross_section =
    this->evaluateWithElectronMomentumProjection( incoming_energy,
                                                  electron_momentum_projection,
                                                  scattering_angle_cosine );

  const double integrated_cross_section =
    this->evaluateIntegratedCrossSection( incoming_energy,
                                          scattering_angle_cosine,
                                          precision );

  if( integrated_cross_section > 0.0 )
    return diff_cross_section/integrated_cross_section;
  else
    return 0.0;
}

// Evaluate the PDF
/*! \details The PDF has units of inverse MeV.
 */
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluatePDFExact(
			           const double incoming_energy,
				   const double outgoing_energy,
			           const double scattering_angle_cosine,
                                   const double precision ) const
{
  const double diff_cross_section =
    this->evaluateExact( incoming_energy,
                         outgoing_energy,
                         scattering_angle_cosine );

  const double integrated_cross_section =
    this->evaluateIntegratedCrossSectionExact( incoming_energy,
                                               scattering_angle_cosine,
                                               precision );

  if( integrated_cross_section > 0.0 )
    return diff_cross_section/integrated_cross_section;
  else
    return 0.0;
}

// Evaluate the integrated cross section (b/mu)
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluateIntegratedCrossSection(
					  const double incoming_energy,
					  const double scattering_angle_cosine,
					  const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  boost::function<double (double x)> double_diff_cs_wrapper =
    boost::bind<double>( &StandardSubshellDopplerBroadenedPhotonEnergyDistribution::evaluateWithElectronMomentumProjection,
                         boost::cref( *this ),
                         incoming_energy,
                         _1,
                         scattering_angle_cosine );

  // Get the subshell binding energy
  const double binding_energy = this->getSubshellBindingEnergy();

  // Calculate the max electron momentum projection
  double pz_max =
    calculateMaxElectronMomentumProjection( incoming_energy,
                                            binding_energy,
                                            scattering_angle_cosine );

  // Don't go above the table max (profile will evaluate to zero beyond it)
  pz_max = ComptonProfilePolicy::getUpperLimitOfIntegration(
                               *d_compton_profile,
                               pz_max*ComptonProfile::MomentumUnit() ).value();

  // Calculate the min electron momentum projection
  double pz_min = ComptonProfilePolicy::getLowerLimitOfIntegration(
                               pz_max*ComptonProfile::MomentumUnit() ).value();

  // Calculate the absolute error and the integrated cross section
  double abs_error, diff_cs;

  Utility::GaussKronrodIntegrator<double> quadrature_set( precision );

  if( pz_min < pz_max )
  {
    quadrature_set.integrateAdaptively<15>( double_diff_cs_wrapper,
                                            pz_min,
                                            pz_max,
                                            diff_cs,
                                            abs_error );
  }
  else
  {
    abs_error = 0.0;
    diff_cs = 0.0;
  }

  // Make sure that the differential cross section is valid
  testPostcondition( diff_cs >= 0.0 );

  return diff_cs;
}

// Evaluate the exact integrated cross section (b/mu)
template<typename ComptonProfilePolicy>
double StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::evaluateIntegratedCrossSectionExact(
					  const double incoming_energy,
					  const double scattering_angle_cosine,
					  const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  boost::function<double (double x)> double_diff_cs_wrapper =
    boost::bind<double>( &StandardSubshellDopplerBroadenedPhotonEnergyDistribution::evaluateExact,
                         boost::cref( *this ),
                         incoming_energy,
                         _1,
                         scattering_angle_cosine );

  // Calculate the max energy
  double energy_max = incoming_energy - this->getSubshellBindingEnergy();

  // Calculate the max electron momentum projection
  double pz_max =
    calculateMaxElectronMomentumProjection( incoming_energy,
                                            this->getSubshellBindingEnergy(),
                                            scattering_angle_cosine );

  // Calculate the max table energy
  const double pz_table_max =
    ComptonProfilePolicy::getUpperBoundOfMomentum(*d_compton_profile).value();

  // Don't go above the table max (profile will evaluate to zero beyond it)
  if( pz_max > pz_table_max )
  {
    bool energetically_possible;

    energy_max = calculateDopplerBroadenedEnergy( pz_table_max,
                                                  incoming_energy,
                                                  scattering_angle_cosine,
                                                  energetically_possible );
  }

  // Calculate the absolute error and the integrated cross section
  double abs_error, diff_cs;

  Utility::GaussKronrodIntegrator<double> quadrature_set( precision );

  quadrature_set.integrateAdaptively<15>( double_diff_cs_wrapper,
                                          0.0,
                                          energy_max,
                                          diff_cs,
                                          abs_error );

  // Make sure that the differential cross section is valid
  testPostcondition( diff_cs >= 0.0 );

  return diff_cs;
}

// Sample an outgoing energy from the distribution
template<typename ComptonProfilePolicy>
void StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::sample(
				     const double incoming_energy,
				     const double scattering_angle_cosine,
				     double& outgoing_energy,
				     Data::SubshellType& shell_of_interaction ) const
{
  Counter trial_dummy;

  this->sampleAndRecordTrials( incoming_energy,
			       scattering_angle_cosine,
			       outgoing_energy,
			       shell_of_interaction,
			       trial_dummy );
}

// Sample an outgoing energy and record the number of trials
template<typename ComptonProfilePolicy>
void StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::sampleAndRecordTrials(
                                      const double incoming_energy,
                                      const double scattering_angle_cosine,
                                      double& outgoing_energy,
				      Data::SubshellType& shell_of_interaction,
                                      Counter& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy >= this->getSubshellBindingEnergy() );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  // Calculate the max electron momentum projection
  ComptonProfile::MomentumQuantity pz_max = ComptonProfile::MomentumUnit()*
    calculateMaxElectronMomentumProjection( incoming_energy,
					    this->getSubshellBindingEnergy(),
					    scattering_angle_cosine );

  // Calculate the doppler broadened energy
  bool energetically_possible = false;

  while( !energetically_possible )
  {
    double pz;

    this->sampleMomentumAndRecordTrials( incoming_energy,
                                         scattering_angle_cosine,
                                         pz,
                                         shell_of_interaction,
                                         trials );

    outgoing_energy = calculateDopplerBroadenedEnergy(pz,
						      incoming_energy,
						      scattering_angle_cosine,
						      energetically_possible );
  }

  // An energy of zero is not allowed in the rest of the code
  if( outgoing_energy == 0.0 )
    outgoing_energy = std::numeric_limits<double>::min();

  // Make sure the outgoing energy is valid
  testPostcondition( energetically_possible );
  testPostcondition( outgoing_energy >= 0.0 );
}

// Sample an electron momentum projection and record the number of trials
template<typename ComptonProfilePolicy>
void StandardSubshellDopplerBroadenedPhotonEnergyDistribution<ComptonProfilePolicy>::sampleMomentumAndRecordTrials(
                                      const double incoming_energy,
                                      const double scattering_angle_cosine,
                                      double& electron_momentum_projection,
                                      Data::SubshellType& shell_of_interaction,
                                      Counter& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy >= this->getSubshellBindingEnergy() );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  ++trials;

  // Calculate the max electron momentum projection
  ComptonProfile::MomentumQuantity pz_max = ComptonProfile::MomentumUnit()*
    calculateMaxElectronMomentumProjection( incoming_energy,
					    this->getSubshellBindingEnergy(),
					    scattering_angle_cosine );

  // Calculate the doppler broadened energy
  ComptonProfile::MomentumQuantity pz =
    ComptonProfilePolicy::sample( *d_compton_profile, pz_max );

  electron_momentum_projection = pz.value();

  shell_of_interaction = this->getSubshell();
}

EXTERN_EXPLICIT_TEMPLATE_CLASS_INST( StandardSubshellDopplerBroadenedPhotonEnergyDistribution<FullComptonProfilePolicy> );
EXTERN_EXPLICIT_TEMPLATE_CLASS_INST( StandardSubshellDopplerBroadenedPhotonEnergyDistribution<HalfComptonProfilePolicy> );
EXTERN_EXPLICIT_TEMPLATE_CLASS_INST( StandardSubshellDopplerBroadenedPhotonEnergyDistribution<DoubledHalfComptonProfilePolicy> );

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_STANDARD_SUBSHELL_DOPPLER_BROADENED_PHOTON_ENERGY_DISTRIBUTION_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_StandardSubshellDopplerBroadenedPhotonEnergyDistribution_def.hpp
//---------------------------------------------------------------------------//