#include <votca/ctp/pewald3d.h>
#include <boost/format.hpp>
#include <algorithm>
#include <boost/date_time/posix_time/posix_time.hpp>
//#include <boost/timer/timer.hpp>

namespace votca { namespace ctp {

using boost::format;

PEwald3D3D::~PEwald3D3D() { ; }

PEwald3D3D::PEwald3D3D(Topology *top, PolarTop *ptop, Property *opt, Logger *log)
  : Ewald3DnD(top, ptop, opt, log) {}

void PEwald3D3D::GenerateKVectors(vector<PolarSeg*> &ps1, vector<PolarSeg*> &ps2) {

    // Take care of norm for grading function
    // All three components non-zero
    //              S(kx)*S(ky)*S(kz)
    // G = A(k) * ---------------------
    //            (<S(kx)><S(ky)><S(kz)>)**(2/3)
    // Component i zero
    //                   S(kj)*S(kk)
    // G = A(k) * -------------------------
    //             (<S(kj)><S(kk)>)**(1/2)
    // Components i,j zero
    // => All S(k) calculated anyway, no need to grade
    // We can use the same grading function if we set
    //
    // S(ki=0) = <S(ki)>**(2/3) (<S(kj)><S(kk)>)**(1/6)

    assert(!_did_generate_kvectors);

    CTP_LOG(logINFO,*_log) << flush
        << "Generating K-vectors (Sx*Sy*Sz grading system in place)" << flush;

    vector< EWD::KVector > kvecs_2_0; // 2 components zero
    vector< EWD::KVector > kvecs_1_0; // 1 component zero
    vector< EWD::KVector > kvecs_0_0; // 0 components zero

    // CONTAINERS FOR GRADING K-VECTORS
    vector< double > kx_s1s2;
    kx_s1s2.push_back(1);
    vector< double > ky_s1s2;
    ky_s1s2.push_back(1);
    vector< double > kz_s1s2;
    kz_s1s2.push_back(1);
    double avg_kx_s1s2 = 0.0;
    double avg_ky_s1s2 = 0.0;
    double avg_kz_s1s2 = 0.0;

    // TWO COMPONENTS ZERO, ONE NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "  o K-lines through origin: Exploring K resonances" << flush;
    for (int i = 1; i < _NA_max+1; ++i) {
        vec k = +i*_A;
        EWD::triple<EWD::cmplx> ppuu_posk = _ewdactor.S1S2(k, ps1, ps2);
        kx_s1s2.push_back(0.5*std::abs(ppuu_posk._pp._re));
        avg_kx_s1s2 += 0.5*std::abs(ppuu_posk._pp._re);
        EWD::KVector kvec_pos = EWD::KVector(+1*k,0.);
        EWD::KVector kvec_neg = EWD::KVector(-1*k,0.);
        kvecs_2_0.push_back(kvec_pos);
        kvecs_2_0.push_back(kvec_neg);
    }
    avg_kx_s1s2 /= _NA_max;

    for (int i = 1; i < _NB_max+1; ++i) {
        vec k = +i*_B;
        EWD::triple<EWD::cmplx> ppuu_posk = _ewdactor.S1S2(k, ps1, ps2);
        ky_s1s2.push_back(0.5*std::abs(ppuu_posk._pp._re));
        avg_ky_s1s2 += 0.5*std::abs(ppuu_posk._pp._re);
        EWD::KVector kvec_pos = EWD::KVector(+1*k,0);
        EWD::KVector kvec_neg = EWD::KVector(-1*k,0);
        kvecs_2_0.push_back(kvec_pos);
        kvecs_2_0.push_back(kvec_neg);
    }
    avg_ky_s1s2 /= _NB_max;

    for (int i = 1; i < _NC_max+1; ++i) {
        vec k = +i*_C;
        EWD::triple<EWD::cmplx> ppuu_posk = _ewdactor.S1S2(k, ps1, ps2);
        kz_s1s2.push_back(0.5*std::abs(ppuu_posk._pp._re));
        avg_kz_s1s2 += 0.5*std::abs(ppuu_posk._pp._re);
        EWD::KVector kvec_pos = EWD::KVector(+1*k,0);
        EWD::KVector kvec_neg = EWD::KVector(-1*k,0);
        kvecs_2_0.push_back(kvec_pos);
        kvecs_2_0.push_back(kvec_neg);
    }
    avg_kz_s1s2 /= _NC_max;

    double kxyz_s1s2_norm;
    bool neutral_mode = false;
    if (pow(avg_kx_s1s2*avg_ky_s1s2*avg_kz_s1s2,2./3.) > 1e-100)
        kxyz_s1s2_norm = 1./pow(avg_kx_s1s2*avg_ky_s1s2*avg_kz_s1s2,2./3.) * EWD::int2eV / _LxLyLz;
    else {
        CTP_LOG(logDEBUG,*_log)
            << "    - Symptoms of a neutral system: Use Ark2Expk2 grading." << flush;
        kxyz_s1s2_norm = EWD::int2eV / _LxLyLz;
        neutral_mode = true;
    }

    kx_s1s2[0] = pow(avg_ky_s1s2*avg_kz_s1s2,1./6.)*pow(avg_kx_s1s2,2./3.);
    ky_s1s2[0] = pow(avg_kz_s1s2*avg_kx_s1s2,1./6.)*pow(avg_ky_s1s2,2./3.);
    kz_s1s2[0] = pow(avg_kx_s1s2*avg_ky_s1s2,1./6.)*pow(avg_kz_s1s2,2./3.);

    // ONE COMPONENT ZERO, TWO NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "  o K-planes through origin: Applying K resonances" << flush;

    int kx, ky, kz;
    kx = 0;
    for (ky = -_NB_max; ky < _NB_max+1; ++ky) {
        if (ky == 0) continue;
        for (kz = -_NC_max; kz < _NC_max+1; ++kz) {
            if (kz == 0) continue;
            vec k = kx*_A + ky*_B + kz*_C;
            double grade = _ewdactor.Ark2Expk2(k) * kx_s1s2[std::abs(kx)] * ky_s1s2[std::abs(ky)] * kz_s1s2[std::abs(kz)] * kxyz_s1s2_norm;
            if (neutral_mode) grade = _ewdactor.Ark2Expk2(k) * EWD::int2eV / _LxLyLz;
            EWD::KVector kvec = EWD::KVector(k,grade);
            kvecs_1_0.push_back(kvec);
        }
    }
    ky = 0;
    for (kx = -_NA_max; kx < _NA_max+1; ++kx) {
        if (kx == 0) continue;
        for (kz = -_NC_max; kz < _NC_max+1; ++kz) {
            if (kz == 0) continue;
            vec k = kx*_A + ky*_B + kz*_C;
            double grade = _ewdactor.Ark2Expk2(k) * kx_s1s2[std::abs(kx)] * ky_s1s2[std::abs(ky)] * kz_s1s2[std::abs(kz)] * kxyz_s1s2_norm;
            if (neutral_mode) grade = _ewdactor.Ark2Expk2(k) * EWD::int2eV / _LxLyLz;
            EWD::KVector kvec = EWD::KVector(k,grade);
            kvecs_1_0.push_back(kvec);
        }
    }
    kz = 0;
    for (kx = -_NA_max; kx < _NA_max+1; ++kx) {
        if (kx == 0) continue;
        for (ky = -_NB_max; ky < _NB_max+1; ++ky) {
            if (ky == 0) continue;
            vec k = kx*_A + ky*_B + kz*_C;
            double grade = _ewdactor.Ark2Expk2(k) * kx_s1s2[std::abs(kx)] * ky_s1s2[std::abs(ky)] * kz_s1s2[std::abs(kz)] * kxyz_s1s2_norm;
            if (neutral_mode) grade = _ewdactor.Ark2Expk2(k) * EWD::int2eV / _LxLyLz;
            EWD::KVector kvec = EWD::KVector(k,grade);
            kvecs_1_0.push_back(kvec);
        }
    }
    _kvecsort._p = 1e-300;
    std::sort(kvecs_1_0.begin(), kvecs_1_0.end(), _kvecsort);
    //for (kvit = kvecs_1_0.begin(); kvit < kvecs_1_0.end(); ++kvit) {
    //    EWD::KVector kvec = *kvit;
    //    cout << endl << std::scientific << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ() << " grade " << kvec.getGrade() << flush;
    //}

    // ZERO COMPONENTS ZERO, THREE NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "  o K-space (off-axis): Applying K resonances" << flush;

    for (kx = -_NA_max; kx < _NA_max+1; ++kx) {
        if (kx == 0) continue;
        for (ky = -_NB_max; ky < _NB_max+1; ++ky) {
            if (ky == 0) continue;
            for (kz = -_NC_max; kz < _NC_max+1; ++kz) {
                if (kz == 0) continue;
                vec k = kx*_A + ky*_B + kz*_C;
                double grade = _ewdactor.Ark2Expk2(k) * kx_s1s2[std::abs(kx)] * ky_s1s2[std::abs(ky)] * kz_s1s2[std::abs(kz)] * kxyz_s1s2_norm;
                if (neutral_mode) grade = _ewdactor.Ark2Expk2(k) * EWD::int2eV / _LxLyLz;
                EWD::KVector kvec = EWD::KVector(k,grade);
                kvecs_0_0.push_back(kvec);
            }
        }
    }

    _kvecsort._p = 1e-300;
    std::sort(kvecs_0_0.begin(), kvecs_0_0.end(), _kvecsort);
    //for (kvit = kvecs_0_0.begin(); kvit < kvecs_0_0.end(); ++kvit) {
    //    EWD::KVector kvec = *kvit;
    //    cout << endl << std::scientific << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ() << " grade " << kvec.getGrade() << flush;
    //}

    _kvecs_2_0.clear();
    _kvecs_1_0.clear();
    _kvecs_0_0.clear();

    _kvecs_2_0 = kvecs_2_0;
    _kvecs_1_0 = kvecs_1_0;
    _kvecs_0_0 = kvecs_0_0;
    _kxyz_s1s2_norm = kxyz_s1s2_norm;

    _did_generate_kvectors = true;
    return;
}

void PEwald3D3D::ScanCutoff() {
    /*
    double sum = 0.0;
    double sum_ppuu = 0.0;

    CTP_LOG(logDEBUG,*_log) << flush
        << "Scan cutoff (long-range check)" << flush;

    std::ofstream ofs;
    string scan_file = "scan_cut_"
        + boost::lexical_cast<string>(_polar_qm0[0]->getId())
        + "_" + _jobType + ".tab";
    ofs.open(scan_file.c_str(), ofstream::out);

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;
    vector<PolarNb*>::iterator nit;
    vector< vector<PolarSeg*> > ::iterator vsit;

    double dR_shell = 0.5;
    double R_overhead = 1.1;
    double R_add = 3;
    double R_max = _R_co*R_overhead+R_add;
    double R_max_shell = R_max+2*_polar_cutoff+_max_int_dist_qm0;
    this->SetupMidground(R_max);
    */
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>

    double sum = 0.0;
    double sum_ppuu = 0.0;

    CTP_LOG(logDEBUG,*_log) << flush
        << "Scan cutoff (long-range check)" << flush;

    std::ofstream ofs;
    string scan_file = "scan_cut_"
        + boost::lexical_cast<string>(_polar_qm0[0]->getId())
        + "_" + _jobType + ".tab";
    ofs.open(scan_file.c_str(), ofstream::out);

    vector<PolarSeg*>::iterator sit;
    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;

    double R_factor = 1.;
    double dR_shell = 0.5;
    double R_overhead = 1.1;
    double R_add = 3;
    double R_max = R_factor*_R_co*R_overhead+R_add;
    double R_max_shell = R_max+2*_polar_cutoff+_max_int_dist_qm0;

	R_max = 64;
	R_max_shell = 70;

    vector<TinyNeighbour*> nbs;
    vector<TinyNeighbour*>::iterator tnit;
    nbs.reserve(1000000);

    int scan_na_max = ceil((R_max_shell)/maxnorm(_a)-0.5)+1;
    int scan_nb_max = ceil((R_max_shell)/maxnorm(_b)-0.5)+1;
    int scan_nc_max = ceil((R_max_shell)/maxnorm(_c)-0.5)+1;

    if (this->_shape == "xyslab") scan_nc_max = 0;

    // loop, create images, bin, interact
    for (sit = _bg_P.begin(); sit < _bg_P.end(); ++sit) {
        PolarSeg *pseg = *sit;
        // Periodic images
        for (int na = -scan_na_max; na < scan_na_max+1; ++na) {
        for (int nb = -scan_nb_max; nb < scan_nb_max+1; ++nb) {
        for (int nc = -scan_nc_max; nc < scan_nc_max+1; ++nc) {
            vec L = na*_a + nb*_b + nc*_c;
            // In foreground ?
            bool is_in_fg = _fg_table->IsInForeground(pseg->getId(),na,nb,nc);
            if (!is_in_fg) {
                bool is_within_range = false;
                // Within range ?
                // NOTE Calculate distance with respect to NEUTRAL foreground
                // to achieve consistency between threads in case neutral
                // and charged geometries differ slightly
                for (sit2 = _fg_N.begin(); sit2 < _fg_N.end(); ++sit2) {
                    vec pos_L = pseg->getPos() + L;
                    double dR_L = abs((*sit2)->getPos()-pos_L);
                    if (dR_L <= R_max_shell) {
                        is_within_range = true;
                    }
                }
                // Add if appropriate, depolarize = false
                if (is_within_range) {
                    TinyNeighbour *newNb = new TinyNeighbour(pseg, L);
                    nbs.push_back(newNb);
                }
            }
            else ;
        }}} // Loop over na, nb, nc
    } // Loop over BGP

    for (sit1 = _fg_C.begin(); sit1 != _fg_C.end(); ++sit1) {

        // Bin midground into shells
        vector< vector<TinyNeighbour*> > shelled_nbs;
        int N_shells = int(R_max_shell/dR_shell)+1;
        shelled_nbs.resize(N_shells);

        for (tnit = nbs.begin(); tnit != nbs.end(); ++tnit) {
            PolarSeg* nb = (*tnit)->_nb;
            double R = votca::tools::abs((*sit1)->getPos()-nb->getPos()-(*tnit)->_L);
            int shell_idx = int(R/dR_shell);
            shelled_nbs[shell_idx].push_back(*tnit);
        }

        // Sum over consecutive shells
        for (int sidx = 0; sidx < N_shells; ++sidx) {
            // Shell rms trackers
            double shell_sum = 0.0;
            double shell_term = 0.0;
            double shell_rms = 0.0;
            int shell_count = 0;
            // Interact with shell
            vector<TinyNeighbour*> &nb_shell = shelled_nbs[sidx];
            double shell_R = (sidx+1)*dR_shell;
            if (nb_shell.size() < 1) continue;
            EWD::triple<double> ppuu(0,0,0);
            for (tnit = nb_shell.begin(); tnit < nb_shell.end(); ++tnit) {
                PolarSeg *nb = (*tnit)->_nb;
                vec L = (*tnit)->_L;
                nb->Translate(L);
                for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                    for (pit2 = nb->begin(); pit2 < nb->end(); ++pit2) {
    //                    ppuu = _ewdactor.U12_ERFC(*(*pit1), *(*pit2));
   //                     sum_ppuu += ppuu._pp;
     //                   shell_term = ppuu._pp;
      //                  shell_sum += shell_term;
       //                 shell_rms += shell_term*shell_term;
        //                shell_count += 1;
                        _actor.BiasIndu(*(*pit1), *(*pit2));
                        double ef = _actor.E_f(*(*pit1), *(*pit2));
                        sum_ppuu += ef;
                        shell_term = ef;
                        shell_sum += shell_term;
                        shell_rms += shell_term*shell_term;
                        shell_count += 1;
                    }
                }
                nb->Translate(-L);
            }
            shell_rms = sqrt(shell_rms/shell_count)*EWD::int2eV;
            sum += shell_sum;
            CTP_LOG(logDEBUG,*_log)
                << (format("  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f eV   dER2(sum) = %4$+1.3e eV")
                % shell_R % (sum*EWD::int2eV) % nb_shell.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << flush;
            ofs
                << (format("MST DBG ...  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f eV   dER2(sum) = %4$+1.3e eV")
                % shell_R % (sum*EWD::int2eV) % nb_shell.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << endl;
        }
    }

    ofs.close();
    return;

    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

//    for (int N = 0; N < 10; ++N) {
//        for (int na=-N; na<N+1; ++na) {
//        for (int nb=-N; nb<N+1; ++nb) {
//        for (int nc=-N; nc<N+1; ++nc) {
//            if ( std::abs(na) != N && std::abs(nb) != N && std::abs(nc) != N ) continue;
//            vec L = na*_a + nb*_b + nc*_c;
//
//            for (sit1 = _bg_P.begin(); sit1 < _bg_P.end(); ++sit1) {
//                PolarSeg *pseg = *sit1;
//
//                // Skip if N==0 and ID match
//                // Shift
//                // Interact
//                // Shift back
//
//
//            }
//        }}}
//    }

//    double ax = _a.getX();
//    double by = _b.getY();
//    double cz = _c.getZ();
//    vec ax_by_cz = vec(1./ax,1./by,1./cz);
//    double norm = 1./maxnorm(ax_by_cz);
//    CTP_LOG(logDEBUG,*_log) << ax << " " << by << " " << cz << " " << norm << flush;

    /*
    // FOR EACH FOREGROUND SEGMENT (FGC) ...
    for (sit1 = _fg_C.begin(); sit1 != _fg_C.end(); ++sit1) {

        // Bin midground into shells
        vector< vector<PolarSeg*> > shelled_mg_N;
        int N_shells = int(R_max_shell/dR_shell)+1;
        shelled_mg_N.resize(N_shells);

        for (sit2 = _mg_N.begin(); sit2 != _mg_N.end(); ++sit2) {
            double R = votca::tools::abs((*sit1)->getPos()-(*sit2)->getPos());
//            vec dr = (*sit1)->getPos()-(*sit2)->getPos();
//            dr = norm*vec(dr.getX()/ax, dr.getY()/by, dr.getZ()/cz);
//            double R = votca::tools::maxnorm(dr);

            int shell_idx = int(R/dR_shell);
            shelled_mg_N[shell_idx].push_back(*sit2);
        }

        // Sum over consecutive shells
        for (int sidx = 0; sidx < N_shells; ++sidx) {
            // Shell rms trackers
            double shell_sum = 0.0;
            double shell_term = 0.0;
            double shell_rms = 0.0;
            int shell_count = 0;
            // Interact with shell
            vector<PolarSeg*> &shell_mg = shelled_mg_N[sidx];
            double shell_R = (sidx+1)*dR_shell;
            if (shell_mg.size() < 1) continue;
            EWD::triple<double> ppuu(0,0,0);
            for (sit2 = shell_mg.begin(); sit2 < shell_mg.end(); ++sit2) {
                for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                    for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                        _actor.BiasIndu(*(*pit1), *(*pit2));
                        double ef = _actor.E_f(*(*pit1), *(*pit2));
                        sum_ppuu += ef;
                        shell_term = ef;
                        shell_sum += shell_term;
                        shell_rms += shell_term*shell_term;
                        shell_count += 1;
                    }
                }
            }
            shell_rms = sqrt(shell_rms/shell_count)*EWD::int2eV;
            sum += shell_sum;
            CTP_LOG(logDEBUG,*_log)
                << (format("  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f eV   dER2(sum) = %4$+1.3e eV")
                % shell_R % (sum*EWD::int2eV) % shell_mg.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << flush;
            ofs
                << (format("MST DBG ...  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f eV   dER2(sum) = %4$+1.3e eV")
                % shell_R % (sum*EWD::int2eV) % shell_mg.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << endl;
        }
    }

    ofs.close();
    */
    return;
}

EWD::triple<> PEwald3D3D::ConvergeRealSpaceSum(vector<PolarSeg*> &target) {

    double sum = 0.0;
    double sum_pp = 0.0;
    double sum_pu = 0.0;
    double sum_uu = 0.0;
    _converged_R = false;

    CTP_LOG(logDEBUG,*_log) << flush
        << "R-space energy via midground" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;
    vector<PolarNb*>::iterator nit;

    // ENERGY - REUSE NEIGHBOURS ?
    if (_did_field_pin_R_shell) {
        EWD::triple<double> ppuu(0,0,0);
        for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
            vector<PolarNb*> &nbs = (*sit1)->PolarNbs();
            for (nit = nbs.begin(); nit != nbs.end(); ++nit) {
                PolarSeg *nb = (*nit)->getNb();
                for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                    for (pit2 = nb->begin(); pit2 < nb->end(); ++pit2) {
                        ppuu = _ewdactor.U12_ERFC(*(*pit1), *(*pit2));
                        sum_pp += ppuu._pp;
                        sum_pu += ppuu._pu;
                        sum_uu += ppuu._uu;
//                        _actor.BiasIndu(*(*pit1), *(*pit2));
//                        double ef = _actor.E_f(*(*pit1), *(*pit2));
//                        sum_pp += ef;
                    }
                }
            }
            if (tools::globals::verbose){ CTP_LOG(logDEBUG,*_log)
                << (format("  o Id = %5$-4d Rc = %1$+02.7f   |MGN| = %2$5d   dF(rms) = %3$+1.3e V/m   [1eA => %4$+1.3e eV]")
                % -1.0 % nbs.size() % -1.0  % -1.0 % ((*sit1)->getId())).str() << flush;
	    }
        }
        _converged_R = true;
//        cout << endl << "XINTERACTOR " << _actor.getEPP() << " " << _actor.getEPU() << " "<< _actor.getEUU() << flush;
    }

    // ENERGY - REGENERATE NEIGHBOURS ?
    else {
        double dR_shell = 0.5;
        double R_overhead = 1.1;
        double R_add = 3;
        double R_max = _R_co*R_overhead+R_add;
        double R_max_shell = R_max+2*_polar_cutoff+_max_int_dist_qm0;
        this->SetupMidground(R_max);

        // FOR EACH FOREGROUND SEGMENT (FGC) ...
        unsigned int energy_converged_count = 0;
        for (sit1 = target.begin(); sit1 != target.end(); ++sit1) {
            (*sit1)->ClearPolarNbs();

            // Bin midground into shells
            vector< vector<PolarSeg*> > shelled_mg_N;
            int N_shells = int(R_max_shell/dR_shell)+1;
            shelled_mg_N.resize(N_shells);

            for (sit2 = _mg_N.begin(); sit2 != _mg_N.end(); ++sit2) {
                double R = votca::tools::abs((*sit1)->getPos()-(*sit2)->getPos());
                int shell_idx = int(R/dR_shell);
                shelled_mg_N[shell_idx].push_back(*sit2);
            }

            // Sum over consecutive shells
            for (int sidx = 0; sidx < N_shells; ++sidx) {
                // Shell rms trackers
                double shell_sum = 0.0;
                double shell_term = 0.0;
                double shell_rms = 0.0;
                int shell_count = 0;
                // Interact with shell
                vector<PolarSeg*> &shell_mg = shelled_mg_N[sidx];
                double shell_R = (sidx+1)*dR_shell;
                if (shell_mg.size() < 1) continue;
                EWD::triple<double> ppuu(0,0,0);
                for (sit2 = shell_mg.begin(); sit2 < shell_mg.end(); ++sit2) {
                    for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                        for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                            ppuu = _ewdactor.U12_ERFC(*(*pit1), *(*pit2));
                            sum_pp += ppuu._pp;
                            sum_pu += ppuu._pu;
                            sum_uu += ppuu._uu;
                            shell_term = ppuu._pp + ppuu._pu + ppuu._uu;
                            shell_sum += shell_term;
                            shell_rms += shell_term*shell_term;
                            shell_count += 1;
//                            _actor.BiasIndu(*(*pit1), *(*pit2));
//                            double ef = _actor.E_f(*(*pit1), *(*pit2));
//                            sum_pp += ef;
//                            shell_term = ef;
//                            shell_sum += shell_term;
//                            shell_rms += shell_term*shell_term;
//                            shell_count += 1;
                        }
                    }
                }
                shell_rms = sqrt(shell_rms/shell_count)*EWD::int2eV;
                sum += shell_sum;
                if (tools::globals::verbose){ CTP_LOG(logDEBUG,*_log)
                    << (format("  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f eV   dER2(sum) = %4$+1.3e eV")
                    % shell_R % (sum*EWD::int2eV) % shell_mg.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << flush;
		}

                if (shell_rms*shell_count <= _crit_dE && shell_R >= _R_co) {
                    energy_converged_count += 1;
                    if (tools::globals::verbose){ CTP_LOG(logDEBUG,*_log)
                        << (format("  :: ID = %2$-4d : Converged to precision as of Rc = %1$+1.3f nm")
                        % shell_R % (*sit1)->getId()) << flush;
		    }
                    break;
                }
            }
        }

//        cout << endl << "XINTERACTOR " << _actor.getEPP() << " " << _actor.getEPU() << " "<< _actor.getEUU() << flush;

        if (energy_converged_count == target.size()) {
            CTP_LOG(logDEBUG,*_log)
                << (format(":::: Converged to precision (%1$d items)")
                % energy_converged_count) << flush;
            _converged_R = true;
        }
        else if (energy_converged_count < target.size()) {
            CTP_LOG(logERROR,*_log) << "ERROR Energy not converged on "
                << target.size() - energy_converged_count << " counts." << flush;
            _converged_R = false;
        }
        else {
            assert(false);
        }
    }

    //boost::timer::auto_cpu_timer t0(*_log);
    //t0.start();
    //t0.stop();
    //t0.report();

    return EWD::triple<>(sum_pp, sum_pu, sum_uu);
}

EWD::triple<> PEwald3D3D::ConvergeReciprocalSpaceSum(vector<PolarSeg*> &target) {

    // ATTENTION K-vectors are generated based on an interaction-energy
    //           criterion between FGC and BGP. Hence, the <target> density
    //           should at the very least be located within the space
    //           covered by FGC.
    if (!_did_generate_kvectors)
        this->GenerateKVectors(_fg_C, _bg_P);
    vector< EWD::KVector >::iterator kvit;

    double sum_re = 0.0;
    double sum_re_pp = 0.0;
    double sum_re_pu = 0.0;
    double sum_re_uu = 0.0;
    double sum_im = 0.0;
    _converged_K = false;

    // TWO COMPONENTS ZERO, ONE NON-ZERO
    CTP_LOG(logINFO,*_log) << flush
        << "K-lines through origin: Checking K resonances" << flush;
    for (kvit = _kvecs_2_0.begin(); kvit < _kvecs_2_0.end(); ++kvit) {
        EWD::KVector kvec = *kvit;
        double Ak = _ewdactor.Ark2Expk2(kvec.getK());
        EWD::triple<EWD::cmplx> ppuu = _ewdactor.S1S2(kvec.getK(), target, _bg_P);
        sum_re_pp += Ak*ppuu._pp._re;
        sum_re_pu += Ak*ppuu._pu._re;
        sum_re_uu += Ak*ppuu._uu._re;
        sum_re += Ak*(ppuu._pp._re + ppuu._pu._re + ppuu._uu._re);
        sum_im += Ak*(ppuu._pp._im + ppuu._pu._im + ppuu._uu._im);
    }

    CTP_LOG(logINFO,*_log)
        << (format("  :: RE %1$+1.7e IM %2$+1.7e")
            % (sum_re/_LxLyLz*EWD::int2eV)
            % (sum_im/_LxLyLz*EWD::int2eV)).str() << flush;

    // ONE COMPONENT ZERO, TWO NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-planes through origin: Applying K resonances" << flush;

    double crit_grade = 1. * _kxyz_s1s2_norm;
    bool converged12 = false;
    kvit = _kvecs_1_0.begin();
    while (!converged12 && kvit < _kvecs_1_0.end()) {

        double de_this_shell = 0.0;
        int shell_count = 0;
        bool increment_grade = true;

        while (kvit < _kvecs_1_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            if (shell_count > 1000) {
                increment_grade = false;
                break;
            }
            EWD::triple<EWD::cmplx> ppuu = _ewdactor.AS1S2(kvec.getK(), target, _bg_P);

            sum_re_pp += ppuu._pp._re;
            sum_re_pu += ppuu._pu._re;
            sum_re_uu += ppuu._uu._re;
            sum_re += ppuu._pp._re + ppuu._pu._re + ppuu._uu._re;
            sum_im += ppuu._pp._im + ppuu._pu._im + ppuu._uu._im;

            //de_this_shell += sqrt(as1s2._re*as1s2._re + as1s2._im*as1s2._im);
            de_this_shell += ppuu._pp._re + ppuu._pu._re + ppuu._uu._re;
            //cout << endl << std::showpos << std::scientific
            //   << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ()
            //   << " grade " << kvec.getGrade() << " re " << (as1s2._re/_LxLyLz*_ewdactor.int2eV) << flush;
            ++kvit;
            ++shell_count;
        }
        de_this_shell = (de_this_shell < 0.) ? -de_this_shell : de_this_shell;

        if (shell_count > 0){ CTP_LOG(logDEBUG,*_log)
             << (format("  o M = %1$04d   G = %2$+1.3e   dE(rms) = %3$+1.3e eV")
             % shell_count
             % crit_grade
             % (de_this_shell/_LxLyLz*EWD::int2eV)).str() << flush;
	}

        if (shell_count > 10 && de_this_shell/_LxLyLz*EWD::int2eV < _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sum_re/_LxLyLz*EWD::int2eV)
                % (sum_im/_LxLyLz*EWD::int2eV)).str() << flush;
            converged12 = true;
        }

        if (increment_grade) crit_grade /= 10.0;
    }

    // ZERO COMPONENTS ZERO, THREE NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-space (off-axis): Applying K resonances" << flush;

    crit_grade = 1. * _kxyz_s1s2_norm;
    double converged03 = false;
    kvit = _kvecs_0_0.begin();
    while (!converged03 && kvit < _kvecs_0_0.end()) {

        double de_this_shell = 0.0;
        int shell_count = 0;
        bool increment_grade = true;

        while (kvit < _kvecs_0_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            if (shell_count > 1000) {
                increment_grade = false;
                break;
            }
            EWD::triple<EWD::cmplx> ppuu = _ewdactor.AS1S2(kvec.getK(), target, _bg_P);

            sum_re_pp += ppuu._pp._re;
            sum_re_pu += ppuu._pu._re;
            sum_re_uu += ppuu._uu._re;
            sum_re += ppuu._pp._re + ppuu._pu._re + ppuu._uu._re;
            sum_im += ppuu._pp._im + ppuu._pu._im + ppuu._uu._im;

            //de_this_shell += sqrt(as1s2._re*as1s2._re + as1s2._im*as1s2._im);
            de_this_shell += ppuu._pp._re + ppuu._pu._re + ppuu._uu._re;
            //cout << endl << std::showpos << std::scientific
            //   << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ()
            //   << " grade " << kvec.getGrade() << " re " << (as1s2._re/_LxLyLz*_ewdactor.int2eV) << flush;
            ++kvit;
            ++shell_count;
        }
        de_this_shell = (de_this_shell < 0.) ? -de_this_shell : de_this_shell;

        if (shell_count > 0){ CTP_LOG(logDEBUG,*_log)
             << (format("  o M = %1$04d   G = %2$+1.3e   dE(rms) = %3$+1.3e eV")
             % shell_count
             % crit_grade
             % (de_this_shell/_LxLyLz*EWD::int2eV)).str() << flush;
	}

        if (shell_count > 10 && de_this_shell/_LxLyLz*EWD::int2eV < _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sum_re/_LxLyLz*EWD::int2eV)
                % (sum_im/_LxLyLz*EWD::int2eV)).str() << flush;
            converged03 = true;
        }

        if (increment_grade) crit_grade /= 10.0;
    }

    _converged_K = converged12 && converged03;

    if (_converged_K)
        CTP_LOG(logINFO,*_log)
            << (format(":::: Converged to precision, {0-2}, {1-2}, {0-3}."))
            << flush;
    else ;

    return EWD::triple<>(sum_re_pp/_LxLyLz, sum_re_pu/_LxLyLz, sum_re_uu/_LxLyLz);
}

EWD::triple<> PEwald3D3D::CalculateShapeCorrection(vector<PolarSeg*> &target) {

    CTP_LOG(logDEBUG,*_log) << flush
        << "Energy correction terms" << flush;
    CTP_LOG(logDEBUG,*_log)
        << "  o Shape-correction to energy, using '" << _shape << "'" << flush;

    EWD::triple<double> ppuu = _ewdactor.U12_ShapeTerm(target, _bg_P,
        _shape, _LxLyLz, _log);
    double sum_pp = ppuu._pp;
    double sum_pu = ppuu._pu;
    double sum_uu = ppuu._uu;

//    vector<PolarSeg*>::iterator sit1;
//    vector<APolarSite*> ::iterator pit1;
//    vector<PolarSeg*>::iterator sit2;
//    vector<APolarSite*> ::iterator pit2;
//
//    double EJ = 0.0;

//    if (_shape == "xyslab") {
//        EWD::triple<double> ppuu(0,0,0);
//        for (sit1 = _fg_C.begin(); sit1 < _fg_C.end(); ++sit1) {
//           for (sit2 = _bg_P.begin(); sit2 < _bg_P.end(); ++sit2) {
//              for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
//                 for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
//                    ppuu = _ewdactor.U12_XYSlab(*(*pit1), *(*pit2));
//                    sum_pp += ppuu._pp;
//                    sum_pu += ppuu._pu;
//                    sum_uu += ppuu._uu;
//                 }
//              }
//           }
//        }
//        sum_pp *= -2*M_PI/_LxLyLz;
//        sum_pu *= -2*M_PI/_LxLyLz;
//        sum_uu *= -2*M_PI/_LxLyLz;
//        EJ = sum_pp + sum_pu + sum_uu;
//
//        // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//        EWD::triple<double> ppuu_fast = _ewdactor.U12_XYSlab_Factorized(_fg_C, _bg_P);
//        double sum_pp_fast = ppuu_fast._pp;
//        double sum_pu_fast = ppuu_fast._pu;
//        double sum_uu_fast = ppuu_fast._uu;
//        sum_pp_fast *= -4*M_PI/_LxLyLz;
//        sum_pu_fast *= -4*M_PI/_LxLyLz;
//        sum_uu_fast *= -4*M_PI/_LxLyLz;
//        cout << endl << "Slow: " << sum_pp << " " << sum_pu << " " << sum_uu << flush;
//        cout << endl << "Fast: " << sum_pp_fast << " " << sum_pu_fast << " " << sum_uu_fast << flush;
//
//        EWD::triple<double> ppuu_cube = _ewdactor.U12_ShapeTerm_Factorized(_fg_C, _bg_P, _shape, _LxLyLz);
//        double sum_pp_cube = ppuu_cube._pp;
//        double sum_pu_cube = ppuu_cube._pu;
//        double sum_uu_cube = ppuu_cube._uu;
//        cout << endl << "Cube: " << sum_pp_cube << " " << sum_pu_cube << " " << sum_uu_cube << flush;
//        // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//    }
//    else {
//        CTP_LOG(logERROR,*_log)
//            << (format("Shape %1$s not implemented. Setting EJ = 0.0 ...")
//            % _shape) << flush;
//        EJ = 0.0;
//    }

    return EWD::triple<>(sum_pp, sum_pu, sum_uu);
    //return EJ;
}

EWD::triple<> PEwald3D3D::CalculateForegroundCorrection(vector<PolarSeg*> &target) {

    CTP_LOG(logDEBUG,*_log)
        << "  o Foreground-correction to energy via FGN" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;
    //[-Wunused-variable]
    //double EC = 0.0;
    double sum_pp = 0.0;
    double sum_pu = 0.0;
    double sum_uu = 0.0;

    EWD::triple<double> ppuu(0,0,0);
    for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
        for (sit2 = _fg_N.begin(); sit2 < _fg_N.end(); ++sit2) {
            for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                    ppuu = _ewdactor.U12_ERF(*(*pit1), *(*pit2));
                    sum_pp += ppuu._pp;
                    sum_pu += ppuu._pu;
                    sum_uu += ppuu._uu;
                }
            }
        }
    }

    //EC = sum_pp + sum_pu + sum_uu;
    return EWD::triple<>(sum_pp, sum_pu, sum_uu);
    //return EC;
}

void PEwald3D3D::Field_ConvergeRealSpaceSum() {
    //[-Wunused-variable]
    //double sum = 0.0;

    _field_converged_R = false;

    CTP_LOG(logDEBUG,*_log) << flush
        << "R-space fields via midground" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;

    // GENERATE MIDGROUND & ASSEMBLE IT INTO SHELLS
    double dR_shell = 0.5;
    double R_overhead = 1.1;
    double R_add = 3;
    double R_max = _R_co*R_overhead+R_add;
    double R_max_shell = R_max+2*_polar_cutoff+_max_int_dist_qm0;
    this->SetupMidground(R_max);

    // FOR EACH FOREGROUND SEGMENT (FGC) ...
    unsigned int field_converged_count = 0;
    for (sit1 = _fg_C.begin(); sit1 != _fg_C.end(); ++sit1) {
        (*sit1)->ClearPolarNbs();

        // Bin midground into shells
        vector< vector<PolarSeg*> > shelled_mg_N;
        int N_shells = int(R_max_shell/dR_shell)+1;
        shelled_mg_N.resize(N_shells);

        for (sit2 = _mg_N.begin(); sit2 != _mg_N.end(); ++sit2) {
            double R = votca::tools::abs((*sit1)->getPos()-(*sit2)->getPos());
            int shell_idx = int(R/dR_shell);
            shelled_mg_N[shell_idx].push_back(*sit2);
        }

        // Sum over consecutive shells
        for (int sidx = 0; sidx < N_shells; ++sidx) {
            // Shell rms trackers
            double shell_rms = 0.0;
            int shell_count = 0;
            // Interact with shell
            vector<PolarSeg*> &shell_mg = shelled_mg_N[sidx];
            double shell_R = (sidx+1)*dR_shell;
            if (shell_mg.size() < 1) continue;
            for (sit2 = shell_mg.begin(); sit2 != shell_mg.end(); ++sit2) {
                if (_save_nblist) (*sit1)->AddNewPolarNb(*sit2);
                for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                    for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                        shell_rms += _ewdactor.FPU12_ERFC_At_By(*(*pit1), *(*pit2));
                        shell_count += 1;
//                        vec f0 = (*pit1)->getFieldP();
//                        _actor.BiasStat(*(*pit1), *(*pit2));
//                        _actor.FieldPerm(*(*pit1), *(*pit2));
//                        vec f1 = (*pit1)->getFieldP();
//                        vec df = f1-f0;
//                        shell_rms += df*df;
//                        shell_count += 1;
                    }
                }
            }

            // Assert convergence: Energy of dipole of size 0.1*e*nm summed over shell
            shell_rms = sqrt(shell_rms/shell_count)*EWD::int2V_m;
            double e_measure = shell_rms*1e-10*shell_count; //

            if (tools::globals::verbose){ CTP_LOG(logDEBUG,*_log)
                << (format("  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %2$5d   dF(rms) = %3$+1.3e V/m   [1eA => %4$+1.3e eV]")
                % shell_R % shell_mg.size() % shell_rms  % e_measure % ((*sit1)->getId())).str() << flush;
	    }

            if (e_measure <= _crit_dE && shell_R >= _R_co) {
                field_converged_count += 1;
                if (tools::globals::verbose){ CTP_LOG(logDEBUG,*_log)
                    << (format("  :: ID = %2$-4d Converged to precision as of Rc = %1$+1.3f nm")
                    % shell_R % (*sit1)->getId()) << flush;
		}
                break;
            }
        }
    }

    if (field_converged_count == _fg_C.size()) {
        CTP_LOG(logDEBUG,*_log)
            << (format(":::: Converged to precision (%1$d items)")
            % field_converged_count) << flush;
        _field_converged_R = true;
    }
    else if (field_converged_count < _fg_C.size()) {
        CTP_LOG(logERROR,*_log) << "ERROR Field not converged on "
            << _fg_C.size() - field_converged_count << " counts." << flush;
        _field_converged_R = false;
    }
    else {
        assert(false);
    }

	if (_save_nblist)
        _did_field_pin_R_shell = true;
    else
        _did_field_pin_R_shell = false;

//    // THERE IS A MEMORY ISSUE HERE - VERY STRANGE
//    // Change 18000 to 20000 and the leak disappears!?
//    double sum = 0.0;
//    _field_converged_R = false;
//    CTP_LOG(logDEBUG,*_log) << flush
//        << "R-space fields via midground" << flush;
//
//    vector<PolarSeg*>::iterator sit1;
//
//    // FOR EACH FOREGROUND SEGMENT (FGC) ...
//    int field_converged_count = 0;
//    for (sit1 = _fg_C.begin(); sit1 != _fg_C.end(); ++sit1) {
//
//        for (int cnt = 0; cnt < 18000; ++cnt) {
//            PolarNb *new_nb = (*sit1)->AddNewPolarNb(*sit1);
//        }
//
//        (*sit1)->ClearPolarNbs();
//        assert((*sit1)->PolarNbs().size() == 0);
//        cout << endl << "sit1 = ID " << (*sit1)->getId() << flush;
//
//
//    }
//    _did_field_pin_R_shell = true;

    return;
}

void PEwald3D3D::Field_ConvergeReciprocalSpaceSum() {

    this->GenerateKVectors(_fg_C, _bg_P);
    double sum_re = 0.0;
    double sum_im = 0.0;
    _field_converged_K = false;
    double rV = 1./_LxLyLz;

    vector< EWD::KVector >::iterator kvit;

    // TWO COMPONENTS ZERO, ONE NON-ZERO
    CTP_LOG(logINFO,*_log) << flush
        << "K-lines through origin: Checking K resonances" << flush;
    for (kvit = _kvecs_2_0.begin(); kvit < _kvecs_2_0.end(); ++kvit) {
        EWD::KVector kvec = *kvit;
        EWD::cmplx f_as1s2 = _ewdactor.FPU12_AS1S2_At_By(kvec.getK(), _fg_C, _bg_P, rV);
        sum_re += sqrt(f_as1s2._re);
        sum_im += f_as1s2._im;
    }

    CTP_LOG(logINFO,*_log)
        << (format("  :: RE %1$+1.7e IM %2$+1.7e")
            % (sum_re*EWD::int2V_m)
            % (sum_im*EWD::int2V_m)).str() << flush;

    // ONE COMPONENT ZERO, TWO NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-planes through origin: Applying K resonances" << flush;

    double crit_grade = 1. * _kxyz_s1s2_norm;
    bool converged12 = false;
    kvit = _kvecs_1_0.begin();
    while (!converged12 && kvit < _kvecs_1_0.end()) {

        double shell_rms = 0.0;
        int rms_count = 0;

        while (kvit < _kvecs_1_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            EWD::cmplx f_as1s2 = _ewdactor.FPU12_AS1S2_At_By(kvec.getK(), _fg_C, _bg_P, rV);
            sum_re += f_as1s2._re;
            sum_im += f_as1s2._im;
            shell_rms += f_as1s2._re;
            //cout << endl << std::showpos << std::scientific
            //   << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ()
            //   << " grade " << kvec.getGrade() << " re " << (as1s2._re/_LxLyLz*_ewdactor.int2eV) << flush;
            ++kvit;
            ++rms_count;
        }
        shell_rms = (rms_count > 0) ? sqrt(shell_rms/rms_count)*EWD::int2V_m : 0.0;
        double e_measure = shell_rms*1e-10*rms_count;

        if (rms_count > 0){ CTP_LOG(logDEBUG,*_log)
             << (format("  o M = %1$04d   G = %2$+1.3e   dF(rms) = %3$+1.3e V/m   [1eA => %4$+1.3e eV]")
             % rms_count
             % crit_grade
             % shell_rms
             % e_measure).str() << flush;
	}

        if (rms_count > 10 && e_measure <= _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sqrt(sum_re)*EWD::int2V_m)
                % (sum_im*EWD::int2V_m)).str() << flush;
            converged12 = true;
        }

        crit_grade /= 10.0;
    }

    // ZERO COMPONENTS ZERO, THREE NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-space (off-axis): Applying K resonances" << flush;

    crit_grade = 1. * _kxyz_s1s2_norm;
    double converged03 = false;
    kvit = _kvecs_0_0.begin();
    while (!converged03 && kvit < _kvecs_0_0.end()) {

        double shell_rms = 0.0;
        int rms_count = 0;

        while (kvit < _kvecs_0_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            EWD::cmplx f_as1s2 = _ewdactor.FPU12_AS1S2_At_By(kvec.getK(), _fg_C, _bg_P, rV);
            sum_re += f_as1s2._re;
            sum_im += f_as1s2._im;
            shell_rms += f_as1s2._re;
            //cout << endl << std::showpos << std::scientific
            //   << kvec.getX() << " " << kvec.getY() << " " << kvec.getZ()
            //   << " grade " << kvec.getGrade() << " re " << (as1s2._re/_LxLyLz*_ewdactor.int2eV) << flush;
            ++kvit;
            ++rms_count;
        }
        shell_rms = (rms_count > 0) ? sqrt(shell_rms/rms_count)*EWD::int2V_m : 0.0;
        double e_measure = shell_rms*1e-10*rms_count;

        if (rms_count > 0){ CTP_LOG(logDEBUG,*_log)
             << (format("  o M = %1$04d   G = %2$+1.3e   dF(rms) = %3$+1.3e V/m   [1eA => %4$+1.3e eV]")
             % rms_count
             % crit_grade
             % shell_rms
             % e_measure).str() << flush;
	}

        if (rms_count > 10 && e_measure <= _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sqrt(sum_re)*EWD::int2V_m)
                % (sum_im*EWD::int2V_m)).str() << flush;
            converged03 = true;
        }

        crit_grade /= 10.0;
    }

    _field_converged_K = converged12 && converged03;

    if (_field_converged_K)
        CTP_LOG(logINFO,*_log)
            << (format(":::: Converged to precision, {0-2}, {1-2}, {0-3}."))
            << flush;
    else ;

    return;
}

void PEwald3D3D::Field_CalculateForegroundCorrection() {

    CTP_LOG(logDEBUG,*_log)
        << "  o Foreground-correction to fields via FGN" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;

    double rms = 0.0;
    int rms_count = 0;
    for (sit1 = _fg_C.begin(); sit1 < _fg_C.end(); ++sit1) {
        for (sit2 = _fg_N.begin(); sit2 < _fg_N.end(); ++sit2) {
            for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                    rms += _ewdactor.FPU12_ERF_At_By(*(*pit1), *(*pit2));
                    rms_count += 1;
                }
            }
        }
    }
    rms = sqrt(rms/rms_count)*EWD::int2V_m;

    return;
}

void PEwald3D3D::Field_CalculateShapeCorrection() {

    CTP_LOG(logDEBUG,*_log) << flush
        << "Field correction terms" << flush;
    CTP_LOG(logDEBUG,*_log)
        << "  o Shape-correction to fields, using '" << _shape << "'" << flush;

    _ewdactor.FPU12_ShapeField_At_By(_fg_C, _bg_P, _shape, _LxLyLz);

//    vector<PolarSeg*>::iterator sit1;
//    vector<APolarSite*> ::iterator pit1;
//    vector<PolarSeg*>::iterator sit2;
//    vector<APolarSite*> ::iterator pit2;
//
//    double rms = 0.0;
//    int rms_count = 0;
//    if (_shape == "xyslab") {
//        double TwoPi_V = 2*M_PI/_LxLyLz;
//
////        for (sit1 = _fg_C.begin(); sit1 < _fg_C.end(); ++sit1) {
////           for (sit2 = _bg_P.begin(); sit2 < _bg_P.end(); ++sit2) {
////              for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
////                 for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
////                    rms += _ewdactor.F12_XYSlab_At_By(*(*pit1), *(*pit2), TwoPi_V);
////                    rms_count += 1;
////                 }
////              }
////           }
////        }
//
//        _ewdactor.FPU12_XYSlab_ShapeField_At_By(_fg_C, _bg_P, TwoPi_V);
//
//    }
//    else {
//        CTP_LOG(logERROR,*_log)
//            << (format("Shape %1$s not implemented. Setting EJ = 0.0 ...")
//            % _shape) << flush;
//    }
//    rms = sqrt(rms/rms_count)*EWD::int2V_m;

    return;
}

void PEwald3D3D::Potential_ConvergeRealSpaceSum(vector<PolarSeg*> &target) {

    double sum = 0.0;
    double sum_phi = 0.0;
    _potential_converged_R = false;

    CTP_LOG(logDEBUG,*_log) << flush
        << "R-space potentials via midground" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;
    vector<PolarNb*>::iterator nit;

    bool neighbours_stored = false;
    for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
        int nb_count = (*sit1)->PolarNbs().size();
        if (nb_count > 0) {
            neighbours_stored = true;
            break;
        }
    }

    // ENERGY - REUSE NEIGHBOURS ?
    if (neighbours_stored) {
        for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
            vector<PolarNb*> &nbs = (*sit1)->PolarNbs();
            for (nit = nbs.begin(); nit != nbs.end(); ++nit) {
                PolarSeg *nb = (*nit)->getNb();
                for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                    for (pit2 = nb->begin(); pit2 < nb->end(); ++pit2) {
                        double phi = _ewdactor.PhiPU12_ERFC_At_By(*(*pit1), *(*pit2));
                        sum_phi += phi;
                    }
                }
            }
            if (tools::globals::verbose) {
                CTP_LOG(logDEBUG,*_log)
                << (format("  o Id = %5$-4d Rc = %1$+02.7f   |MGN| = %2$5d   dF(rms) = %3$+1.3e V/m   [1eA => %4$+1.3e eV]")
                % -1.0 % nbs.size() % -1.0  % -1.0 % ((*sit1)->getId())).str() << flush;
            }
        }
        _potential_converged_R = true;
    }

    // ENERGY - REGENERATE NEIGHBOURS ?
    else {
        double dR_shell = 0.5;
        double R_overhead = 1.1;
        double R_add = 3;
        double R_max = _R_co*R_overhead+R_add;
        double R_max_shell = R_max+2*_polar_cutoff+_max_int_dist_qm0;
        this->SetupMidground(R_max);

        // FOR EACH FOREGROUND SEGMENT (FGC) ...
        unsigned int energy_converged_count = 0;
        for (sit1 = target.begin(); sit1 != target.end(); ++sit1) {
            (*sit1)->ClearPolarNbs();

            // Bin midground into shells
            vector< vector<PolarSeg*> > shelled_mg_N;
            int N_shells = int(R_max_shell/dR_shell)+1;
            shelled_mg_N.resize(N_shells);

            for (sit2 = _mg_N.begin(); sit2 != _mg_N.end(); ++sit2) {
                double R = votca::tools::abs((*sit1)->getPos()-(*sit2)->getPos());
                int shell_idx = int(R/dR_shell);
                shelled_mg_N[shell_idx].push_back(*sit2);
            }

            // Sum over consecutive shells
            for (int sidx = 0; sidx < N_shells; ++sidx) {
                // Shell rms trackers
                double shell_sum = 0.0;
                double shell_term = 0.0;
                double shell_rms = 0.0;
                int shell_count = 0;
                // Interact with shell
                vector<PolarSeg*> &shell_mg = shelled_mg_N[sidx];
                double shell_R = (sidx+1)*dR_shell;
                if (shell_mg.size() < 1) continue;
                EWD::triple<double> ppuu(0,0,0);
                for (sit2 = shell_mg.begin(); sit2 < shell_mg.end(); ++sit2) {
                    for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                        for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                            double phi = _ewdactor.PhiPU12_ERFC_At_By(*(*pit1), *(*pit2));
                            sum_phi += phi;
                            shell_term = phi;
                            shell_sum += shell_term;
                            shell_rms += shell_term*shell_term;
                            shell_count += 1;
                        }
                    }
                }
                shell_rms = sqrt(shell_rms/shell_count)*EWD::int2eV;
                sum += shell_sum;
                if (tools::globals::verbose) {
                    CTP_LOG(logDEBUG,*_log)
                    << (format("  o ID = %5$-4d Rc = %1$+02.7f   |MGN| = %3$5d   ER = %2$+1.7f V   dER2(sum) = %4$+1.3e V")
                    % shell_R % (sum*EWD::int2eV) % shell_mg.size() % (shell_rms*shell_count) % (*sit1)->getId()).str() << flush;
                }

                if (shell_rms*shell_count <= _crit_dE && shell_R >= _R_co) {
                    energy_converged_count += 1;
                    if (tools::globals::verbose) {
                        CTP_LOG(logDEBUG,*_log)
                        << (format("  :: ID = %2$-4d : Converged to precision as of Rc = %1$+1.3f nm")
                        % shell_R % (*sit1)->getId()) << flush;
                    }
                    break;
                }
            }
        }

        if (energy_converged_count == target.size()) {
            CTP_LOG(logDEBUG,*_log)
                << (format(":::: Converged to precision (%1$d items)")
                % energy_converged_count) << flush;
            _potential_converged_R = true;
        }
        else if (energy_converged_count < target.size()) {
            CTP_LOG(logERROR,*_log) << "ERROR Energy not converged on "
                << target.size() - energy_converged_count << " counts." << flush;
            _potential_converged_R = false;
        }
        else {
            assert(false);
        }
    }

    //boost::timer::auto_cpu_timer t0(*_log);
    //t0.start();
    //t0.stop();
    //t0.report();

    return;
}

void PEwald3D3D::Potential_ConvergeReciprocalSpaceSum(vector<PolarSeg*> &target) {

    // ATTENTION K-vectors are generated based on an interaction-energy
    //           criterion between FGC and BGP. Hence, the <target> density
    //           should at the very least be located within the space
    //           covered by FGC.
    if (!_did_generate_kvectors)
        this->GenerateKVectors(_fg_C, _bg_P);
    vector< EWD::KVector >::iterator kvit;

    double sum_re = 0.0;
    double sum_im = 0.0;
    _potential_converged_K = false;
    double rV = 1./_LxLyLz;

    // TWO COMPONENTS ZERO, ONE NON-ZERO
    CTP_LOG(logINFO,*_log) << flush
        << "K-lines through origin: Checking K resonances" << flush;
    for (kvit = _kvecs_2_0.begin(); kvit < _kvecs_2_0.end(); ++kvit) {
        EWD::KVector kvec = *kvit;
        EWD::cmplx f_as1s2 = _ewdactor.PhiPU12_AS1S2_At_By(kvec.getK(), target, _bg_P, rV);
        sum_re += sqrt(f_as1s2._re);
        sum_im += f_as1s2._im;
    }

    CTP_LOG(logINFO,*_log)
        << (format("  :: RE %1$+1.7e IM %2$+1.7e")
            % (sum_re*EWD::int2eV)
            % (sum_im*EWD::int2eV)).str() << flush;

    // ONE COMPONENT ZERO, TWO NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-planes through origin: Applying K resonances" << flush;

    double crit_grade = 1. * _kxyz_s1s2_norm;
    bool converged12 = false;
    kvit = _kvecs_1_0.begin();
    while (!converged12 && kvit < _kvecs_1_0.end()) {

        double shell_rms = 0.0;
        int rms_count = 0;

        while (kvit < _kvecs_1_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            EWD::cmplx f_as1s2 = _ewdactor.PhiPU12_AS1S2_At_By(kvec.getK(), target, _bg_P, rV);
            sum_re += f_as1s2._re;
            sum_im += f_as1s2._im;
            shell_rms += f_as1s2._re;
            ++kvit;
            ++rms_count;
        }
        shell_rms = (rms_count > 0) ? sqrt(shell_rms/rms_count)*EWD::int2eV : 0.0;
        double e_measure = shell_rms*rms_count;

        if (rms_count > 0) {
            CTP_LOG(logDEBUG,*_log)
                << (format("  o M = %1$04d   G = %2$+1.3e   dPhi(rms) = %3$+1.3e V   [1e => %4$+1.3e eV]")
                % rms_count
                % crit_grade
                % shell_rms
                % e_measure).str() << flush;
        }

        if (rms_count > 10 && e_measure <= _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sqrt(sum_re)*EWD::int2eV)
                % (sum_im*EWD::int2eV)).str() << flush;
            converged12 = true;
        }

        crit_grade /= 10.0;
    }

    // ZERO COMPONENTS ZERO, THREE NON-ZERO
    CTP_LOG(logINFO,*_log)
        << "K-space (off-axis): Applying K resonances" << flush;

    crit_grade = 1. * _kxyz_s1s2_norm;
    double converged03 = false;
    kvit = _kvecs_0_0.begin();
    while (!converged03 && kvit < _kvecs_0_0.end()) {

        double shell_rms = 0.0;
        int rms_count = 0;

        while (kvit < _kvecs_0_0.end()) {
            EWD::KVector kvec = *kvit;
            if (kvec.getGrade() < crit_grade) break;
            EWD::cmplx f_as1s2 = _ewdactor.PhiPU12_AS1S2_At_By(kvec.getK(), target, _bg_P, rV);
            sum_re += f_as1s2._re;
            sum_im += f_as1s2._im;
            shell_rms += f_as1s2._re;
            ++kvit;
            ++rms_count;
        }
        shell_rms = (rms_count > 0) ? sqrt(shell_rms/rms_count)*EWD::int2eV : 0.0;
        double e_measure = shell_rms*1e-10*rms_count;

        if (rms_count > 0) {
            CTP_LOG(logDEBUG,*_log)
                << (format("  o M = %1$04d   G = %2$+1.3e   dF(rms) = %3$+1.3e V   [1e => %4$+1.3e eV]")
                % rms_count
                % crit_grade
                % shell_rms
                % e_measure).str() << flush;
        }

        if (rms_count > 10 && e_measure <= _crit_dE) {
            CTP_LOG(logINFO,*_log)
                << (format("  :: RE %1$+1.7e IM %2$+1.7e")
                % (sqrt(sum_re)*EWD::int2eV)
                % (sum_im*EWD::int2eV)).str() << flush;
            converged03 = true;
        }

        crit_grade /= 10.0;
    }

    _potential_converged_K = converged12 && converged03;

    if (_potential_converged_K)
        CTP_LOG(logINFO,*_log)
            << (format(":::: Converged to precision, {0-2}, {1-2}, {0-3}."))
            << flush;
    else ;

    return;
}

void PEwald3D3D::Potential_CalculateForegroundCorrection(vector<PolarSeg*> &target) {
    CTP_LOG(logDEBUG,*_log)
        << "  o Foreground-correction to potentials via FGN" << flush;

    vector<PolarSeg*>::iterator sit1;
    vector<APolarSite*> ::iterator pit1;
    vector<PolarSeg*>::iterator sit2;
    vector<APolarSite*> ::iterator pit2;

    double rms = 0.0;
    int rms_count = 0;
    for (sit1 = target.begin(); sit1 < target.end(); ++sit1) {
        for (sit2 = _fg_N.begin(); sit2 < _fg_N.end(); ++sit2) {
            for (pit1 = (*sit1)->begin(); pit1 < (*sit1)->end(); ++pit1) {
                for (pit2 = (*sit2)->begin(); pit2 < (*sit2)->end(); ++pit2) {
                    rms += _ewdactor.PhiPU12_ERF_At_By(*(*pit1), *(*pit2));
                    rms_count += 1;
                }
            }
        }
    }
    rms = sqrt(rms/rms_count)*EWD::int2eV;

    return;
}

void PEwald3D3D::Potential_CalculateShapeCorrection(vector<PolarSeg*> &target) {
    CTP_LOG(logDEBUG,*_log) << flush
        << "Potential correction terms" << flush;
    CTP_LOG(logDEBUG,*_log)
        << "  o Shape-correction to potentials, using '" << _shape << "'" << flush;

    _ewdactor.PhiPU12_ShapeField_At_By(target, _bg_P, _shape, _LxLyLz);
    return;
}

}}


#include <Rcpp.h>

//[[Rcpp::depends(RcppEigen)]]
#include <RcppEigen.h>

#include "experimentalSetup.hpp"
#include "individual.hpp"
#include "encodingScheme.hpp"
#include "logLikelihoods.hpp"

#include "AuxiliaryFunctions.hpp"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

//[[Rcpp::export(.oneStepApproximationCpp)]]
Rcpp::List oneStepApproximationCpp(const Eigen::VectorXd & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                                   const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                                   const Eigen::VectorXd & coverage, const Eigen::VectorXd markerImbalances,
                                   const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                                   const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                                   const Eigen::VectorXd & alleleFrequencies, const double & theta,
                                   const std::size_t & numberOfContributors, const std::size_t & numberOfMarkers,
                                   const Eigen::VectorXd & numberOfAlleles, const std::size_t & levelsOfStutterRecursion,
                                   const bool & dualEstimation)
{
    const std::size_t numberOfKnownContributors = knownProfiles.cols();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(numberOfMarkers, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerImbalances, 0.8,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    const std::size_t & N = numberOfAlleles[0] * (numberOfAlleles[0] + 1) / 2.0;
    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;

    std::vector<Eigen::MatrixXd> genotypeMatrices(N);
    Eigen::VectorXd estimatedProbability = Eigen::VectorXd::Zero(N);
    double estimatedProbabilitySum = 0.0;
    double maxValue = -HUGE_VAL;

    std::size_t n = 0;
    for (std::size_t u = 0; u < numberOfUnknownContributors; u++)
    {
        const std::size_t & a = 2 * u;
        Eigen::VectorXd encodedProfiles_um = encodedProfiles;
        for (std::size_t i = 0; i < numberOfAlleles[0]; i++)
        {
            Eigen::VectorXd encodedProfiles_um = encodedProfiles;
            double tt = encodedProfiles_um[a] + 1;
            encodedProfiles_um[a] = i;

            for (std::size_t j = i; j < numberOfAlleles[0]; j++)
            {

                encodedProfiles_um[a + 1] = j;
                Individual I_umij(encodedProfiles_um, sampleParameters, noiseParameters, mixtureParameters, markerImbalances, ES);

                genotypeMatrices[n] = decoding(I_umij.EncodedProfile, ES.NumberOfAlleles, 1, numberOfUnknownContributors);
                estimatedProbability[n] = I_umij.Fitness;
                estimatedProbabilitySum += std::exp(I_umij.Fitness);

                if (I_umij.Fitness > maxValue)
                    maxValue = I_umij.Fitness;

                n++;
            }
        }
    }

    Eigen::VectorXd normalisedProbability = (estimatedProbability - maxValue * Eigen::VectorXd::Ones(N)).array().exp();
    normalisedProbability = normalisedProbability / normalisedProbability.sum();

    return Rcpp::List::create(Rcpp::Named("GenotypeMatrix") = genotypeMatrices,
                              Rcpp::Named("LogUnnormalisedProbabilitySum") = std::log(estimatedProbabilitySum),
                              Rcpp::Named("LogUnnormalisedProbability") = estimatedProbability,
                              Rcpp::Named("NormalisedProbabilities") = normalisedProbability);
}

//[[Rcpp::export(.EAApproximationCpp)]]
Rcpp::List EAApproximationCpp(const std::vector<Eigen::VectorXd> & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                              const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                              const Eigen::VectorXd & coverage, const Eigen::VectorXd markerImbalances,
                              const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                              const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                              const Eigen::VectorXd & alleleFrequencies, const double & theta,
                              const std::size_t & numberOfContributors, const std::size_t & numberOfMarkers,
                              const Eigen::VectorXd & numberOfAlleles, const std::size_t & levelsOfStutterRecursion,
                              const bool & type1, const bool & dualEstimation)
{
    const std::size_t numberOfKnownContributors = knownProfiles.cols();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(numberOfMarkers, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerImbalances, 0.8,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    const std::size_t & numberOfUnknownContributors = ES.NumberOfContributors - ES.NumberOfKnownContributors;
    const std::size_t & N = encodedProfiles.size();

    std::vector<Eigen::MatrixXd> genotypeMatrices(N);
    Eigen::VectorXd estimatedProbability = Eigen::VectorXd::Zero(N);
    double estimatedProbabilitySum = 0.0;
    double maxValue = -HUGE_VAL;
    for (std::size_t n = 0; n < N; n++)
    {
        const Eigen::VectorXd & encodedProfiles_n = encodedProfiles[n];
        Individual I(encodedProfiles_n, sampleParameters, noiseParameters, mixtureParameters, markerImbalances, ES);

        genotypeMatrices[n] = decoding(I.EncodedProfile, ES.NumberOfAlleles, 1, numberOfUnknownContributors);

        if (type1) {
            estimatedProbability[n] = I.Fitness;
            estimatedProbabilitySum += std::exp(I.Fitness - estimatedProbability[0]);
        }
        else {
            estimatedProbability[n] = I.Fitness;
            estimatedProbabilitySum += std::exp(I.Fitness);
        }

        if (I.Fitness > maxValue)
            maxValue = I.Fitness;
    }

    Eigen::VectorXd normalisedProbability = (estimatedProbability - maxValue * Eigen::VectorXd::Ones(N)).array().exp();
    normalisedProbability = normalisedProbability / normalisedProbability.sum();

    double logEstimatedProbabilitySum = std::log(estimatedProbabilitySum);
    if (type1) {
        logEstimatedProbabilitySum += estimatedProbability[0];
    }

    return Rcpp::List::create(Rcpp::Named("GenotypeMatrix") = genotypeMatrices,
                              Rcpp::Named("LogUnnormalisedProbabilitySum") = logEstimatedProbabilitySum,
                              Rcpp::Named("LogUnnormalisedProbability") = estimatedProbability,
                              Rcpp::Named("NormalisedProbabilities") = normalisedProbability);
}

////
struct sampledPriorGenotype {
    Eigen::VectorXd encodedGenotype;
    double priorProbability;

    sampledPriorGenotype(Eigen::VectorXd encodedGenotype_, double priorProbability_) :
        encodedGenotype(encodedGenotype_), priorProbability(priorProbability_) {};
};

Eigen::MatrixXd expectedContributionMarker(const Eigen::MatrixXd & genotype, const std::vector<Eigen::MatrixXd> &potentialParents_m,
                                           const double & levelsOfStutterRecursion)
{
    const std::size_t & N = genotype.rows();
    const std::size_t & M = genotype.cols();

    Eigen::MatrixXd expectedContributionProfile_m = Eigen::MatrixXd::Zero(N, M);
    for (std::size_t u = 0; u < M; u++)
    {
        Eigen::VectorXd genotype_u = genotype.col(u);
        Eigen::VectorXd stutterContribution = Eigen::VectorXd::Zero(N);
        for (std::size_t n = 0; n < N; n++)
        {
            stutterContribution[n] = ParentStutterContribution(n, levelsOfStutterRecursion, 1, genotype_u, potentialParents_m, N);
        }

        expectedContributionProfile_m.col(u) = genotype_u + stutterContribution;
    }

    return expectedContributionProfile_m;
}

void adjustCoverage(Eigen::VectorXd & adjustedCoverage, const double & sampleParameter, const Eigen::VectorXd & mixtureParamters,
                    const double & markerParameter, const Eigen::MatrixXd & genotype, const std::vector< Eigen::MatrixXd > & potentialParents_m,
                    const double & levelsOfStutterRecursion)
{
    const std::size_t & N = adjustedCoverage.size();
    const Eigen::VectorXd & E_c = expectedContributionMarker(genotype, potentialParents_m, levelsOfStutterRecursion);
    const Eigen::VectorXd & mu_ma = sampleParameter * markerParameter * (E_c * mixtureParamters);

    adjustedCoverage = adjustedCoverage - mu_ma;
    for (std::size_t n = 0; n < N; n++)
    {
        if (adjustedCoverage[n] <= 0.0)
        {
            adjustedCoverage[n] = std::exp(adjustedCoverage[n]);
        }

    }
}

double calculatePriorProbabiliy(const Eigen::VectorXd & E, const Eigen::MatrixXd &knownProfiles, const Eigen::VectorXd &coverage,
                                std::vector<Eigen::MatrixXd> potentialParents_m,
                                const double &sampleParameter, const Eigen::VectorXd &mixtureParameters, const double &markerParameter,
                                const std::size_t & numberOfContributors, const double & levelsOfStutterRecursion,
                                const bool &suggestionBool)
{
    if (!suggestionBool)
    {
        return 0.0;
    }

    const std::size_t numberOfKnownContributors = knownProfiles.cols();
    const std::size_t N = E.size();
    const std::size_t M = coverage.size();

    Eigen::MatrixXd genotype = Eigen::MatrixXd::Zero(M, numberOfContributors);
    for (std::size_t k = 0; k < numberOfKnownContributors; k++)
    {
        genotype.col(k) = knownProfiles.col(k);
    }

    Eigen::VectorXd adjustedCoverage = coverage;
    if (suggestionBool)
    {
        adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters, markerParameter, genotype, potentialParents_m, levelsOfStutterRecursion);
    }
    else
    {
        adjustedCoverage = Eigen::VectorXd::Ones(M);
    }

    double priorProbability = 0.0;
    for (std::size_t n = 0; n < N; n++)
    {
        std::size_t u = std::floor(n / 2);
        Eigen::VectorXd proportionalProbability = adjustedCoverage / adjustedCoverage.sum();

        genotype(E[n], numberOfKnownContributors + u) += 1.0;

        if (suggestionBool)
        {
            Eigen::VectorXd singleAllele = Eigen::VectorXd::Zero(M);
            singleAllele[E[n]] = 1.0;

            priorProbability += std::log(proportionalProbability[E[n]]);
            adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters.row(u), markerParameter, singleAllele, potentialParents_m,
                           levelsOfStutterRecursion);
        }
    }

    return priorProbability;
}

sampledPriorGenotype samplePriorGenotypeMarker(const Eigen::MatrixXd &knownProfiles, const Eigen::VectorXd &coverage,
                                               const std::vector<Eigen::MatrixXd> &potentialParents_m,
                                               const double &sampleParameter, const Eigen::VectorXd &mixtureParameters,
                                               const double &markerParameter, const std::size_t numberOfContributors,
                                               const double &levelsOfStutterRecursion, const bool & suggestionBool,
                                               const std::size_t &seed)
{
    boost::random::mt19937 rng(seed);
    boost::random::uniform_real_distribution<> uniform(0, 1);

    const std::size_t &N = coverage.size();
    const std::size_t &numberOfKnownContributors = knownProfiles.cols();
    const std::size_t &numberOfUnknownContributors = numberOfContributors - numberOfKnownContributors;

    Eigen::MatrixXd sampledGenotype = Eigen::MatrixXd::Zero(N, numberOfContributors);
    for (std::size_t k = 0; k < numberOfKnownContributors; k++)
        sampledGenotype.col(k) = knownProfiles.col(k);

    Eigen::VectorXd adjustedCoverage = Eigen::VectorXd::Ones(N);
    if (suggestionBool)
    {
        adjustedCoverage = coverage;
        adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters, markerParameter, sampledGenotype, potentialParents_m,
                       levelsOfStutterRecursion);
    }

    Eigen::VectorXd sampledGenotypeEncoded = Eigen::VectorXd::Zero(2 * numberOfUnknownContributors);
    double sampledPriorProbability = 0.0;
    for (std::size_t u = 0; u < numberOfUnknownContributors; u++)
    {
        for (std::size_t i = 0; i < 2; i++)
        {
            Eigen::VectorXd proportionalProbability = adjustedCoverage / adjustedCoverage.sum();
            Eigen::VectorXd partialSumProportionalProbability = partialSumEigen(proportionalProbability);
            double randomVariate = uniform(rng);
            std::size_t j = 0.0;
            while (randomVariate > partialSumProportionalProbability[j + 1])
                j++;

            sampledGenotype(j, numberOfKnownContributors + u) += 1.0;

            if (suggestionBool)
            {
                Eigen::VectorXd singleAllele = Eigen::VectorXd::Zero(N);
                singleAllele[j] = 1.0;

                sampledPriorProbability += std::log(proportionalProbability[j]);
                adjustCoverage(adjustedCoverage, sampleParameter, mixtureParameters.row(u), markerParameter, singleAllele, potentialParents_m,
                               levelsOfStutterRecursion);
            }

            if (i == 0)
            {
                sampledGenotypeEncoded[2 * u] = j;
            }
            else
            {
                if (j < sampledGenotypeEncoded[2 * u])
                {
                    sampledGenotypeEncoded[2 * u + i] = sampledGenotypeEncoded[2 * u];
                    sampledGenotypeEncoded[2 * u] = j;
                }
                else
                {
                    sampledGenotypeEncoded[2 * u + i] = j;
                }
            }
        }
    }

    sampledPriorGenotype spg(sampledGenotypeEncoded, sampledPriorProbability);
    return spg;
}

//[[Rcpp::export(.samplePosteriorGenotypesGuidedCpp)]]
Rcpp::List samplePosteriorGenotypesGuidedCpp(const Eigen::VectorXd & encodedProfiles, const Eigen::VectorXd & sampleParameters,
                                             const Eigen::VectorXd & noiseParameters, const Eigen::VectorXd & mixtureParameters,
                                             const Eigen::VectorXd markerParameters, const Eigen::VectorXd & coverage,
                                             const std::vector< std::vector< Eigen::MatrixXd > > potentialParents,
                                             const Eigen::MatrixXd & knownProfiles, const Eigen::MatrixXd & allKnownProfiles,
                                             const Eigen::VectorXd & alleleFrequencies, const double & theta,
                                             const std::size_t & numberOfContributors,  const Eigen::VectorXd & numberOfAlleles,
                                             const std::size_t & levelsOfStutterRecursion,
                                             const std::size_t &numberOfSimulations, const bool & suggestionBool,
                                             const std::size_t &seed, const bool & dualEstimation)
{
    boost::random::mt19937 rng(seed);
    boost::random::uniform_int_distribution<> seedShift(0, seed);
    boost::random::uniform_real_distribution<> uniform(0, 1);

    const std::size_t &numberOfKnownContributors = knownProfiles.cols();
    const std::size_t &numberOfMarkers = numberOfAlleles.size();

    const Eigen::VectorXd tolerance = Eigen::VectorXd::Zero(4);
    const ExperimentalSetup ES(1, numberOfAlleles, numberOfContributors, numberOfKnownContributors,
                               knownProfiles, allKnownProfiles, coverage, potentialParents, markerParameters, 0.0,
                               noiseParameters, tolerance, theta, alleleFrequencies, levelsOfStutterRecursion, dualEstimation);

    Eigen::VectorXd currentEncodedGenotype = encodedProfiles;
    double currentPriorProbability = calculatePriorProbabiliy(currentEncodedGenotype, knownProfiles, coverage,
                                                              potentialParents[0], sampleParameters[0], mixtureParameters,
                                                              markerParameters[0], numberOfContributors, levelsOfStutterRecursion,
                                                              suggestionBool);

    Individual currentI(currentEncodedGenotype, sampleParameters, noiseParameters, mixtureParameters, markerParameters, ES);
    double currentLogLikelihood = currentI.Fitness;

    std::vector<Eigen::MatrixXd> sampledGenotypeList(numberOfSimulations);
    Eigen::VectorXd unnormalisedLogLikelihood = Eigen::VectorXd::Zero(numberOfSimulations);
    double acceptedProposals = 0.0;
    for (std::size_t n = 0; n < numberOfSimulations; n++)
    {
        int seedShift_n = seedShift(rng);
        const sampledPriorGenotype & newGenotype = samplePriorGenotypeMarker(knownProfiles, coverage, potentialParents[0],
                                                                             sampleParameters[0], mixtureParameters,
                                                                             markerParameters[0], numberOfContributors, levelsOfStutterRecursion,
                                                                             suggestionBool, seed + n + seedShift_n);

        const Eigen::VectorXd & newEncodedGenotype = newGenotype.encodedGenotype;
        const double & newPriorProbability = newGenotype.priorProbability;

        const Individual newI(newEncodedGenotype, sampleParameters, noiseParameters, mixtureParameters, markerParameters, ES);
        const double & newLogLikelihood = newI.Fitness;

        const double & logHastingsRatio = newLogLikelihood - currentLogLikelihood + currentPriorProbability - newPriorProbability;
        double acceptProbability = 1.0;
        if (logHastingsRatio < 0)
        {
            acceptProbability = std::exp(logHastingsRatio);
        }

        double randomVariate = uniform(rng);
        if (randomVariate < acceptProbability)
        {
            currentEncodedGenotype = newEncodedGenotype;
            currentPriorProbability = newPriorProbability;
            currentLogLikelihood = newLogLikelihood;
            acceptedProposals += 1;
        }

        Eigen::MatrixXd currentDecodedGenotype = decoding(currentEncodedGenotype, numberOfAlleles, 1,
                                                          numberOfContributors - numberOfKnownContributors);
        sampledGenotypeList[n] = currentDecodedGenotype;
        unnormalisedLogLikelihood[n] = currentLogLikelihood;
    }

    return Rcpp::List::create(Rcpp::Named("SampledGenotypes") = sampledGenotypeList,
                              Rcpp::Named("UnnormalisedLogLikelihood") = unnormalisedLogLikelihood,
                              Rcpp::Named("AcceptedProposals") = acceptedProposals);
}


#pragma once

#include <boost/multiprecision/cpp_int.hpp>

#include <crypto/cryptopp/osrng.h>

namespace nano
{
using uint128_t = boost::multiprecision::uint128_t;
using uint256_t = boost::multiprecision::uint256_t;
using uint512_t = boost::multiprecision::uint512_t;
// SI dividers
nano::uint128_t const Gxrb_ratio = nano::uint128_t ("1000000000000000000000000000000000"); // 10^33
nano::uint128_t const Mxrb_ratio = nano::uint128_t ("1000000000000000000000000000000"); // 10^30
nano::uint128_t const kxrb_ratio = nano::uint128_t ("1000000000000000000000000000"); // 10^27
nano::uint128_t const xrb_ratio = nano::uint128_t ("1000000000000000000000000"); // 10^24
nano::uint128_t const raw_ratio = nano::uint128_t ("1"); // 10^0

union uint128_union final
{
public:
	uint128_union () = default;
	/**
	 * Decode from hex string
	 * @warning Aborts at runtime if the input is invalid
	 */
	uint128_union (std::string const &);
	uint128_union (uint64_t);
	uint128_union (nano::uint128_t const &);
	bool operator== (nano::uint128_union const &) const;
	bool operator!= (nano::uint128_union const &) const;
	bool operator< (nano::uint128_union const &) const;
	bool operator> (nano::uint128_union const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &, bool = false);
	bool decode_dec (std::string const &, nano::uint128_t);
	std::string format_balance (nano::uint128_t scale, int precision, bool group_digits);
	std::string format_balance (nano::uint128_t scale, int precision, bool group_digits, const std::locale & locale);
	nano::uint128_t number () const;
	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	std::string to_string_dec () const;
	std::array<uint8_t, 16> bytes;
	std::array<char, 16> chars;
	std::array<uint32_t, 4> dwords;
	std::array<uint64_t, 2> qwords;
};
// Balances are 128 bit.
using amount = uint128_union;
class raw_key;
union uint256_union final
{
	uint256_union () = default;
	/**
	 * Decode from hex string
	 * @warning Aborts at runtime if the input is invalid
	 */
	uint256_union (std::string const &);
	uint256_union (uint64_t);
	uint256_union (nano::uint256_t const &);
	void encrypt (nano::raw_key const &, nano::raw_key const &, uint128_union const &);
	uint256_union & operator^= (nano::uint256_union const &);
	uint256_union operator^ (nano::uint256_union const &) const;
	bool operator== (nano::uint256_union const &) const;
	bool operator!= (nano::uint256_union const &) const;
	bool operator< (nano::uint256_union const &) const;
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	void encode_dec (std::string &) const;
	bool decode_dec (std::string const &);
	void encode_account (std::string &) const;
	std::string to_account () const;
	bool decode_account (std::string const &);
	std::array<uint8_t, 32> bytes;
	std::array<char, 32> chars;
	std::array<uint32_t, 8> dwords;
	std::array<uint64_t, 4> qwords;
	std::array<uint128_union, 2> owords;
	void clear ();
	bool is_zero () const;
	std::string to_string () const;
	nano::uint256_t number () const;
};
// All keys and hashes are 256 bit.
using block_hash = uint256_union;
using account = uint256_union;
using public_key = uint256_union;
using private_key = uint256_union;
using secret_key = uint256_union;
class raw_key final
{
public:
	~raw_key ();
	void decrypt (nano::uint256_union const &, nano::raw_key const &, uint128_union const &);
	bool operator== (nano::raw_key const &) const;
	bool operator!= (nano::raw_key const &) const;
	nano::uint256_union data;
};
union uint512_union final
{
	uint512_union () = default;
	uint512_union (nano::uint256_union const &, nano::uint256_union const &);
	uint512_union (nano::uint512_t const &);
	bool operator== (nano::uint512_union const &) const;
	bool operator!= (nano::uint512_union const &) const;
	nano::uint512_union & operator^= (nano::uint512_union const &);
	void encode_hex (std::string &) const;
	bool decode_hex (std::string const &);
	std::array<uint8_t, 64> bytes;
	std::array<uint32_t, 16> dwords;
	std::array<uint64_t, 8> qwords;
	std::array<uint256_union, 2> uint256s;
	void clear ();
	bool is_zero () const;
	nano::uint512_t number () const;
	std::string to_string () const;
};
using signature = uint512_union;
using qualified_root = uint512_union;

nano::uint512_union sign_message (nano::raw_key const &, nano::public_key const &, nano::uint256_union const &);
bool validate_message (nano::public_key const &, nano::uint256_union const &, nano::uint512_union const &);
bool validate_message_batch (const unsigned char **, size_t *, const unsigned char **, const unsigned char **, size_t, int *);
void deterministic_key (nano::uint256_union const &, uint32_t, nano::uint256_union &);
nano::public_key pub_key (nano::private_key const &);

/* Conversion methods */
std::string to_string_hex (uint64_t const);
bool from_string_hex (std::string const &, uint64_t &);

/**
 * Convert a double to string in fixed format
 * @param precision_a (optional) use a specific precision (default is the maximum)
 */
std::string to_string (double const, int const precision_a = std::numeric_limits<double>::digits10);

namespace difficulty
{
	uint64_t from_multiplier (double const, uint64_t const);
	double to_multiplier (uint64_t const, uint64_t const);
}
}

namespace std
{
template <>
struct hash<::nano::uint256_union>
{
	size_t operator() (::nano::uint256_union const & data_a) const
	{
		return *reinterpret_cast<size_t const *> (data_a.bytes.data ());
	}
};
template <>
struct hash<::nano::uint256_t>
{
	size_t operator() (::nano::uint256_t const & number_a) const
	{
		return number_a.convert_to<size_t> ();
	}
};
template <>
struct hash<::nano::uint512_union>
{
	size_t operator() (::nano::uint512_union const & data_a) const
	{
		return *reinterpret_cast<size_t const *> (data_a.bytes.data ());
	}
};
}


// https://www.boost.org/doc/libs/1_67_0/doc/html/chrono/users_guide.html#chrono.users_guide.examples

#include <boost/chrono.hpp>
#include <iostream>

namespace I_dont_like_the_default_duration_behavior
{

template <class R>
class zero_default
{
  public:
    typedef R rep;

  private:
    rep rep_;

  public:
    zero_default(rep i = 0) : rep_(i) {}
    operator rep() const { return rep_; }

    zero_default &operator+=(zero_default x)
    {
        rep_ += x.rep_;
        return *this;
    }
    zero_default &operator-=(zero_default x)
    {
        rep_ -= x.rep_;
        return *this;
    }
    zero_default &operator*=(zero_default x)
    {
        rep_ *= x.rep_;
        return *this;
    }
    zero_default &operator/=(zero_default x)
    {
        rep_ /= x.rep_;
        return *this;
    }

    zero_default operator+() const { return *this; }
    zero_default operator-() const { return zero_default(-rep_); }
    zero_default &operator++()
    {
        ++rep_;
        return *this;
    }
    zero_default operator++(int) { return zero_default(rep_++); }
    zero_default &operator--()
    {
        --rep_;
        return *this;
    }
    zero_default operator--(int) { return zero_default(rep_--); }

    friend zero_default operator+(zero_default x, zero_default y) { return x += y; }
    friend zero_default operator-(zero_default x, zero_default y) { return x -= y; }
    friend zero_default operator*(zero_default x, zero_default y) { return x *= y; }
    friend zero_default operator/(zero_default x, zero_default y) { return x /= y; }

    friend bool operator==(zero_default x, zero_default y) { return x.rep_ == y.rep_; }
    friend bool operator!=(zero_default x, zero_default y) { return !(x == y); }
    friend bool operator<(zero_default x, zero_default y) { return x.rep_ < y.rep_; }
    friend bool operator<=(zero_default x, zero_default y) { return !(y < x); }
    friend bool operator>(zero_default x, zero_default y) { return y < x; }
    friend bool operator>=(zero_default x, zero_default y) { return !(x < y); }
};

typedef boost::chrono::duration<zero_default<long long>, boost::nano> nanoseconds;
typedef boost::chrono::duration<zero_default<long long>, boost::micro> microseconds;
typedef boost::chrono::duration<zero_default<long long>, boost::milli> milliseconds;
typedef boost::chrono::duration<zero_default<long long>> seconds;
typedef boost::chrono::duration<zero_default<long long>, boost::ratio<60>> minutes;
typedef boost::chrono::duration<zero_default<long long>, boost::ratio<3600>> hours;
} // namespace I_dont_like_the_default_duration_behavior

int main()
{
    using namespace I_dont_like_the_default_duration_behavior;

    milliseconds ms;
    std::cout << ms.count() << '\n';
}


// c libraries
#include <cstdio>
#include <ctime>
// c++ libraries
#include <iostream>
#include <string>
#include <stdexcept>
// eigen libraries
#include <Eigen/Dense>
// ann - structure
#include "src/struc/structure.hpp"
// ann - strings
#include "src/str/string.hpp"
// ann - chem
#include "src/chem/units.hpp"
#include "src/chem/ptable.hpp"
// ann - vasp
#include "src/format/vasp_sim.hpp"

//*****************************************************
//XDATCAR
//*****************************************************

namespace VASP{

namespace XDATCAR{

void read(const char* file, const Interval& interval, const AtomType& atomT, Simulation& sim){
	static const char* funcName="read(const char*,const Interval&,Simulation&)";
	if(VASP_PRINT_FUNC>0) std::cout<<NAMESPACE_GLOBAL<<"::"<<NAMESPACE_LOCAL<<"::"<<funcName<<":\n";
	//==== local function variables ====
	//file i/o
		FILE* reader=NULL;
		char* input=new char[string::M];
		char* str_name=new char[string::M];
		char* str_number=new char[string::M];
		std::string str;
	//simulation flags
		bool direct;//whether the coordinates are in direct or Cartesian coordinates
	//time info
		int ts=0;//number of timesteps
		int tsint=0;//requested interval
	//cell info
		double scale=1;
		Eigen::Matrix3d lv;
	//units
		double s_len=0.0;
		if(units::consts::system()==units::System::AU) s_len=1.0/units::BOHR;
		else if(units::consts::system()==units::System::METAL) s_len=1.0;
		else throw std::runtime_error("Invalid units.");
	//misc
		bool error=false;

	try{
		//start the timer
		const clock_t start=std::clock();

		//open the file
		if(VASP_PRINT_STATUS>0) std::cout<<"Opening file\n";
		reader=fopen(file,"r");
		if(reader==NULL) throw std::runtime_error("I/O Error: Unable to open file.");

		//==== clear the simulation ====
		if(VASP_PRINT_STATUS>0) std::cout<<"clearing simulation\n";
		sim.clear();

		//==== read in the system name ====
		if(VASP_PRINT_STATUS>0) std::cout<<"reading system name\n";
		sim.name()=string::trim(fgets(input,string::M,reader));

		//==== read the simulation cell ====
		if(VASP_PRINT_STATUS>0) std::cout<<"reading cell\n";
		std::sscanf(fgets(input,string::M,reader),"%lf",&scale);
		std::sscanf(fgets(input,string::M,reader),"%lf %lf %lf",&lv(0,0),&lv(1,0),&lv(2,0));
		std::sscanf(fgets(input,string::M,reader),"%lf %lf %lf",&lv(0,1),&lv(1,1),&lv(2,1));
		std::sscanf(fgets(input,string::M,reader),"%lf %lf %lf",&lv(0,2),&lv(1,2),&lv(2,2));
		lv*=s_len*scale;

		//==== read species ====
		if(VASP_PRINT_STATUS>0) std::cout<<"read species\n";
		//read number of species
		fgets(str_name,string::M,reader);
		fgets(str_number,string::M,reader);
		const int nNames=string::substrN(str_name,string::WS);
		const int nNumbers=string::substrN(str_number,string::WS);
		const int nSpecies=string::substrN(str_name,string::WS);
		if(nNames!=nNumbers) throw std::runtime_error("Invalid number of species");
		if(nSpecies<=0) throw std::runtime_error("Invalid number of species");
		//read in the species names
		std::vector<std::string> species(nSpecies);
		species[0]=std::strtok(str_name,string::WS);
		for(int i=1; i<nSpecies; ++i) species[i]=std::strtok(NULL,string::WS);
		//read in the species numbers
		std::vector<int> nAtoms(nSpecies);
		nAtoms[0]=std::atoi(std::strtok(str_number,string::WS));
		for(int i=1; i<nSpecies; ++i) nAtoms[i]=std::atoi(std::strtok(NULL,string::WS));
		int nAtomsT=0;
		for(int i=0; i<nSpecies; ++i) nAtomsT+=nAtoms[i];

		//==== read coord ====
		if(VASP_PRINT_STATUS>0) std::cout<<"read coord\n";
		fgets(input, string::M, reader);
		if(input[0]=='D') direct=true;
		else direct=false;

		//==== check if the cell is variable or not ====
		if(VASP_PRINT_STATUS>0) std::cout<<"Checking whether cell is variable\n";
		for(int n=0; n<nAtomsT; ++n) fgets(input, string::M, reader);
		str=std::string(string::trim(fgets(input,string::M,reader)));
		if(str==sim.name()) sim.cell_fixed()=false;
		else sim.cell_fixed()=true;
		// reset the line position
		if(!sim.cell_fixed()) for(int i=0; i<HEADER_SIZE; ++i) fgets(input,string::M,reader);

		//==== find the number of timesteps ====
		if(VASP_PRINT_STATUS>0) std::cout<<"reading the number of timesteps\n";
		std::rewind(reader);
		//find the total number of lines in the file
		int nLines=0;
		while(fgets(input, string::M, reader)!=NULL){++nLines;};
		if(sim.cell_fixed()) ts=std::floor((1.0*nLines-HEADER_SIZE)/(1.0*nAtomsT+1.0));
		else ts=std::floor((1.0*nLines)/(1.0*nAtomsT+1.0+HEADER_SIZE));

		//==== reset the line position ====
		if(VASP_PRINT_STATUS>0) std::cout<<"resetting the line position\n";
		std::rewind(reader);
		if(sim.cell_fixed()) for(int i=0; i<HEADER_SIZE; ++i) fgets(input,string::M,reader);

		//==== set the interval ====
		if(VASP_PRINT_STATUS>0) std::cout<<"setting the interval\n";
		if(interval.beg<0) throw std::invalid_argument("Invalid beginning timestep.");
		const int beg=interval.beg-1;
		int end=interval.end-1;
		if(interval.end<0) end=ts+interval.end;
		tsint=end-beg+1;

		//==== print data to screen ====
		if(VASP_PRINT_DATA>0){
			std::cout<<"NAME    = "<<sim.name()<<"\n";
			std::cout<<"ATOMT   = "<<atomT<<"\n";
			std::cout<<"DIRECT  = "<<(direct?"T":"F")<<"\n";
			std::cout<<"CELL    = \n"<<lv<<"\n";
			std::cout<<"SPECIES = "; for(int i=0; i<nSpecies; ++i) std::cout<<species[i]<<" "; std::cout<<"\n";
			std::cout<<"NUMBERS = "; for(int i=0; i<nSpecies; ++i) std::cout<<nAtoms[i]<<" "; std::cout<<"\n";
			std::cout<<"NATOMST = "<<nAtomsT<<"\n";
			std::cout<<"INTERVAL   = "<<interval<<"\n";
			std::cout<<"TIMESTEPS  = "<<ts<<"\n";
			std::cout<<"N_STEPS    = "<<tsint/interval.stride<<"\n";
		}

		//==== resize the simulation ====
		if(VASP_PRINT_STATUS>0) std::cout<<"allocating memory\n";
		sim.resize(tsint/interval.stride,nAtomsT,atomT);

		//==== read positions ====
		if(VASP_PRINT_STATUS>0) std::cout<<"reading positions\n";
		//skip timesteps until beg is reached
		for(int t=0; t<beg; ++t){
			if(!sim.cell_fixed()) for(int i=0; i<HEADER_SIZE; ++i) fgets(input,string::M,reader); //skip header
			fgets(input,string::M,reader);//skip single line
			for(int n=0; n<nAtomsT; ++n) fgets(input,string::M,reader);
		}
		//read the positions
		if(sim.cell_fixed()){
			for(int t=0; t<sim.timesteps(); ++t) static_cast<Cell&>(sim.frame(t)).init(lv);
			for(int t=0; t<sim.timesteps(); ++t){
				if(VASP_PRINT_STATUS>1) std::cout<<"T = "<<t<<"\n";
				else if(t%1000==0) std::cout<<"T = "<<t<<"\n";
				fgets(input,string::M,reader);//skip line
				for(int n=0; n<nAtomsT; ++n){
					std::sscanf(
						fgets(input,string::M,reader),"%lf %lf %lf",
						&sim.frame(t).posn(n)[0],&sim.frame(t).posn(n)[1],&sim.frame(t).posn(n)[2]
					);
				}
				//skip "stride-1" steps
				for(int tt=0; tt<interval.stride-1; ++tt){
					fgets(input,string::M,reader);//skip line
					for(int n=0; n<nAtomsT; ++n){
						fgets(input,string::M,reader);
					}
				}
			}
		} else {
			for(int t=0; t<sim.timesteps(); ++t){
				if(VASP_PRINT_STATUS>1) std::cout<<"T = "<<t<<"\n";
				else if(t%1000==0) std::cout<<"T = "<<t<<"\n";
				//read in lattice vectors
				fgets(input,string::M,reader);//name
				scale=std::atof(fgets(input,string::M,reader));//scale
				for(int i=0; i<3; ++i){
					fgets(input, string::M, reader);
					lv(0,i)=std::atof(std::strtok(input,string::WS));
					for(int j=1; j<3; ++j){
						lv(j,i)=std::atof(std::strtok(NULL,string::WS));
					}
				}
				static_cast<Cell&>(sim.frame(t)).init(s_len*scale*lv);
				fgets(input,string::M,reader);//skip line (atom names)
				fgets(input,string::M,reader);//skip line (atom numbers)
				fgets(input,string::M,reader);//skip line (Direct or Cart)
				for(int n=0; n<nAtomsT; ++n){
					std::sscanf(
						fgets(input,string::M,reader),"%lf %lf %lf",
						&sim.frame(t).posn(n)[0],&sim.frame(t).posn(n)[1],&sim.frame(t).posn(n)[2]
					);
				}
				//skip "stride-1" steps
				for(int tt=0; tt<interval.stride-1; ++tt){
					fgets(input,string::M,reader);//name
					fgets(input,string::M,reader);//scale
					for(int i=0; i<3; ++i){
						fgets(input,string::M,reader);//lv
					}
					fgets(input,string::M,reader);//skip line (atom names)
					fgets(input,string::M,reader);//skip line (atom numbers)
					fgets(input,string::M,reader);//skip line (Direct or Cart)
					for(int n=0; n<nAtomsT; ++n){
						fgets(input,string::M,reader);
					}
				}
			}
		}

		//==== convert to Cartesian coordinates if necessary ====
		if(direct){
			if(VASP_PRINT_STATUS>-1) std::cout<<"converting to cartesian coordinates\n";
			for(int t=0; t<sim.timesteps(); ++t){
				for(int n=0; n<nAtomsT; ++n){
					sim.frame(t).posn(n)=sim.frame(t).R()*sim.frame(t).posn(n);
				}
			}
		} else if(s_len!=1.0){
			for(int t=0; t<sim.timesteps(); ++t){
				for(int n=0; n<nAtomsT; ++n){
					sim.frame(t).posn(n)*=s_len;
				}
			}
		}

		//==== close the file ====
		fclose(reader);
		reader=NULL;

		//==== stop the timer ====
		const clock_t stop=std::clock();

		//==== print the time ====
		const double time=((double)(stop-start))/CLOCKS_PER_SEC;
		std::cout<<"Simulation loaded in "<<time<<" seconds.\n";
	}catch(std::exception& e){
		std::cout<<"ERROR in "<<NAMESPACE_GLOBAL<<"::"<<NAMESPACE_LOCAL<<"::"<<funcName<<":\n";
		std::cout<<e.what()<<"\n";
		error=true;
	}

	//free all local variables
	if(reader!=NULL) fclose(reader);
	delete[] input;

	if(error) throw std::runtime_error("I/O Exception: Could not read data.");
}

void write(const char* file, const Interval& interval, const AtomType& atomT, const Simulation& sim){
	/*
	const char* funcName="write(const char*,const Interval&,const AtomType&,Simulation&)";
	if(VASP_PRINT_FUNC>0) std::cout<<NAMESPACE_GLOBAL<<"::"<<NAMESPACE_LOCAL<<"::"<<funcName<<":\n";
	FILE* writer=NULL;
	bool error=false;
	//units
		double s_len=0.0;
		if(units::consts::system()==units::System::AU) s_len=units::BOHR;
		else if(units::consts::system()==units::System::METAL) s_len=1.0;
		else throw std::runtime_error("Invalid units.");

	try{
		//==== open the file ====
		writer=fopen(file,"w");
		if(writer==NULL) throw std::runtime_error(std::string("I/O Error: Could not open file: \"")+std::string(file)+std::string("\"\n"));

		//==== check simulation ====
		if(sim.timesteps()==0) throw std::invalid_argument("Invalid simulation.");

		//==== check timing info ====
		int beg=interval.beg-1;
		int end;
		if(interval.end>=0) end=interval.end-1;
		else end=interval.end+sim.timesteps();
		if(beg<0) throw std::invalid_argument("Invalid beginning timestep.");
		if(end>=sim.timesteps()) throw std::invalid_argument("Invalid ending timestep.");
		if(end<beg) throw std::invalid_argument("Invalid timestep interval.");

		//==== write the coord ====
		std::string coord;
		if(atomT.frac) coord=std::string("Direct");
		else coord=std::string("Cart");

		if(sim.cell_fixed()){
			fprintf(writer,"%s\n",sim.name().c_str());
			fprintf(writer,"1.0\n");
			for(int i=0; i<3; ++i){
				for(int j=0; j<3; ++j){
					fprintf(writer,"%f ",sim.frame(0).R()(j,i)*s_len);
				}
				fprintf(writer,"\n");
			}
			for(int i=0; i<nSpecies; ++i) fprintf(writer,"%s ",species[i].c_str());
			fprintf(writer,"\n");
			for(int i=0; i<nSpecies; ++i) fprintf(writer,"%i ",nAtoms[i]);
			fprintf(writer,"\n");
			for(int t=beg; t<=end; ++t){
				fprintf(writer,"%s\n",coord.c_str());
				int count=0;
				for(int n=0; n<nSpecies; ++n){
					for(int m=0; m<nAtoms[n]; ++m){
						Eigen::Vector3d posn=sim.frame(t).RInv()*sim.frame(t).posn(count++);
						fprintf(writer,"%f %f %f\n",posn[0],posn[1],posn[2]);
					}
				}
			}
		} else {
			for(int t=beg; t<=end; ++t){
				fprintf(writer,"%s\n",sim.name().c_str());
				fprintf(writer,"1.0\n");
				for(int i=0; i<3; ++i){
					for(int j=0; j<3; ++j){
						fprintf(writer,"%f ",sim.frame(t).R()(j,i)*s_len);
					}
					fprintf(writer,"\n");
				}
				for(int i=0; i<nSpecies; ++i) fprintf(writer,"%s ",species[i].c_str());
				fprintf(writer,"\n");
				for(int i=0; i<nSpecies; ++i) fprintf(writer,"%i ",nAtoms[i]);
				fprintf(writer,"\n");
				fprintf(writer,"%s\n",coord.c_str());
				int count=0;
				for(int n=0; n<as.nSpecies(); ++n){
					for(int m=0; m<as.nAtoms(n); ++m){
						Eigen::Vector3d posn=sim.frame(t).RInv()*sim.frame(t).posn(count++);
						fprintf(writer,"%f %f %f\n",posn[0],posn[1],posn[2]);
					}
				}
			}
		}

		fclose(writer);
		writer=NULL;
	}catch(std::exception& e){
		std::cout<<"ERROR in "<<NAMESPACE_GLOBAL<<"::"<<NAMESPACE_LOCAL<<"::"<<funcName<<":\n";
		std::cout<<e.what()<<"\n";
		error=true;
	}

	if(error) throw std::runtime_error("I/O Exception: Could not write data.");
	*/
}

}

}