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

#ifdef HAVE_CONFIG_H
#	include "config.h"
#endif

#define _USE_MATH_DEFINES

#include <iostream>

#include <boost/math/special_functions/zeta.hpp>

#include "sample.h"
#include "sample_k2p.h"
#include "ccvector.h"
#include "series.h"
#include "invert_matrix.h"
#include "table.h"
#include "emdel.h"

using namespace std;

void sample_k2p_zeta::preallocate(size_t maxa, size_t maxd)
{
	p.resize(maxa+1,maxd+1,0.0);
}

void sample_k2p_zeta::presample_step(const params_type &params, const sequence &seq_a, const sequence &seq_d)
{
	size_t sz_max = std::max(sz_height, sz_width);
	size_t sz_anc = seq_a.size();
	size_t sz_dec = seq_d.size();
	sa = seq_a;
	sd = seq_d;

	const model_type &model = get_model();

	p(0,0) = prob_scale;

	for(size_t d = 1; d <= sz_dec; ++d) {
		p(0,d) = 0.0;
		for(size_t k = d; k > 0; --k)
			p(0,d) += p(0,d-k)*model.p_indel_size[k];
	}

	for(size_t a = 1; a <= sz_anc; ++a) {
		p(a,0) = 0.0;
		for(size_t k = a; k > 0; --k)
			p(a,0) += p(a-k,0)*model.p_indel_size[k];
	}

	for(size_t a=1;a<=sz_anc;++a) {
		for(size_t d=1;d<= sz_dec;++d) {
			double pt = p(a-1,d-1)*model.p_substitution[sa[a-1]][sd[d-1]];

			for(size_t k = d; k > 0; --k)
				pt += p(a,d-k)*model.p_indel_size[k];
			for(size_t k = a; k > 0; --k)
				pt += p(a-k,d)*model.p_indel_size[k];
			p(a,d) = pt;
		}
	}
}

void sample_k2p_zeta::sample_once(std::string &seq_a, std::string &seq_d)
{
	size_t a = sa.size();
	size_t d = sd.size();
	seq_a.clear();
	seq_d.clear();

	const model_type &model = get_model();

	while(a != 0 || d != 0) {
		double u = p(a,d)*myrand.uniform01();
		double t = (a > 0 && d > 0) ? p(a-1,d-1)*model.p_substitution[sa[a-1]][sd[d-1]] : 0.0;
		if(u < t) {
			// match
			a = a-1;
			d = d-1;
			seq_a.append(1, ccNuc[sa[a]]);
			seq_d.append(1, ccNuc[sd[d]]);
		} else {
			for(size_t k = 1; k <= a || k <= d; ++k) {
				if(k <= a) {
					t += p(a-k,d)*model.p_indel_size[k];
					if(u < t) {
						// gap in d
						seq_d.append(k, '-');
						while(k--) seq_a.append(1, ccNuc[sa[--a]]);
						break;
					}
				}
				if(k <= d)
				{
					t += p(a,d-k)*model.p_indel_size[k];
					if(u < t) {
						// gap in a
						seq_a.append(k, '-');
						while(k--) seq_d.append(1, ccNuc[sd[--d]]);
						break;
					}
				}
			}
		}
		//seq_a.append(1,',');
		//seq_d.append(1,',');
	}
	reverse(seq_a.begin(), seq_a.end());
	reverse(seq_d.begin(), seq_d.end());
}

void sample_k2p_geo::preallocate(size_t maxa, size_t maxd)
{
	p.resize(maxa+1,maxd+1,0.0);
}

void sample_k2p_geo::presample_step(const params_type &params, const sequence &seq_a, const sequence &seq_d)
{
	size_t sz_max = std::max(sz_height, sz_width);
	size_t sz_anc = seq_a.size();
	size_t sz_dec = seq_d.size();
	sa = seq_a;
	sd = seq_d;

	const model_type &model = get_model();

	double row_cache = 0.0;
	vector<double> col_cache(sz_dec+1, 0.0);

	p(0,0) = prob_scale;
	for(size_t d = 1; d <= sz_dec; ++d) {
		row_cache *= model.p_extend;
		row_cache += p(0,d-1)*model.p_open;
		p(0,d) = row_cache;
	}

	row_cache = 0.0;
	for(size_t a = 1; a <= sz_anc; ++a) {
		row_cache *= model.p_extend;
		row_cache += p(a-1,0)*model.p_open;
		p(a,0) = row_cache;
	}

	for(size_t a=1;a<=sz_anc;++a) {
		row_cache = 0.0;
		for(size_t d=1;d<= sz_dec;++d) {
			double pt = p(a-1,d-1)*model.p_substitution[sa[a-1]][sd[d-1]];
			row_cache *= model.p_extend;
			row_cache += p(a,d-1)*model.p_open;
			pt += row_cache;
			col_cache[d] *= model.p_extend;
			col_cache[d] += p(a-1,d)*model.p_open;
			pt += col_cache[d];
			p(a,d) = pt;
		}
	}
}

void sample_k2p_geo::sample_once(std::string &seq_a, std::string &seq_d)
{
	size_t a = sa.size();
	size_t d = sd.size();
	seq_a.clear();
	seq_d.clear();

	const model_type &model = get_model();

	while(a != 0 || d != 0) {
		double u = p(a,d)*myrand.uniform01();
		double t = (a > 0 && d > 0) ? p(a-1,d-1)*model.p_substitution[sa[a-1]][sd[d-1]] : 0.0;
		if(u < t) {
			// match
			a = a-1;
			d = d-1;
			seq_a.append(1, ccNuc[sa[a]]);
			seq_d.append(1, ccNuc[sd[d]]);
		} else {
			for(size_t k = 1; k <= a || k <= d; ++k) {
				if(k <= a) {
					t += p(a-k,d)*model.p_indel_size[k];
					if(u < t) {
						// gap in d
						seq_d.append(k, '-');
						while(k--) seq_a.append(1, ccNuc[sa[--a]]);
						break;
					}
				}
				if(k <= d)
				{
					t += p(a,d-k)*model.p_indel_size[k];
					if(u < t) {
						// gap in a
						seq_a.append(k, '-');
						while(k--) seq_d.append(1, ccNuc[sd[--d]]);
						break;
					}
				}
			}
		}
		//seq_a.append(1,',');
		//seq_d.append(1,',');
	}
	reverse(seq_a.begin(), seq_a.end());
	reverse(seq_d.begin(), seq_d.end());
}

// Draw from Zipf distribution, with parameter a > 1.0
// Devroye Luc (1986) Non-uniform random variate generation.
//     Springer-Verlag: Berlin. p551
inline unsigned int rand_zipf(double a)
{
	double b = pow(2.0, a-1.0);
	double x,t;
	do {
	 x = floor(pow(myrand.uniform01(), -1.0/(a-1.0)));
	 t = pow(1.0+1.0/x, a-1.0);
	} while( myrand.uniform01()*x*(t-1.0)*b >= t*(b-1.0));
	return (unsigned int)x;
}

const char g_nuc[] = "ACGT";
const char g_nuc2[] = "ACGT" "GTAC" "TGCA" "CATG";

void gen_sample_k2p_zeta::sample_once(std::string &seq_a, std::string &seq_d)
{
	seq_a.clear();
	seq_d.clear();

	const model_type &model = get_model();
	const double z = get_params()[model_k2p_zeta::pZ];
	while(1) {
		double p = myrand.uniform01();
		if(p < model.p_2h) {
			// M
			unsigned int x = myrand.uniform(4);
			seq_a.append(1, g_nuc[x]);
			p = myrand.uniform01();
			if(p < model.p_match) {
				seq_d.append(1, g_nuc2[x]);
			} else if(p < model.p_match+model.p_ts) {
				seq_d.append(1, g_nuc2[x+4]);
			} else if(p < 1.0 - 0.5*model.p_ts) {
				seq_d.append(1, g_nuc2[x+8]);
			} else {
				seq_d.append(1, g_nuc2[x+12]);
			}
		} else if(p <  model.p_2h+model.p_2g) {
			// U
			unsigned int x;
			do {
				x = rand_zipf(z);
			} while(x > nmax);
			seq_d.append(x, '-');
			while(x--) {
				seq_a.append(1, g_nuc[myrand.uniform(4)]);
			}
		} else if(p <  1.0-model.p_end) {
			// V
			unsigned int x;
			do {
				x = rand_zipf(z);
			} while(x > nmax);
			seq_a.append(x, '-');
			while(x--) {
				seq_d.append(1, g_nuc[myrand.uniform(4)]);
			}
		} else {
			// E
			break;
		}
	}
}

void gen_sample_k2p_geo::sample_once(std::string &seq_a, std::string &seq_d)
{
	seq_a.clear();
	seq_d.clear();

	const model_type &model = get_model();
	const double q = 1.0/get_params()[model_k2p_geo::pQ];
	while(1) {
		double p = myrand.uniform01();
		if(p < model.p_2h) {
			// M
			unsigned int x = myrand.uniform(4);
			seq_a.append(1, g_nuc[x]);
			p = myrand.uniform01();
			if(p < model.p_match) {
				seq_d.append(1, g_nuc2[x]);
			} else if(p < model.p_match+model.p_ts) {
				seq_d.append(1, g_nuc2[x+4]);
			} else if(p < 1.0 - 0.5*model.p_ts) {
				seq_d.append(1, g_nuc2[x+8]);
			} else {
				seq_d.append(1, g_nuc2[x+12]);
			}
		} else if(p <  model.p_2h+model.p_2g) {
			// U
			unsigned int x = myrand.geometric(q);
			seq_d.append(x, '-');
			while(x--) {
				seq_a.append(1, g_nuc[myrand.uniform(4)]);
			}
		} else if(p <  1.0-model.p_end) {
			// V
			unsigned int x = myrand.geometric(q);
			seq_a.append(x, '-');
			while(x--) {
				seq_d.append(1, g_nuc[myrand.uniform(4)]);
			}
		} else {
			// E
			break;
		}
	}
}


/*
 * cg.cpp
 *
 * Author: H. Hofbauer (hhofbaue@cosy.sbg.ac.at), E. Pschernig (epschern@cosy.sbg.ac.at), P. Wild (pwild@cosy.sbg.ac.at)
 *
 * Generates an iris code from iris texture using complex gabor filters with different filter size and wavelength.
 *
 */
#include "version.h"
#include <cstdio>
#include <map>
#include <vector>
#include <string>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time_types.hpp>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

using namespace std;
using namespace cv;

/** no globbing in win32 mode **/
int _CRT_glob = 0;

/** Program modes **/
static const int MODE_MAIN = 1, MODE_HELP = 2;

int FILTER_HEIGHT=31;

/*
 * Print command line usage for this program
 */
void printUsage() {
    printVersion();
	printf("+-----------------------------------------------------------------------------+\n");
	printf("| cg - Iris-code generation (feature extraction) using complex gabor filters  |\n");
	printf("|      with varying wavelengths.                                              |\n");
	printf("|                                                                             |\n");
	printf("| John Daugman. The importance of being random: statistical principles of     |\n");
	printf("| iris recognition. Pattern Recognition 36(2003) 279--291.                    |\n");
	printf("|                                                                             |\n");
	printf("| MODES                                                                       |\n");
	printf("|                                                                             |\n");
    printf("| (# 1) cg iris code extraction from iris textures                            |\n");
    printf("| (# 2) usage                                                                 |\n");
    printf("|                                                                             |\n");
    printf("| ARGUMENTS                                                                   |\n");
    printf("|                                                                             |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| Name | Parameters | # | ? | Description                                     |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("| -i   | infile     | 1 | N | input iris texture (use * as wildcard, all other|\n");
    printf("|      |            |   |   | file parameters may refer to n-th * with ?n)    |\n");
    printf("| -o   | outfile    | 1 | N | output iris code image                          |\n");
//    printf("| -m   | inmaskfile | 1 | Y | noise pixelmask (0: noise, 255: noise-free)     |\n");
//    printf("|      | outmaskfile|   |   | code bitmask (0: noise, 1: noise-free, off)     |\n");
    printf("| -q   |            | 1 | Y | quiet mode on (off)                             |\n");
    printf("| -t   |            | 1 | Y | time progress on (off)                          |\n");
    printf("| -h   |            | 2 | N | prints usage                                    |\n");
    printf("|      |            |   |   |                                                 |\n");
    printf("| -pwl | wavelength | 1 | N | Base wavelength of the gabor filter in pixel.   |\n");
    printf("|      |            |   |   | Size of base filter will then be 2*wl+1.        |\n");
    printf("|      |            |   |   | (default 6)                                     |\n");
    printf("| -pbp | borderpower| 1 | N | Remaining power of the gauss-part of the filter |\n");
    printf("|      |            |   |   | relative to the center (default 0.01).          |\n");
    printf("| -psx | x-samples  | 1 | N | Number of horizontal samples (default 256)      |\n");
    printf("| -x   | width      | 1 | N | Width of input texture (default 512).           |\n");
    printf("| -y   | height     | 1 | N | Height of input texture (default 64).           |\n");
    printf("|      |            |   |   |                                                 |\n");
    printf("| -wf  |            | 1 | N | Write filterbank images                         |\n");
    printf("| -ws  |            | 1 | N | Write image sample with filtered versions       |\n");
    printf("| -wp  |            | 1 | N | Write iris image with extraction points         |\n");
    printf("+------+------------+---+---+-------------------------------------------------+\n");
    printf("|                                                                             |\n");
    printf("| EXAMPLE USAGE                                                               |\n");
    printf("|                                                                             |\n");
    printf("| -i s1.tiff -o s1.png                                                        |\n");
    printf("| -i *.tiff -o ?1.png -q -t                                                   |\n");
    printf("| -i *.tiff -im ?1_mask.png -o ?1_code.png -om ?1_codemask.png -q -t          |\n");
    printf("|                                                                             |\n");
    printf("| AUTHORS                                                                     |\n");
    printf("|                                                                             |\n");
    printf("| Heinz Hofbauer (hhofbaue@cosy.sbg.ac.at)                                    |\n");
    printf("| Elias Pschernig (epschern@cosy.sbg.ac.at)                                   |\n");
    printf("| Peter Wild (pwild@cosy.sbg.ac.at)                                           |\n");
    printf("|                                                                             |\n");
    printf("| COPYRIGHT                                                                   |\n");
    printf("|                                                                             |\n");
    printf("| (C) 2012 All rights reserved. Do not distribute without written permission. |\n");
    printf("+-----------------------------------------------------------------------------+\n");
}

/** --------------- writerstuff for debugging/control --------------- **/

void writeNormMatToFile(const Mat &what, const string &where, const string &txtmessage = "", bool write_sign=false){
    if( write_sign){
        Mat img = ( what >= 0);
        imwrite( where, img);
    } else {
        Mat img(what.rows, what.cols, CV_8U);
        normalize( what, img, 0, 255, NORM_MINMAX);
        imwrite( where, img);
    }
    if( !txtmessage.empty()) cout << txtmessage << " file("<<where<<")"<<endl;
}

void writeExtractionsequence( const Mat &img, const vector<Point> &extractionsequence, bool &writeagain){
    Mat pointoverlay = img.clone();
    for( vector<Point>::const_iterator eit = extractionsequence.begin(); eit != extractionsequence.end(); advance(eit, 1) )
        circle( pointoverlay, *eit, 1, Scalar(255,128,128));
    writeagain = false;
    imwrite( "extractionpointlocation.png", pointoverlay);
    cout << "Extraction point locations written to: extractionpointlocation.png"<<endl;
}
void writeFilterbank( const vector<Mat> &filterbank){
    for( vector<Mat>::const_iterator it = filterbank.begin(); it != filterbank.end(); advance(it,1)){
        string filename = "filter_";
        int idx = distance( filterbank.begin(), it);
        if ( idx % 2 == 0 ) filename+="re";
        else filename+="im";
        filename += boost::lexical_cast<string>(idx/2+1) + "_wl" + boost::lexical_cast<string>((it->cols-1)/2);
        filename += ".png";
        writeNormMatToFile( *it, filename);
        cout << "filterbank written: "<<filename<<endl;
    }
}
/** ------------------------------- image processing functions ------------------------------- **/

Mat extendByWrap (const Mat &src,  int ext_width,  int  ext_height=0){
    CV_Assert(ext_width >=0 && ext_height >= 0);
    int w = src.cols + ext_width;
    int woff = ext_width/2;
    int wrest = ext_width-woff;
    int h = src.rows + ext_height;
    int hoff = ext_height/2;
    int hrest = ext_height-hoff;

    Mat dst(h,w,src.type());
    copyMakeBorder( src, dst, hoff, hrest, woff, wrest, BORDER_WRAP);
    return dst;
}

void featureExtract(Mat &code, Mat &codeMask, const Mat &img, const Mat &mask, const vector<Mat> &filterbank, const vector<Point> &extractionsequence, bool &write_samples){
    CV_Assert(code.size() == Size( filterbank.size()  * extractionsequence.size()/8,1));
    CV_Assert(mask.empty() || codeMask.empty() || codeMask.size() == code.size());
    bool useMask = !mask.empty() && !codeMask.empty();

    int filter_w=0, filter_h=0;
    for( vector<Mat>::const_iterator it=filterbank.begin(); it != filterbank.end(); advance(it,1)){
        if( it->cols > filter_w) filter_w = it->cols;
        if( it->rows > filter_h) filter_h = it->rows;
    }
    Mat extimg = extendByWrap( img, filter_w/* , filter_h */);
    Point extoff(filter_w/2, 0);

    vector<Mat> filteredExtImg;
    for( unsigned int i=0; i < filterbank.size(); ++i){
        filteredExtImg.push_back(extimg.clone());
        filter2D(extimg, filteredExtImg[i], CV_32F, filterbank[i]);
        if( write_samples){
            writeNormMatToFile(filteredExtImg[i], "filtered_ext_fb"+boost::lexical_cast<string>(i)+".png", "sample written:");
            writeNormMatToFile(filteredExtImg[i], "filtered_ext_fb"+boost::lexical_cast<string>(i)+"_bitmap.png", "sample written:", true);
        }
    }
    if( write_samples) writeNormMatToFile(extimg, "filtered_ext_orig.png", "sample written");

    uchar *codeMaskData = NULL;
	if (useMask){
        codeMask.setTo(255);
        codeMaskData = codeMask.data;
    }
	code.setTo(0);
    uchar * codeData = code.data;
    int bitpos = 0;
    float fval;
    for( vector<Mat>::const_iterator fit = filteredExtImg.begin(); fit != filteredExtImg.end(); advance( fit, 1) ){
        for( vector<Point>::const_iterator eit = extractionsequence.begin(); eit != extractionsequence.end(); advance( eit,1) ){
            // true = 1 for sign, 0 is default
            fval = fit->at<float>(*eit + extoff);
            if( fval >= 0) codeData[0] |= 1<<(7-bitpos);
            if( useMask ){
                if( ( fval*fval < 0.000000001 ) ||    // values are very small, discard as possible error
                    ( codeMask.at<uchar>(*eit) > 0 ) )     // texture is masked
                            codeMaskData[0] &= 0xff ^ 1 << (7-bitpos);
            }
            bitpos++;
            if( bitpos == 8){
                codeData++;
                bitpos = 0;
                if (useMask) codeMaskData++;
            }
        }
    }
    write_samples = false; // only write one set
}

/** ------------------------------- commandline functions ------------------------------- **/

/**
 * Parses a command line
 * This routine should be called for parsing command lines for executables.
 * Note, that all options require '-' as prefix and may contain an arbitrary
 * number of optional arguments.
 *
 * cmd: commandline representation
 * argc: number of parameters
 * argv: string array of argument values
 */
void cmdRead(map<string ,vector<string> >& cmd, int argc, char *argv[]){
	for (int i=1; i< argc; i++){
		char * argument = argv[i];
		if (strlen(argument) > 1 && argument[0] == '-' && (argument[1] < '0' || argument[1] > '9')){
			cmd[argument]; // insert
			char * argument2;
			while (i + 1 < argc && (strlen(argument2 = argv[i+1]) <= 1 || argument2[0] != '-'  || (argument2[1] >= '0' && argument2[1] <= '9'))){
				cmd[argument].push_back(argument2);
				i++;
			}
		}
		else {
			CV_Error(CV_StsBadArg,"Invalid command line format");
		}
	}
}

/**
 * Checks, if each command line option is valid, i.e. exists in the options array
 *
 * cmd: commandline representation
 * validOptions: list of valid options separated by pipe (i.e. |) character
 */
void cmdCheckOpts(map<string ,vector<string> >& cmd, const string &validOptions){
	vector<string> tokens;
	const string delimiters = "|";
	string::size_type lastPos = validOptions.find_first_not_of(delimiters,0); // skip delimiters at beginning
	string::size_type pos = validOptions.find_first_of(delimiters, lastPos); // find first non-delimiter
	while (string::npos != pos || string::npos != lastPos){
		tokens.push_back(validOptions.substr(lastPos,pos - lastPos)); // add found token to vector
		lastPos = validOptions.find_first_not_of(delimiters,pos); // skip delimiters
		pos = validOptions.find_first_of(delimiters,lastPos); // find next non-delimiter
	}
	sort(tokens.begin(), tokens.end());
	for (map<string, vector<string> >::iterator it = cmd.begin(); it != cmd.end(); std::advance(it,1)){
		if (!binary_search(tokens.begin(),tokens.end(),it->first)){
			CV_Error(CV_StsBadArg,"Command line parameter '" + it->first + "' not allowed.");
			tokens.clear();
			return;
		}
	}
	tokens.clear();
}

/*
 * Checks, if a specific required option exists in the command line
 *
 * cmd: commandline representation
 * option: option name
 */
void cmdCheckOptExists(map<string ,vector<string> >& cmd, const string &option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it == cmd.end()) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is required, but does not exist.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * size: appropriate number of parameters for the option
 */
void cmdCheckOptSize(map<string ,vector<string> >& cmd, const string &option, const unsigned int size = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it->second.size() != size) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' has unexpected size.");
}

/*
 * Checks, if a specific option has the appropriate number of parameters
 *
 * cmd: commandline representation
 * option: option name
 * min: minimum appropriate number of parameters for the option
 * max: maximum appropriate number of parameters for the option
 */
void cmdCheckOptRange(map<string ,vector<string> >& cmd, string option, unsigned int min = 0, unsigned int max = 1){
	map<string, vector<string> >::iterator it = cmd.find(option);
	unsigned int size = it->second.size();
	if (size < min || size > max) CV_Error(CV_StsBadArg,"Command line parameter '" + option + "' is out of range.");
}

/*
 * Returns the list of parameters for a given option
 *
 * cmd: commandline representation
 * option: name of the option
 */
vector<string> * cmdGetOpt(map<string ,vector<string> >& cmd, const string &option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? &(it->second) : 0;
}

/*
 * Returns number of parameters in an option
 *
 * cmd: commandline representation
 * option: name of the option
 */
unsigned int cmdSizePars(map<string ,vector<string> >& cmd, const string &option){
	map<string, vector<string> >::iterator it = cmd.find(option);
	return (it != cmd.end()) ? it->second.size() : 0;
}

/*
 * Returns a specific parameter type (int) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
int cmdGetParInt(map<string ,vector<string> >& cmd, string option, unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atoi(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (float) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
float cmdGetParFloat(map<string ,vector<string> >& cmd, const string &option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return atof(it->second[param].c_str());
		}
	}
	return 0;
}

/*
 * Returns a specific parameter type (string) given an option and parameter index
 *
 * cmd: commandline representation
 * option: name of option
 * param: name of parameter
 */
string cmdGetPar(map<string ,vector<string> >& cmd, const string &option, const unsigned int param = 0){
	map<string, vector<string> >::iterator it = cmd.find(option);
	if (it != cmd.end()) {
		if (param < it->second.size()) {
			return it->second[param];
		}
	}
	return 0;
}

/** ------------------------------- timing functions ------------------------------- **/

/**
 * Class for handling timing progress information
 */
class Timing{
public:
	/** integer indicating progress with respect tot total **/
	int progress;
	/** total count for progress **/
	int total;

	/*
	 * Default constructor for timing initializing time.
	 * Automatically calls init()
	 *
	 * seconds: update interval in seconds
	 * eraseMode: if true, outputs sends erase characters at each print command
	 */
	Timing(long seconds, bool eraseMode){
		updateInterval = seconds;
		progress = 1;
		total = 100;
		eraseCount=0;
		erase = eraseMode;
		init();
	}

	/*
	 * Destructor
	 */
	~Timing(){}

	/*
	 * Initializes timing variables
	 */
	void init(void){
		start = boost::posix_time::microsec_clock::universal_time();
		lastPrint = start - boost::posix_time::seconds(updateInterval);
	}

	/*
	 * Clears printing (for erase option only)
	 */
	void clear(void){
		string erase(eraseCount,'\r');
		erase.append(eraseCount,' ');
		erase.append(eraseCount,'\r');
		printf("%s",erase.c_str());
		eraseCount = 0;
	}

	/*
	 * Updates current time and returns true, if output should be printed
	 */
	bool update(void){
		current = boost::posix_time::microsec_clock::universal_time();
		return ((current - lastPrint > boost::posix_time::seconds(updateInterval)) || (progress == total));
	}

	/*
	 * Prints timing object to STDOUT
	 */
	void print(void){
		lastPrint = current;
		float percent = 100.f * progress / total;
		boost::posix_time::time_duration passed = (current - start);
		boost::posix_time::time_duration togo = passed * (total - progress) / max(1,progress);
		if (erase) {
			string erase(eraseCount,'\r');
			printf("%s",erase.c_str());
			int newEraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
			if (newEraseCount < eraseCount) {
				string erase(newEraseCount-eraseCount,' ');
				erase.append(newEraseCount-eraseCount,'\r');
				printf("%s",erase.c_str());
			}
			eraseCount = newEraseCount;
		}
		else {
			eraseCount = (progress != total) ? printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03i Remaining ca. %i:%02i:%02i.%03i)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000),togo.hours(),togo.minutes(),togo.seconds(),(int)(togo.total_milliseconds() % 1000)) : printf("Progress ... %3.2f%% (%i/%i Total %i:%02i:%02i.%03d)\n",percent,progress,total,passed.hours(),passed.minutes(),passed.seconds(),(int)(passed.total_milliseconds()%1000));
		}
	}
private:
	long updateInterval;
	boost::posix_time::ptime start;
	boost::posix_time::ptime current;
	boost::posix_time::ptime lastPrint;
	int eraseCount;
	bool erase;
};

/** ------------------------------- file pattern matching functions ------------------------------- **/

/*
 * Formats a given string, such that it can be used as a regular expression
 * I.e. escapes special characters and uses * and ? as wildcards
 *
 * pattern: regular expression path pattern
 * pos: substring starting index
 * n: substring size
 *
 * returning: escaped substring
 */
string patternSubstrRegex(string& pattern, size_t pos, size_t n){
	string result;
	for (size_t i=pos, e=pos+n; i < e; i++ ) {
		char c = pattern[i];
		if ( c == '\\' || c == '.' || c == '+' || c == '[' || c == '{' || c == '|' || c == '(' || c == ')' || c == '^' || c == '$' || c == '}' || c == ']') {
			result.append(1,'\\');
			result.append(1,c);
		}
		else if (c == '*'){
			result.append("([^/\\\\]*)");
		}
		else if (c == '?'){
			result.append("([^/\\\\])");
		}
		else {
			result.append(1,c);
		}
	}
	return result;
}

/*
 * Converts a regular expression path pattern into a list of files matching with this pattern by replacing wildcards
 * starting in position pos assuming that all prior wildcards have been resolved yielding intermediate directory path.
 * I.e. this function appends the files in the specified path according to yet unresolved pattern by recursive calling.
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 * pos: an index such that positions 0...pos-1 of pattern are already considered/matched yielding path
 * path: the current directory (or empty)
 */
void patternToFiles(string& pattern, vector<string>& files, const size_t& pos, const string& path){
	size_t first_unknown = pattern.find_first_of("*?",pos); // find unknown * in pattern
	if (first_unknown != string::npos){
		size_t last_dirpath = pattern.find_last_of("/\\",first_unknown);
		size_t next_dirpath = pattern.find_first_of("/\\",first_unknown);
		if (next_dirpath != string::npos){
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,next_dirpath-last_dirpath-1) : patternSubstrRegex(pattern,pos,next_dirpath-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr( ((path.length() > 0) ? path + pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					if (boost::filesystem::is_directory(itr->path())){
						boost::filesystem::path p = itr->path().filename();
						string s =  p.string();
						if (boost::regex_match(s.c_str(), expr)){
							patternToFiles(pattern,files,(int)(next_dirpath+1),((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
						}
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
		else {
			boost::regex expr((last_dirpath != string::npos && last_dirpath > pos) ? patternSubstrRegex(pattern,last_dirpath+1,pattern.length()-last_dirpath-1) : patternSubstrRegex(pattern,pos,pattern.length()-pos));
			boost::filesystem::directory_iterator end_itr; // default construction yields past-the-end
			try {
				for ( boost::filesystem::directory_iterator itr(((path.length() > 0) ? path +  pattern[pos-1] : (last_dirpath != string::npos && last_dirpath > pos) ? "" : "./") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) : "")); itr != end_itr; ++itr )
				{
					boost::filesystem::path p = itr->path().filename();
					string s =  p.string();
					if (boost::regex_match(s.c_str(), expr)){
						files.push_back(((path.length() > 0) ? path + pattern[pos-1] : "") + ((last_dirpath != string::npos && last_dirpath > pos) ? pattern.substr(pos,last_dirpath-pos) + pattern[last_dirpath] : "") + s);
					}
				}
			}
			catch (boost::filesystem::filesystem_error &e){}
		}
	}
	else { // no unknown symbols
		boost::filesystem::path file(((path.length() > 0) ? path + "/" : "") + pattern.substr(pos,pattern.length()-pos));
		if (boost::filesystem::exists(file)){
			files.push_back(file.string());
		}
	}
}
/**
 * Converts a regular expression path pattern into a list of files matching with this pattern
 *
 * pattern: regular expression path pattern
 * files: the list to which new files can be applied
 */
void patternToFiles(string& pattern, vector<string>& files){
	patternToFiles(pattern,files,0,"");
}

/*
 * Renames a given filename corresponding to the actual file pattern using a renaming pattern.
 * Wildcards can be referred to as ?1, ?2, ... in the order they appeared in the file pattern.
 *
 * pattern: regular expression path pattern
 * renamePattern: renaming pattern using ?1, ?2, ... as placeholders for wildcards
 * infile: path of the file (matching with pattern) to be renamed
 * outfile: path of the renamed file
 * par: used parameter (default: '?')
 */
void patternFileRename(string& pattern, const string& renamePattern, const string& infile, string& outfile, const char par = '?'){
	size_t first_unknown = renamePattern.find_first_of(par,0); // find unknown ? in renamePattern
	if (first_unknown != string::npos){
		string formatOut = "";
		for (size_t i=0, e=renamePattern.length(); i < e; i++ ) {
			char c = renamePattern[i];
			if ( c == par && i+1 < e) {
				c = renamePattern[i+1];
				if (c > '0' && c <= '9'){
					formatOut.append(1,'$');
					formatOut.append(1,c);
				}
				else {
					formatOut.append(1,par);
					formatOut.append(1,c);
				}
				i++;
			}
			else {
				formatOut.append(1,c);
			}
		}
		boost::regex patternOut(patternSubstrRegex(pattern,0,pattern.length()));
		outfile = boost::regex_replace(infile,patternOut,formatOut,boost::match_default | boost::format_perl);
	} else {
		outfile = renamePattern;
	}
}

/**
 * First filter ist square with width wl*2+1, for the other filter horizontal
 * wavelength increase by one octave for each filter.
 * A total of 8 filters are generated, 3 octaves from base with real and
 * imaginary parts of the complex gabor wavelet as separate filter.
 *
 * base_wavelength:       Is the base wavelength of the underlying cosine/sine.
 * border_gauss_residual: Adjust gauss sigma such that this percentage of
 *                        central energy is left at the border.
 */
void generateFilterbank( vector<Mat> &filterbank, const int &base_wavelength, const float &border_gauss_residual, int max_octave=3){
    float amp = -1*M_PI;
    float borderfactor = sqrtf(amp/logf(border_gauss_residual));
    int hr = FILTER_HEIGHT; // height radius and filter height
    int h = 2*hr+1;
    float alpha = hr*borderfactor;

    for( int octave =0, wavelength = base_wavelength; octave <= max_octave; wavelength*=2, octave++){
        int wr = wavelength;
        int w = wr*2+1;
        float beta = wr*borderfactor;
        cout << "generating filter for wavelength: "<<wavelength<<" and size "<<w<<"x"<<h ;
        cout <<" alpha: "<<alpha<<" beta: "<<beta<<endl;
        Mat filter_re( h,w,CV_32F), filter_im( h,w,CV_32F);
        float G,F_RE,F_IM;
        int xp,yp;
        for( int y = 0; y < h; ++y)
        for( int x = 0; x < w; ++x){
            xp = x-wr;
            yp = y-hr;
            G = exp( amp*( powf( float(xp)/beta,2) + pow(float(yp)/alpha,2)) );
            F_RE = cos( xp * 2*M_PI/wavelength );
            F_IM = sin( xp * 2*M_PI/wavelength );
            filter_re.at<float>(y,x) = G*F_RE;
            filter_im.at<float>(y,x) = G*F_IM;
        }
        filter_re -= sum(filter_re)[0]/(w*h); // correct for non/zero response  of real filter
        filterbank.push_back(filter_re);
        filterbank.push_back(filter_im);
    }
}

/**
 * Specifies which points to use for the iris code, points are taken as given each point is taken from each filter, then the next point is taken.
 *
 * This function specifies and equally spaced grid for extraction
 *
 * grid_x: number of points in x-direction to extract
 *
 * returns the number of grouped points for a single angle
 */
void generateExtractsequenceGrid( vector<Point> &extractsequence, const int &base_wavelength, const int &grid_x,  const int size_x, const int size_y){
    int sample_x = (grid_x >= 1) ? grid_x : size_x/base_wavelength ;
    int sample_y = size_y/(FILTER_HEIGHT*2+1) ;
    int dx = size_x/sample_x; // horizontal equally spaced, image is rotationally extended for filtering
    float offy = FILTER_HEIGHT+0.5;
    float resty= size_y - 2.*offy;
    float dy = resty/(sample_y-1); // keep filter in image for vertical
    if (dy!=dy || sample_y == 1) dy=0; //NaN comparissons are always false or smaple_y ==1 ,i.e., dy = inf
    for( int y=0; y<sample_y; ++y)
    for( int x=0; x<sample_x; ++x)
        extractsequence.push_back( Point( int(dx/2. + x*dx),int(offy + y*dy)));

    cout << "Extractsequence samples: "<<sample_x<<"x"<<sample_y<<endl;
}

/** ------------------------------- Program ------------------------------- **/

/*
 * Main program
 */
int main(int argc, char *argv[])
{
	int mode = MODE_HELP;
	map<string,vector<string> > cmd;
	try {
		cmdRead(cmd,argc,argv);
    	if (cmd.empty() || cmdGetOpt(cmd,"-h") != 0) mode = MODE_HELP;
    	else mode = MODE_MAIN;
    	if (mode == MODE_MAIN){
			// validate command line
			cmdCheckOpts(cmd,"-i|-o|-q|-t|-wf|-ws|-wp|-pwl|-pbp|-psx|-x|-y");
			cmdCheckOptExists(cmd,"-i");
			cmdCheckOptSize(cmd,"-i",1);
			string inFiles = cmdGetPar(cmd,"-i");
			cmdCheckOptExists(cmd,"-o");
			cmdCheckOptSize(cmd,"-o",1);
			string outFiles = cmdGetPar(cmd,"-o");
			string imaskFiles, omaskFiles;
			if (cmdGetOpt(cmd,"-m") != 0){
				cmdCheckOptSize(cmd,"-m",2);
				imaskFiles = cmdGetPar(cmd,"-m", 0);
				omaskFiles = cmdGetPar(cmd,"-m", 1);
			}
            /** parmeters **/
            int base_wavelength = 6;
			if (cmdGetOpt(cmd,"-pwl") != 0){
				cmdCheckOptSize(cmd,"-pwl",1);
				base_wavelength = cmdGetParInt(cmd,"-pwl", 0);
			}
            float border_gauss_residual = 0.01;
			if (cmdGetOpt(cmd,"-pbp") != 0){
				cmdCheckOptSize(cmd,"-pbp",1);
				border_gauss_residual = cmdGetParFloat(cmd,"-pbp", 0);
                if( border_gauss_residual <= 0 || border_gauss_residual >= 1){
                    cout << "WARNING: Border power does not make much sense, reverting to 0.01"<<endl;
                    border_gauss_residual = 0.01;
                }
			}
            int grid_x=256;
			if (cmdGetOpt(cmd,"-psx") != 0){
				cmdCheckOptSize(cmd,"-psx",1);
				grid_x = cmdGetParInt(cmd,"-psx", 0);
                if( grid_x <= 0){
                    cout << "WARNING: grid_x does not make much sense, setting to sampling with 50% base filter overlap" << endl;
                    grid_x = -1;
                }
			}
            cout <<" grid_x from parameters" << grid_x << endl;
            int size_x=512;
			if (cmdGetOpt(cmd,"-x") != 0){
				cmdCheckOptSize(cmd,"-x",1);
				size_x = cmdGetParInt(cmd,"-x", 0);
			}
            int size_y=64;
			if (cmdGetOpt(cmd,"-y") != 0){
				cmdCheckOptSize(cmd,"-y",1);
				size_y = cmdGetParInt(cmd,"-y", 0);
			}
            FILTER_HEIGHT=(size_y-1)/2 ;
            /** options **/
			bool quiet = false;
			if (cmdGetOpt(cmd,"-q") != 0){
				cmdCheckOptSize(cmd,"-q",0);
				quiet = true;
			}
			bool time = false;
			if (cmdGetOpt(cmd,"-t") != 0){
				cmdCheckOptSize(cmd,"-t",0);
				time = true;
			}
			bool write_filterbank = false;
			if (cmdGetOpt(cmd,"-wf") != 0){
				cmdCheckOptSize(cmd,"-wf",0);
				write_filterbank = true;
			}
			bool write_samples = false;
			if (cmdGetOpt(cmd,"-ws") != 0){
				cmdCheckOptSize(cmd,"-ws",0);
				write_samples = true;
			}
			bool write_points = false;
			if (cmdGetOpt(cmd,"-wp") != 0){
				cmdCheckOptSize(cmd,"-wp",0);
				write_points = true;
			}
			// starting routine
			Timing timing(1,quiet);
			vector<string> files;
			patternToFiles(inFiles,files);
			CV_Assert(files.size() > 0);
			timing.total = files.size();
            vector<Mat> filterbank;
            generateFilterbank( filterbank, base_wavelength, border_gauss_residual);
            if( write_filterbank) writeFilterbank(filterbank);
            vector<Point> extractsequence;
            generateExtractsequenceGrid( extractsequence, base_wavelength, grid_x,  size_x, size_y);
			Mat code (1,extractsequence.size()*filterbank.size()/8,CV_8UC1);
            cout << "Code size (bytes): "<< code.size()<<endl;
			Mat codeMask(1,extractsequence.size()*filterbank.size()/8,CV_8UC1);
			for (vector<string>::iterator inFile = files.begin(); inFile != files.end(); std::advance( inFile, 1), timing.progress++){
				if (!quiet) printf("Loading texture '%s' ...\n", (*inFile).c_str());;
				Mat img = imread(*inFile, CV_LOAD_IMAGE_GRAYSCALE);
                if( write_points) writeExtractionsequence(img, extractsequence, write_points); // write_points is reset after one use
				CV_Assert(img.data != 0);
				CV_Assert(img.size() == Size(size_x,size_y));
				Mat mask = Mat();
				if (!imaskFiles.empty()) {
					string imaskfile;
					patternFileRename(inFiles,imaskFiles,*inFile,imaskfile);
					if (!quiet) printf("Loading mask image '%s' ...\n", imaskfile.c_str());;
					mask = imread(imaskfile, CV_LOAD_IMAGE_GRAYSCALE);
					CV_Assert(mask.data != 0);
					CV_Assert(mask.size() == Size(size_x,size_y));
				}
				if (!quiet) printf("Creating iris-code ...\n");

				//featureExtract(code, codeMask, img, mask, m, n);
				featureExtract(code, codeMask, img, mask, filterbank, extractsequence, write_samples);
				string outfile;
				patternFileRename(inFiles,outFiles,*inFile,outfile);
				if (!quiet) printf("Storing code '%s' ...\n", outfile.c_str());
				if (!imwrite(outfile,code)) CV_Error(CV_StsError,"Could not save image '" + outfile + "'");
				if (!imaskFiles.empty()) {
					string omaskfile;
					patternFileRename(inFiles,omaskFiles,*inFile,omaskfile);
					if (!quiet) printf("Storing code-mask '%s' ...\n", omaskfile.c_str());
					if (!imwrite(omaskfile,codeMask)) CV_Error(CV_StsError,"Could not save image '" + omaskfile + "'");
				}
				if (time && timing.update()) timing.print();
			}
			if (time && quiet) timing.clear();
    	}
    	else if (mode == MODE_HELP){
			// validate command line
			cmdCheckOpts(cmd,"-h");
			if (cmdGetOpt(cmd,"-h") != 0) cmdCheckOptSize(cmd,"-h",0);
			// starting routine
			printUsage();
    	}
    }
	catch (...){
	   	printf("Exit with errors.\n");
	   	exit(EXIT_FAILURE);
	}
    return EXIT_SUCCESS;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*

 Copyright (C) 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file stochasticcollocationinvcdf.hpp
    Stochastic collocation inverse cumulative distribution function
*/

#ifndef quantlib_stochastic_collation_inv_cdf_hpp
#define quantlib_stochastic_collation_inv_cdf_hpp

#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/interpolations/lagrangeinterpolation.hpp>

#include <boost/function.hpp>
#include <functional>

namespace QuantLib {
    //! Stochastic collocation inverse cumulative distribution function

    /*! References:
        L.A. Grzelak, J.A.S. Witteveen, M.Suárez-Taboada, C.W. Oosterlee,
        The Stochastic Collocation Monte Carlo Sampler: Highly efficient
        sampling from “expensive” distributions
        http://papers.ssrn.com/sol3/papers.cfm?abstract_id=2529691
     */

    class StochasticCollocationInvCDF : public std::unary_function<Real,Real> {
      public:
        StochasticCollocationInvCDF(
            const boost::function<Real(Real)>& invCDF,
            Size lagrangeOrder,
            Real pMax = Null<Real>(),
            Real pMin = Null<Real>());

        Real value(Real x) const;
        Real operator()(Real u) const;

      private:
        const Array x_;
        const Volatility sigma_;
        const Array y_;
        const LagrangeInterpolation interpl_;
    };
}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*

 Copyright (C) 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file stochasticcollationcdf.cpp
*/

#include <ql/mathconstants.hpp>
#include <ql/math/integrals/gaussianquadratures.hpp>

namespace QuantLib {

    namespace {
        Disposable<Array> g(Real sigma, const Array& x,
                            const boost::function<Real(Real)>& invCDF) {

            Array y(x.size());
            const CumulativeNormalDistribution normalCDF;

            for (Size i=0, n=x.size(); i < n; ++i) {
                y[i] = invCDF(normalCDF(x[i]/sigma));
            }

            return y;
        }
    }

    inline StochasticCollocationInvCDF::StochasticCollocationInvCDF(
        const boost::function<Real(Real)>& invCDF,
        Size lagrangeOrder, Real pMax, Real pMin)
    : x_(M_SQRT2*GaussHermiteIntegration(lagrangeOrder).x()),
      sigma_( (pMax != Null<Real>())
              ? x_.back() / InverseCumulativeNormal()(pMax)
              : (pMin != Null<Real>())
                  ? x_.front() / InverseCumulativeNormal()(pMin)
                  : 1.0),
      y_(g(sigma_, x_, invCDF)),
      interpl_(x_.begin(), x_.end(), y_.begin()) {
    }

    inline Real StochasticCollocationInvCDF::value(Real x) const {
        return interpl_(x*sigma_, true);
    }
    inline Real StochasticCollocationInvCDF::operator()(Real u) const {
        return value(InverseCumulativeNormal()(u));
    }
}

#endif


/*
 * Author: Benoit Sklenard benoit.sklenard@cea.fr
 *
 * 3D Poisson solver.
 * Neumann and Dirichlet boundary conditions are supported and read from Kernel::Mesh class.
 *
 * Matrix operations rely on boost::numeric::ublas library. Atlas library should be more appropriate.
 *
 * Conjugate gradient is used to solve linear system Ax = b
 * Newton-Raphson method is used to get self-consistent Thomas-Fermi/Poisson solution
 *
 * Copyright 2014 IMDEA Materials Institute, Getafe, Madrid, Spain
 *
 * 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.
 */

#include <iomanip>

#include "kernel/Domain.h"
#include "kernel/Mesh.h"
#include "kernel/Constants.h"

#include "io/Diagnostic.h"
#include "io/ParameterManager.h"
#include "io/FileParameters.h"
#include "io/Parameters.h"

#include "okmc/MobileParticleParam.h"

#include "lkmc/LatticeDiamondParam.h"

#include "FermiDirac.h"
#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/timer.hpp>

#include "Poisson.h"

using namespace Kernel;
using namespace boost::numeric;
using namespace Electrostatics;

Poisson::Poisson(Tcl_Interp *pTcl, Domain *pDomain) {
	LOWMSG("Loading Poisson...");

	_pDomain            = pDomain;
	_pMesh              = pDomain->_pMesh;
	_pTcl               = pTcl;

	_T                  = 0;

	_nx                 = (_pMesh->getPeriodicX() ? _pMesh->getnx() - 1 : _pMesh->getnx());
	_ny                 = (_pMesh->getPeriodicY() ? _pMesh->getny() - 1 : _pMesh->getny());
	_nz                 = (_pMesh->getPeriodicZ() ? _pMesh->getnz() - 1 : _pMesh->getnz());

	_CG_tol             = Domains::global()->getFileParameters()->getFloat("MC/Electrostatic/cg.tolerance");       // 1e-6;
	_CG_maxIter         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/cg.max.iteration");     // 2000;
	_CG_verbose         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/cg.verbose");

	_NR_tol             = Domains::global()->getFileParameters()->getFloat("MC/Electrostatic/newton.tolerance");   // 1e-4;
	_NR_maxIter         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/newton.max.iteration"); // 50;
	_NR_verbose         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/newton.verbose");

	_FermiDirac         = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/FermiDirac");
	_partialIonization  = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/partial.ionization");
	_selfConsistentLKMC = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/self.consistent.lkmc");

	_Qe                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qh                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qokmc              = ublas::zero_vector<double>(_nx * _ny * _nz);
	_psi                = ublas::zero_vector<double>(_nx * _ny * _nz);

	_Qlkmc_M            = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qlkmc_P            = ublas::zero_vector<double>(_nx * _ny * _nz);

	_Eg                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Eg0                = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Nc                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Nv                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_DEc                = ublas::zero_vector<double>(_nx * _ny * _nz);
	_DEv                = ublas::zero_vector<double>(_nx * _ny * _nz);

	const IO::ParameterManager      *pPM  = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;
	const LKMC::LatticeParam             **pLP  = _pDomain->_pLaPar;

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
	{
		// OKMC
		for(P_TYPE pt = 0; pt < pPM->getNParticles(); ++pt)
		{
			if(!pPM->isParticleDefined(pt, mt))
				continue;
			if (pMPP->_mapToGrid[mt][pt])
				_Nokmc[mt][pt] = ublas::zero_vector<double>(_nx * _ny * _nz);
		}
		// LKMC
		if (pLP[mt] != NULL && pLP[mt]->_mapToGrid)
			_Nlkmc[mt] = ublas::zero_vector<double>(_nx * _ny * _nz);
	}
}

Poisson::~Poisson() {
}

void Poisson::computeElectronicParameters(double TKelvin) {

	MeshNode           ***pNode = _pMesh->getNodes();
	IO::FileParameters   *pPar  = Domains::global()->getFileParameters();
	IO::ParameterManager *pPM   = Domains::global()->PM();

	for (Kernel::M_TYPE mt = 0; mt < Domains::global()->PM()->getNMaterials(); ++mt) {
		std::string path = pPM->getMaterialName(mt) + "/ElectronicStructure";

		if (!pPar->specified(path)) {
			WARNINGMSG(path << " does not exist!");
			_matParam[mt]._Nc  = 0;
			_matParam[mt]._Nv  = 0;
			_matParam[mt]._Eg  = 0;
			_matParam[mt]._Eg0 = 0;

			continue ;
		}
		std::string eDOSPath           = path + "/eDOSMass";
		std::string hDOSPath           = path + "/hDOSMass";
		std::string EgPath             = path + "/Bandgap";
		std::string EcDilatationalPath = path + "/Ec.dilatational";
		std::string EvDilatationalPath = path + "/Ev.dilatational";
		std::string EcDeviatoricPath   = path + "/Ec.deviatoric";
		std::string EvDeviatoricPath   = path + "/Ev.deviatoric";

		std::string T  = boost::lexical_cast<std::string>(_T);
		std::string T0 = boost::lexical_cast<std::string>(300);

		pPar->loadProcedure(_pTcl, eDOSPath, 1);
		pPar->loadProcedure(_pTcl, hDOSPath, 1);
		pPar->loadProcedure(_pTcl, EgPath, 1);

		double mdc = pPar->getFloatProc(_pTcl, eDOSPath, T);
		double mdv = pPar->getFloatProc(_pTcl, hDOSPath, T);

		_matParam[mt]._Nc  = 2. * pow(M0 * mdc * KB * _T / (2 * M_PI * pow(PLANCK_BAR, 2)), 3./2.);  // m3
		_matParam[mt]._Nv  = 2. * pow(M0 * mdv * KB * _T / (2 * M_PI * pow(PLANCK_BAR, 2)), 3./2.);  // m3
		_matParam[mt]._Eg  = ELECTRONVOLT_TO_HARTREE(pPar->getFloatProc(_pTcl, EgPath, T));     // Ha
		_matParam[mt]._Eg0 = ELECTRONVOLT_TO_HARTREE(pPar->getFloatProc(_pTcl, EgPath, T0));    // Ha

		if (pPar->specified(EcDilatationalPath) && pPar->specified(EvDilatationalPath)) {
			_matParam[mt]._Dc  = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcx = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcy = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcz = ublas::zero_vector<double>(3);
			_matParam[mt]._Dv  = ublas::zero_vector<double>(2);
			_matParam[mt]._Dvb = ublas::zero_vector<double>(2);
			_matParam[mt]._Dvd = ublas::zero_vector<double>(2);

			std::map<std::string, float> Dc  = pPar->getFloatMap(EcDilatationalPath);
			std::map<std::string, float> Dcx = pPar->getFloatMap(EcDeviatoricPath + "(1)");
			std::map<std::string, float> Dcy = pPar->getFloatMap(EcDeviatoricPath + "(2)");
			std::map<std::string, float> Dcz = pPar->getFloatMap(EcDeviatoricPath + "(3)");

			std::map<std::string, float> Dv  = pPar->getFloatMap(EvDilatationalPath);
			std::map<std::string, float> Dvb = pPar->getFloatMap(EvDeviatoricPath + "(1)");
			std::map<std::string, float> Dvd = pPar->getFloatMap(EvDeviatoricPath + "(2)");

			for (unsigned i = 0; i < 3; ++i) {
				_matParam[mt]._Dc(i)  = Dc[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcx(i) = Dcx[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcy(i) = Dcy[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcz(i) = Dcz[boost::lexical_cast<std::string>(i + 1)];
			}

			for (unsigned i = 0; i < 2; ++i) {
				_matParam[mt]._Dv(i)  = Dv[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dvb(i) = Dvb[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dvd(i) = Dvd[boost::lexical_cast<std::string>(i + 1)];
			}
			LOWMSG("got stress data for " << pPM->getMaterialName(mt));
		}
	}

	// initialize electronic parameters for the structure
	for (unsigned ix = 0; ix < _nx; ++ix) {
		for (unsigned iy = 0; iy < _ny; ++iy) {
			for (unsigned iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					const unsigned idx = pNode[ix][iy][iz]._index;

					_Nc(idx)  = pNode[ix][iy][iz]._volume * 1e-27 * getNc(&pNode[ix][iy][iz]);
					_Nv(idx)  = pNode[ix][iy][iz]._volume * 1e-27 * getNv(&pNode[ix][iy][iz]);

					_Eg(idx)  = getEg(&pNode[ix][iy][iz]);
					_Eg0(idx) = getEg0(&pNode[ix][iy][iz]);

					if (_Eg(pNode[ix][iy][iz]._index) <= 0. || _Eg0(pNode[ix][iy][iz]._index) <= 0.)
						ERRORMSG("Poisson::computeElectronicParameters: band gap cannot be zero!");
				}
			}
		}
	}
}

void Poisson::setFirstGuess() {
	MeshNode ***pNode                     = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;

	for (size_t ix = 0; ix < _nx; ++ix) {
		for (size_t iy = 0; iy < _ny; ++iy) {
			for (size_t iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					const unsigned i = pNode[ix][iy][iz]._index;
					double n = 0;

					for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
						for(P_TYPE pt = pPM->getNFamilies(); pt < pPM->getNParticles(); ++pt)
						{
							P_TYPE fam = pPM->getFamily(pt);
							P_POS  pos = pPM->getPPos(pt);
							if(fam == V_TYPE || fam == pPM->getMaterial(mt)._pt[0] || fam == pPM->getMaterial(mt)._pt[1] ||
								pos == POS_I || pos == POS_V)
								continue;
							if(!_Nokmc[mt][pt].empty() && _Nokmc[mt][pt](i) > 0)
								n += pMPP->_state2charge[mt][pt][0]; //impurities and dopants only have state 0!
						}

					if (n > 0.)
						_psi(i) = 0.5 * getEg(&pNode[ix][iy][iz]);
					else if (n < 0.)
						_psi(i) = - 0.5 * getEg(&pNode[ix][iy][iz]);
					else
						_psi(pNode[ix][iy][iz]._index) = 0;

					for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt) {
						if (!_Nlkmc[mt].empty() && _Nlkmc[mt](i) > 0) {
							_psi(i) = 0;
						}
					}
				}
			}
		}
	}
}

void Poisson::setT(double T)
{
	if (_T != T)
	{
		_T = T;
		MEDMSG("Building vector for OKMC particles...");
		buildOKMCVector();

		if (_selfConsistentLKMC)
			buildLKMCVector();

		MEDMSG("Building Laplacian matrix...");
		buildLaplacianMatrix();
		computeElectronicParameters(T);
		setFirstGuess();
	}
}

bool Poisson::compute() {
	MEDMSG("Building vector for OKMC particles...");
	buildOKMCVector();

	if (_selfConsistentLKMC)
		buildLKMCVector();

	MEDMSG("Solving non-linear Poisson equation...");

	if (! NewtonRaphson())
		ERRORMSG("Poisson::NewtonRaphson did not converged!");

	map2Nodes();
	map2Elements();

	return true;
}

double Poisson::getDEc(MeshNode *pNode) {
	const double kT = KB / Q * _T;

	std::set<MeshElement *> s;

	double DEc = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it) {
		const M_TYPE mt = (*it)->getMaterial();

		// ublas::vector<double> DEci = ublas::zero_vector<double>(3);

		double tmp = 0;
		for (unsigned i = 0; i < 3; ++i) {
			// const ublas::vector<double> E = (*it)->strain();
			ublas::vector<double> E = ublas::zero_vector<double>(3);

			const double Dc         = (_matParam[mt]._Dc.size()  > 0 ? _matParam[mt]._Dc(i)  : 0);
			const double Dcx        = (_matParam[mt]._Dcx.size() > 0 ? _matParam[mt]._Dcx(i) : 0);
			const double Dcy        = (_matParam[mt]._Dcy.size() > 0 ? _matParam[mt]._Dcy(i) : 0);
			const double Dcz        = (_matParam[mt]._Dcz.size() > 0 ? _matParam[mt]._Dcz(i) : 0);

			tmp += exp(- (Dc * (E(0) + E(1) + E(2)) + Dcx * E(0) + Dcy * E(1) + Dcz * E(2))/ kT);
		}

		DEc += ELECTRONVOLT_TO_HARTREE(-kT * log(1./3. * tmp));

		if (Domains::global()->PM()->getMaterialName(mt) == "Silicon")
			LOWMSG("DEc:" << DEc / static_cast<double>(s.size()));
	}

	return (DEc / static_cast<double>(s.size()));
}

double Poisson::getNc(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Nc = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Nc += _matParam[(*it)->getMaterial()]._Nc;

	return (Nc / static_cast<double>(s.size()));
}

double Poisson::getNv(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Nv = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Nv += _matParam[(*it)->getMaterial()]._Nv;

	return (Nv / static_cast<double>(s.size()));
}

double Poisson::getEg(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Eg = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Eg += _matParam[(*it)->getMaterial()]._Eg;

	return (Eg / static_cast<double>(s.size()));
}

double Poisson::getEg0(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Eg0 = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Eg0 += _matParam[(*it)->getMaterial()]._Eg0;

	return (Eg0 / static_cast<double>(s.size()));
}

// OBSOLETE METHOD
// Do NOT call!
/* double Poisson::getLA(MeshNode *pNode) {
	double w = 0.;

	for (std::map<LKMC::LatticeAtom *, double>::iterator it = pNode->_mLA.begin(); it !=pNode->_mLA.end(); ++it)
		w += it->second;

	return w;
} */

void Poisson::map2Nodes() {
	MeshNode ***pNode = _pMesh->getNodes();

	for (unsigned ix = 0; ix < _nx; ++ix) {
		for (unsigned iy = 0; iy < _ny; ++iy) {
			for (unsigned iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					pNode[ix][iy][iz]._V        = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Eg       = HARTREE_TO_ELECTRONVOLT(_Eg(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Eg0      = HARTREE_TO_ELECTRONVOLT(_Eg0(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Ec       = HARTREE_TO_ELECTRONVOLT(0.5 * getEg(&pNode[ix][iy][iz]) - _psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Ev       = HARTREE_TO_ELECTRONVOLT(- 0.5 * getEg(&pNode[ix][iy][iz]) - _psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._eDensity = _Qe(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-21);
					pNode[ix][iy][iz]._hDensity = _Qh(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-21);

					// pNode[ix][iy][iz]._N_okmc   = _Qokmc(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-27); // FIXME

					if (ix > 0 && ix < (_nx - 1) && iy > 0 && iy < (_ny - 1) && iz > 0 && iz < (_nz - 1)) {
						if (pNode[ix-1][iy][iz]._index < 0)
							continue ;
						if (pNode[ix+1][iy][iz]._index < 0)
							continue ;
						if (pNode[ix][iy-1][iz]._index < 0)
							continue ;
						if (pNode[ix][iy+1][iz]._index < 0)
							continue ;
						if (pNode[ix][iy][iz-1]._index < 0)
							continue ;
						if (pNode[ix][iy][iz+1]._index < 0)
							continue ;

						const double dxp = pNode[ix+1][iy][iz]._coord._x - pNode[ix][iy][iz]._coord._x;
						const double dxm = pNode[ix][iy][iz]._coord._x - pNode[ix-1][iy][iz]._coord._x;
						const double dyp = pNode[ix][iy+1][iz]._coord._y - pNode[ix][iy][iz]._coord._y;
						const double dym = pNode[ix][iy][iz]._coord._y - pNode[ix][iy-1][iz]._coord._y;
						const double dzp = pNode[ix][iy][iz+1]._coord._z - pNode[ix][iy][iz]._coord._z;
						const double dzm = pNode[ix][iy][iz]._coord._z - pNode[ix][iy][iz-1]._coord._z;

						const double Vxp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix+1][iy][iz]._index));
						const double Vxm = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix-1][iy][iz]._index));
						const double Vyp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy+1][iz]._index));
						const double Vym = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy-1][iz]._index));
						const double Vzp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz+1]._index));
						const double Vzm = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz-1]._index));

						const double V   = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz]._index));

						pNode[ix][iy][iz]._E    = ublas::zero_vector<double>(3); // electric field in eV/nm
						pNode[ix][iy][iz]._E(0) = - 0.5 * ((Vxp - V) / dxp + (V  - Vxm) / dxm);
						pNode[ix][iy][iz]._E(1) = - 0.5 * ((Vyp - V) / dyp + (V  - Vym) / dym);
						pNode[ix][iy][iz]._E(2) = - 0.5 * ((Vzp - V) / dzp + (V  - Vzm) / dzm);
					}
				}
			}
		}
	}
}

void Poisson::map2Elements() {
	for(Kernel::Mesh::iterator it = _pMesh->begin(); it != _pMesh->end(); ++it)
	{
		std::set<MeshNode *> s;
		const M_TYPE mt = it->getMaterial();

		_pMesh->getNodesFromElement(&(*it), s);

		if (s.empty())
			ERRORMSG("Poisson::map2Elements: cannot find nodes of element " << it->getIndex());

		double V  = 0;
		for (std::set<MeshNode *>::const_iterator i = s.begin(); i != s.end(); ++i)
			V  += (*i)->_V;

		it->electrostaticPotential() = V / static_cast<double>(s.size());
		it->bandGap() = HARTREE_TO_ELECTRONVOLT(_matParam[mt]._Eg);
	}
}

bool Poisson::NewtonRaphson() {
	MeshNode ***pNode                     = _pMesh->getNodes();
	const double kT                       = ELECTRONVOLT_TO_HARTREE(KB / Q * _T);
	const IO::ParameterManager *pPM       = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;
	bool         ret                      = false;

	ublas::vector<double> deltaPsi  = ublas::zero_vector<double>(_nx * _ny * _nz);
	ublas::vector<double> err       = ublas::zero_vector<double>(_nx * _ny * _nz);
	ublas::vector<double> totCharge = ublas::zero_vector<double>(_nx * _ny * _nz);

	// ublas::vector<double> Qtmp      = ublas::zero_vector<double>(_nx * _ny * _nz);

	boost::timer t;
	t.restart();

	for (size_t it = 0; it < _NR_maxIter; ++it) {
		ublas::compressed_matrix<double> J = _laplacian;

		for (size_t ix = 0; ix < _nx; ++ix) {
			for (size_t iy = 0; iy < _ny; ++iy) {
				for (size_t iz = 0; iz < _nz; ++iz) {
					if (pNode[ix][iy][iz]._active) {
						const size_t i   = pNode[ix][iy][iz]._index;

						double dQe        = 0.;
						double dQh        = 0.;
						double dQokmc     = 0;
						double dQlkmc_M   = 0.;
						double dQlkmc_P   = 0.;

						const double Ef = 0;
						const double CBM = 0.5 * _Eg(i) - _psi(i);
						const double VBM = - 0.5 * _Eg(i) - _psi(i);

						const double EfEc = Ef - CBM;
						const double EvEf = VBM - Ef;
						// const double EfEc = _psi(i) - 0.5 * _Eg(i);
						// const double EvEf = - _psi(i) - 0.5 * _Eg(i);

						_Qokmc(i)   = 0;
						_Qlkmc_M(i) = 0.;
						_Qlkmc_P(i) = 0.;

						if (_FermiDirac) {
							dQe    = _Nc(i) / kT * FermiDirac::mhalf(EfEc / kT);
							dQh    = - _Nv(i) / kT * FermiDirac::mhalf(EvEf / kT);
							_Qe(i) = _Nc(i) * FermiDirac::phalf(EfEc / kT);
							_Qh(i) = _Nv(i) * FermiDirac::phalf(EvEf / kT);
						}
						else {
							dQe    = _Nc(i) / kT * exp(EfEc / kT);
							dQh    = - _Nv(i) / kT * exp(EvEf / kT);
							_Qe(i) = _Nc(i) * exp(EfEc / kT);
							_Qh(i) = _Nv(i) * exp(EvEf / kT);
						}

						for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
						{// OKMC
							for(P_TYPE pt = pPM->getNFamilies(); pt < pPM->getNParticles(); ++pt)
							{
								P_TYPE fam = pPM->getFamily(pt);
								P_POS  pos = pPM->getPPos(pt);
								if(fam == V_TYPE || fam == pPM->getMaterial(mt)._pt[0] || fam == pPM->getMaterial(mt)._pt[1] ||
										pos == POS_I || pos == POS_V)
									continue;
								if (! _Nokmc[mt][pt].empty() && _Nokmc[mt][pt](i) > 0)
								{
									if (pMPP->_state2charge[mt][pt][0] == 1) { //DONOR
										if (_partialIonization) {
											// const double E = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]);
											const double E = Ef - (CBM - _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]));
											_Qokmc(i) += _Nokmc[mt][pt](i) / (1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT));
											dQokmc    += - _Nokmc[mt][pt](i) * pMPP->_stateDegeneracy[mt][pt] / kT * exp(E / kT) / pow(1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT), 2);
										}
										else {
											_Qokmc(i) += _Nokmc[mt][pt](i);
										}
									}
									else if (pMPP->_state2charge[mt][pt][0] == -1) { //ACCEPTOR
										if (_partialIonization) {
											// const double E = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]);
											const double E = (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt])) - Ef;
											_Qokmc(i) -= _Nokmc[mt][pt](i) / (1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT));
											dQokmc    -= _Nokmc[mt][pt](i) * pMPP->_stateDegeneracy[mt][pt] / kT * exp(E / kT) / pow(1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT), 2);
										}
										else {
											_Qokmc(i) -= _Nokmc[mt][pt](i);
										}
									}
								}
							}

							// LKMC
							/*
							if (_selfConsistentGFLS && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND_1) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								LKMC::LatticeDiamondParam *pLDP = static_cast<LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								const double E_lkmc_M = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0);   // Ef - E(0,-)
								const double E_lkmc_P = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0); // E(+,0) - Ef

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								dQlkmc_M    +=   _Nc(i) / _Nlkmc[mt](i) * 1. / kT * g_M / g_0 * exp(E_lkmc_M / kT);
								dQlkmc_P    += - _Nv(i) / _Nlkmc[mt](i) * 1. / kT * g_P / g_0 * exp(E_lkmc_P / kT);
								_Qlkmc_M(i) += _Nc(i) / _Nlkmc[mt](i) * g_M / g_0 * exp(E_lkmc_M / kT);
								_Qlkmc_P(i) += _Nv(i) / _Nlkmc[mt](i) * g_P / g_0 * exp(E_lkmc_P / kT);
							}

							if (_selfConsistentGFLS2 && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND_1) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								LKMC::LatticeDiamondParam *pLDP = static_cast<LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								// const double Ef  = 0;
								// const double VBM = - V - 0.5 * Eg;

							    // Note: defect level scales with bandgap
								const double E_lkmc_M    = Ef - (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0));      // Ef  - E(0,-)
								const double E_lkmc_P    = (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0)) - Ef; // Ef  - E(0,-)

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								const double den_M = 1. + (g_0 / g_M) * exp( - E_lkmc_M / kT) + (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M) / kT);
								const double den_P = 1. + (g_0 / g_P) * exp( - E_lkmc_P / kT) + (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P) / kT);

								dQlkmc_M    +=   _Nlkmc[mt](i) * (1. / kT * (g_0 / g_M) * exp( - E_lkmc_M / kT) + 2. / kT * (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M)/ kT)) / (den_M * den_M);
								dQlkmc_P    += - _Nlkmc[mt](i) * (1. / kT * (g_0 / g_P) * exp( - E_lkmc_P / kT) + 2. / kT * (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P)/ kT)) / (den_P * den_P);
								_Qlkmc_M(i) += _Nlkmc[mt](i) / den_M;
								_Qlkmc_P(i) += _Nlkmc[mt](i) / den_P;
							}*/

							if (_selfConsistentLKMC && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								const LKMC::LatticeDiamondParam *pLDP = static_cast<const LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								const double E_lkmc_M = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0);   // Ef - E(0,-)
								const double E_lkmc_P = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0); // E(+,0) - Ef

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								const double den_M = 1. + (g_0 / g_M) * exp( - E_lkmc_M / kT) + (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M) / kT);
								const double den_P = 1. + (g_0 / g_P) * exp( - E_lkmc_P / kT) + (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P) / kT);

								dQlkmc_M    +=   _Nlkmc[mt](i) * (1. / kT * (g_0 / g_M) * exp( - E_lkmc_M / kT) + 2. / kT * (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M)/ kT)) / (den_M * den_M);
								dQlkmc_P    += - _Nlkmc[mt](i) * (1. / kT * (g_0 / g_P) * exp( - E_lkmc_P / kT) + 2. / kT * (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P)/ kT)) / (den_P * den_P);
								_Qlkmc_M(i) += _Nlkmc[mt](i) / den_M;
								_Qlkmc_P(i) += _Nlkmc[mt](i) / den_P;
							}
						}

						J(i,i) -= 4. * M_PI * (dQh - dQe + dQokmc + dQlkmc_P - dQlkmc_M);
					}
				}
			}
		}

		ublas::noalias(totCharge) = _Qh - _Qe + _Qokmc + _Qeff + _Qlkmc_P - _Qlkmc_M;
		ublas::noalias(err)       = - ublas::prod(_laplacian, _psi) + 4 * M_PI * totCharge;

		if (ublas::norm_2(err) < _NR_tol) {
			ret = true;
			if (_NR_verbose > 0)
				LOWMSG("Poisson::NewtonRaphson converged in " << it << " iterations (" << t.elapsed() << " s)");

			break ;
		}
		if (_NR_verbose > 1)
			LOWMSG("Poisson::NewtonRaphson iteration " << it << " error= " << ublas::norm_2(err) << " (" << t.elapsed() << " s)");

		deltaPsi = ublas::zero_vector<double>(_nx * _ny * _nz);

		if (! ConjugateGradient(J, deltaPsi, err))
			ERRORMSG("Poisson::CG did not converged");

		_psi += deltaPsi;
	}

	return ret;
}

bool Poisson::ConjugateGradient(const ublas::compressed_matrix<double> &A, ublas::vector<double> &x, const ublas::vector<double> &b) {
	boost::timer t;
	t.restart();

	ublas::vector<double> a;

	bool                  ret = false;
	ublas::vector<double> p   = ublas::zero_vector<double>(x.size());
	ublas::vector<double> r   = ublas::zero_vector<double>(x.size());

	ublas::noalias(r) = b - ublas::prod(A, x);
	ublas::noalias(p) = r;

	for (size_t it = 0; it < _CG_maxIter; ++it) {
		if (ublas::norm_2(r) < _CG_tol) {

			if (_CG_verbose > 0)
				LOWMSG("Poisson::CG solved in " << it << " iterations (" << t.elapsed() << " s)");

			ret = true;
			break ;
		}
		if (_CG_verbose > 1) {
			LOWMSG("Poisson::CG iteration " << it << " error= " << ublas::norm_2(r) << " (" << t.elapsed() << " s)");
		}

		a                 = ublas::zero_vector<double>(x.size());
		ublas::noalias(a) = ublas::prod(A, p);

		double  lambda = ublas::inner_prod(r, p) / ublas::inner_prod(a, p);

		x += lambda * p;
		r -= lambda * a;

		p = r - (ublas::inner_prod(r, a) / ublas::inner_prod(a, p)) * p;
	}

	return (ret);
}

// This method is for test purpose only
void Poisson::testPN() {
	MeshNode ***pNode = _pMesh->getNodes();

	for (size_t ix = 0; ix < _nx; ++ix)
		for (size_t iy = 0; iy < _ny; ++iy)
			for (size_t iz = 0; iz < _nz; ++iz)
				if (pNode[ix][iy][iz]._active) {
					if (ix < _nx / 2)
						_Qokmc(pNode[ix][iy][iz]._index) = .01;
					else
						_Qokmc(pNode[ix][iy][iz]._index) = -.005;

					if (ix == 0 || ix == _nx - 1 || iy == 0 || iy == _ny - 1 || iz == 0 || iz == _nz - 1)
						_Qokmc(pNode[ix][iy][iz]._index) *= .5;
				}
}

void Poisson::buildOKMCVector() {
	MeshNode ***pNode = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt) {
		for(P_TYPE pt = 0; pt < pPM->getNParticles(); ++pt) {
			if (!_Nokmc[mt][pt].empty()) {
				_Nokmc[mt][pt] = ublas::zero_vector<double>(_nx * _ny * _nz);
			}
		}
	}

	for (size_t ix = 0; ix < _nx; ++ix) {
		for (size_t iy = 0; iy < _ny; ++iy) {
			for (size_t iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active && !pNode[ix][iy][iz]._mPart.empty()) {
					for (std::map<OKMC::Particle *, double>::const_iterator it = pNode[ix][iy][iz]._mPart.begin(); it != pNode[ix][iy][iz]._mPart.end(); ++it) {
						const OKMC::Particle * pPart = it->first;
						const M_TYPE mt        = pPart->getElement()->getMaterial();
						const P_TYPE pt        = pPart->getPType();
						if(_Nokmc[mt][pt].empty()) //particle not mapped.
							continue;
						_Nokmc[mt][pt](pNode[ix][iy][iz]._index) += it->second;
					}
				}
			}
		}
	}
}

void Poisson::buildLKMCVector() {
	MeshNode ***pNode = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
		if (!_Nlkmc[mt].empty())
		{
			_Nlkmc[mt] = ublas::zero_vector<double>(_nx * _ny * _nz);
			for (size_t ix = 0; ix < _nx; ++ix)
				for (size_t iy = 0; iy < _ny; ++iy)
					for (size_t iz = 0; iz < _nz; ++iz)
						if (pNode[ix][iy][iz]._active && !pNode[ix][iy][iz]._mLA.empty())
							for (std::map<LKMC::LatticeAtom *, double>::const_iterator it = pNode[ix][iy][iz]._mLA.begin(); it != pNode[ix][iy][iz]._mLA.end(); ++it)
								if (/*!it->first->getDefective() && */!it->first->getPerformed())
									_Nlkmc[mt](pNode[ix][iy][iz]._index) += it->second;
		}
}

double Poisson::getPermittivity(MeshNode *n1, MeshNode *n2, MeshNode *n3) {
	double eps = 0.;

	std::set<MeshElement *> s1;
	std::set<MeshElement *> s2;
	std::set<MeshElement *> s3;

	_pMesh->getElementsFromNode(n1, s1);
	_pMesh->getElementsFromNode(n2, s2);
	_pMesh->getElementsFromNode(n3, s3);

	for(std::set<MeshElement *>::const_iterator it = s1.begin(); it != s1.end(); ++it)
		if (s2.find(*it) != s2.end() && s3.find(*it) != s3.end()) {
			// eps = Domains::global()->PM()->getPermittivity((*it)->getMaterial());
			eps = Domains::global()->PM()->getMaterial((*it)->getMaterial())._permittivity;
			break ;
		}

	return eps;
}

void Poisson::buildLaplacianMatrix() {
	double dxm;
	double dxp;
	double dym;
	double dyp;
	double dzm;
	double dzp;

	MeshNode ***pNode = _pMesh->getNodes();

	_laplacian = ublas::compressed_matrix<double> (_nx * _ny * _nz, _nx * _ny * _nz);
	_Qeff      = ublas::zero_vector<double> (_nx * _ny * _nz);

	for (unsigned ix = 0; ix < _pMesh->getnx(); ++ix) {
		for (unsigned iy = 0; iy < _pMesh->getny(); ++iy) {
			for (unsigned iz = 0; iz < _pMesh->getnz(); ++iz) {

				if (pNode[ix][iy][iz]._active) {
					double a[6] = {};

					unsigned ixm = ix - 1;
					unsigned ixp = ix + 1;
					unsigned iym = iy - 1;
					unsigned iyp = iy + 1;
					unsigned izm = iz - 1;
					unsigned izp = iz + 1;

					// xm
					if (ix == 0) {
						ixm = (_pMesh->getPeriodicX() ? _nx-1 : ix);
						dxm = (_pMesh->getPeriodicX() ? pNode[_nx][iy][iz]._coord._x - pNode[_nx-1][iy][iz]._coord._x : 0);
					} else
						dxm = pNode[ix][iy][iz]._coord._x   - pNode[ixm][iy][iz]._coord._x;
					// xp
					if (ix == _nx - 1) {
						ixp = (_pMesh->getPeriodicX() ? 0 : _nx-1);
						dxp = (_pMesh->getPeriodicX() ? pNode[_nx][iy][iz]._coord._x - pNode[_nx-1][iy][iz]._coord._x : 0);
					}
					else
						dxp = pNode[ixp][iy][iz]._coord._x - pNode[ix][iy][iz]._coord._x;
					// ym
					if (iy == 0) {
						iym = (_pMesh->getPeriodicY() ? _ny-1 : iy);
						dym = (_pMesh->getPeriodicY() ? pNode[ix][_ny][iz]._coord._y - pNode[ix][_ny-1][iz]._coord._y : 0);
					} else
						dym = pNode[ix][iy][iz]._coord._y   - pNode[ix][iym][iz]._coord._y;
					// yp
					if (iy == _ny - 1) {
						iyp = (_pMesh->getPeriodicY() ? 0 : _ny-1);
						dyp = (_pMesh->getPeriodicY() ? pNode[ix][_ny][iz]._coord._y - pNode[ix][_ny-1][iz]._coord._y : 0);
					}
					else
						dyp = pNode[ix][iyp][iz]._coord._y - pNode[ix][iy][iz]._coord._y;
					// zm
					if (iz == 0) {
						izm = (_pMesh->getPeriodicZ() ? _nz-1 : iz);
						dzm = (_pMesh->getPeriodicZ() ? pNode[ix][iy][_nz]._coord._z - pNode[ix][iy][_nz-1]._coord._z : 0);
					} else
						dzm = pNode[ix][iy][iz]._coord._z   - pNode[ix][iy][izm]._coord._z;
					// zp
					if (iz == _nz - 1) {
						izp = (_pMesh->getPeriodicZ() ? 0 : _nz-1);
						dzp = (_pMesh->getPeriodicZ() ? pNode[ix][iy][_nz]._coord._z - pNode[ix][iy][_nz-1]._coord._z : 0);
					}
					else
						dzp = pNode[ix][iy][izp]._coord._z - pNode[ix][iy][iz]._coord._z;

					dxm /= BOHR_RADIUS * 1e9;
					dxp /= BOHR_RADIUS * 1e9;
					dym /= BOHR_RADIUS * 1e9;
					dyp /= BOHR_RADIUS * 1e9;
					dzm /= BOHR_RADIUS * 1e9;
					dzp /= BOHR_RADIUS * 1e9;

					if (ix != ixp) {
						const double eps1 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izm]);

						a[0] = 1. / (4. * dxp) * (eps1 * dyp * dzp + eps2 * dym * dzp + eps3 * dyp * dzm + eps4 * dym * dzm);
					}
					if (ix != ixm) {
						const double eps1 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izm]);

						a[1] = 1. / (4. * dxm) * (eps1 * dyp * dzp + eps2 * dym * dzp + eps3 * dyp * dzm + eps4 * dym * dzm);
					}
					if (iy != iyp) {
						const double eps1 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izm]);

						a[2] = 1. / (4. * dyp) * (eps1 * dxp * dzp + eps2 * dxm * dzp + eps3 * dxp * dzm + eps4 * dxm * dzm);
					}
					if (iy != iym) {
						const double eps1 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izm]);

						a[3] = 1. / (4. * dym) * (eps1 * dxp * dzp + eps2 * dxm * dzp + eps3 * dxp * dzm + eps4 * dxm * dzm);
					}
					if (iz != izp) {
						const double eps1 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixp][iy][iz], &pNode[ix][iyp][iz]);
						const double eps2 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixp][iy][iz], &pNode[ix][iym][iz]);
						const double eps3 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixm][iy][iz], &pNode[ix][iyp][iz]);
						const double eps4 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixm][iy][iz], &pNode[ix][iym][iz]);

						a[4] = 1. / (4. * dzp) * (eps1 * dyp * dxp + eps2 * dym * dxp + eps3 * dyp * dxm + eps4 * dym * dxm);
					}
					if (iz != izm) {
						const double eps1 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixp][iy][iz], &pNode[ix][iyp][iz]);
						const double eps2 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixp][iy][iz], &pNode[ix][iym][iz]);
						const double eps3 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixm][iy][iz], &pNode[ix][iyp][iz]);
						const double eps4 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixm][iy][iz], &pNode[ix][iym][iz]);

						a[5] = 1. / (4. * dzm) * (eps1 * dyp * dxp + eps2 * dym * dxp + eps3 * dyp * dxm + eps4 * dym * dxm);
					}

					if (!pNode[ixp][iy][iz]._active && _pMesh->isDirichletX())
						_Qeff(pNode[ix][iy][iz]._index) += a[0] * ELECTRONVOLT_TO_HARTREE(pNode[ixp][iy][iz]._V) / (4. * M_PI);
					if (!pNode[ixm][iy][iz]._active && _pMesh->isDirichletX())
						_Qeff(pNode[ix][iy][iz]._index) += a[1] * ELECTRONVOLT_TO_HARTREE(pNode[ixm][iy][iz]._V) / (4. * M_PI);

					if (!pNode[ix][iyp][iz]._active && _pMesh->isDirichletY())
						_Qeff(pNode[ix][iy][iz]._index) += a[2] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iyp][iz]._V) / (4. * M_PI);
					if (!pNode[ix][iym][iz]._active && _pMesh->isDirichletY())
						_Qeff(pNode[ix][iy][iz]._index) += a[3] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iym][iz]._V) / (4. * M_PI);

					if (!pNode[ix][iy][izm]._active && _pMesh->isDirichletZ())
						_Qeff(pNode[ix][iy][iz]._index) += a[4] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iy][izp]._V) / (4. * M_PI);
					if (!pNode[ix][iy][izp]._active && _pMesh->isDirichletZ())
						_Qeff(pNode[ix][iy][iz]._index) += a[5] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iy][izm]._V) / (4. * M_PI);

					_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][iz]._index)      = a[0] + a[1] + a[2] + a[3] + a[4] + a[5];
					if (ix != ixp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ixp][iy][iz]._index) = - a[0];
					if (ix != ixm)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ixm][iy][iz]._index) = - a[1];
					if (iy != iyp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iyp][iz]._index) = - a[2];
					if (iy != iym)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iym][iz]._index) = - a[3];
					if (iz != izp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][izp]._index) = - a[4];
					if (iz != izm)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][izm]._index) = - a[5];
				}
			}
		}
	}
}