/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <votca/xtp/qmmachine.h>
#include <sys/stat.h>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <votca/tools/elements.h>
#include <votca/xtp/espfit.h>
#include <votca/xtp/aomatrix.h>

using boost::format;

namespace votca {
  namespace xtp {

    QMMachine::QMMachine(ctp::XJob *job, ctp::XInductor *xind, QMPackage *qmpack,
            Property *opt, string sfx)
    : _job(job), _xind(xind), _qmpack(qmpack),
    _isConverged(false) {

      string key = sfx + ".qmmmconvg";
      _crit_dR = opt->ifExistsReturnElseReturnDefault<double>(key + ".dR", 0.01); //nm
      _crit_dQ = opt->ifExistsReturnElseReturnDefault<double>(key + ".dQ", 0.01); //e
      _crit_dE_QM = opt->ifExistsReturnElseReturnDefault<double>(key + ".dQdE_QM", 0.001); //eV
      _crit_dE_MM = opt->ifExistsReturnElseReturnDefault<double>(key + ".dE_MM", _crit_dE_QM); //eV
      _maxIter = opt->ifExistsReturnElseReturnDefault<int>(key + ".max_iter", 32);
      _alpha = opt->ifExistsReturnElseReturnDefault<double>(key + ".mixing", 0.0);

      key = sfx;
      double dpl_spacing = opt->ifExistsReturnElseReturnDefault<double>(key + ".dpl_spacing", 1e-3);

      // check for archiving
      std::string _archiving_string = opt->ifExistsReturnElseReturnDefault<std::string>(key + ".archiving", "");
      if (_archiving_string.find("iterations") != std::string::npos) {
        _do_archive = true;
      } else {
        _do_archive = false;
      }

      // check for static or polarized qmmm
      key = sfx + ".tholemodel";
      _static_qmmm = true;
      qmpack->setWithPolarization(false);
      if (opt->exists(key + ".induce")) {
        bool induce = opt->get(key + ".induce").as<bool>();
        if (induce) {
          qmpack->setWithPolarization(true);
          qmpack->setDipoleSpacing(dpl_spacing);
        }
        _static_qmmm = !induce;
      }

      // GDMA options
      key = sfx + ".gdma";
      if (opt->exists(key)) {
        _do_gdma = true;
        string gdma_xml = opt->get(key).as<string> ();
        load_property_from_xml(_gdma_options, gdma_xml.c_str());
      } else {
        _do_gdma = false;
      }

      // GWBSE options
      key = sfx + ".gwbse";
      if (opt->exists(key)) {
        string _gwbse_xml = opt->get(key + ".gwbse_options").as<string> ();
        load_property_from_xml(_gwbse_options, _gwbse_xml.c_str());
         key = sfx + ".gwbse.filter";
         if(opt->exists(key)){
           tools::Property& filterprop=opt->get(key);
           _filter.Initialize(filterprop);
         }
      }

      return;
    }

    QMMachine::~QMMachine() {
      for (QMMIter* qit : _iters) {
        delete qit;
      }
      _iters.clear();
    }

    int QMMachine::Evaluate(ctp::XJob *job) {

      CTP_LOG(ctp::logINFO, *_log)
              << format("... dR %1$1.4f dQ %2$1.4f QM %3$1.4f MM %4$1.4f IT %5$d")
              % _crit_dR % _crit_dQ % _crit_dE_QM % _crit_dE_MM % _maxIter << flush;

      // FIGURE OUT CHARGE + MULTIPLICITY
      double dQ = 0.0;
      for (unsigned int i = 0; i < _job->getPolarTop()->QM0().size(); ++i) {
        dQ += _job->getPolarTop()->QM0()[i]->CalcTotQ();
      }
      int chrg = std::round(dQ);
      int spin = ((chrg < 0) ? -chrg : chrg) % 2 + 1;
      CTP_LOG(ctp::logINFO, *_log) << "... Q = " << chrg << ", 2S+1 = " << spin << flush;

      // PREPARE JOB DIRECTORY
      boost::filesystem::path arg_path;
      string frame_dir = "frame_" + boost::lexical_cast<string>(_job->getTop()->getDatabaseId());
      string jobFolder = "job_" + boost::lexical_cast<string>(_job->getId())
              + "_" + _job->getTag();
      string work_dir = (arg_path / "QMMM" / frame_dir / jobFolder).c_str();
      bool created = boost::filesystem::create_directories(work_dir);
      if (created) {
        CTP_LOG(ctp::logINFO, *_log) << "Created directory " << work_dir << flush;
      }

      vector<string> split;
      Tokenizer toker(_job->getTag(), ":");
      toker.ToVector(split);
      _initialstate=QMState(split.back());
      if(_initialstate.Type().isExciton() || _initialstate.Type().isGWState()){
       _filter.setInitialState(_initialstate);
       _filter.setLogger(_log);
       _do_gwbse=true;
      }

      _qmpack->setCharge(chrg);
      _qmpack->setSpin(spin);

      int iterCnt = 0;
      int iterMax = _maxIter;
      for (; iterCnt < iterMax; ++iterCnt) {

        // check for polarized QM/MM convergence
        int info = Iterate(work_dir, iterCnt);
        if (info != 0) {
          CTP_LOG(ctp::logERROR, *_log)
                  << format("Iterating job failed!") << flush;
          return 1;
        }
        if (_static_qmmm) {
          _isConverged = true;
          break;
        } else if (hasConverged()) {
          break;
        }

      }

      if (iterCnt == iterMax - 1 && !_isConverged) {
        CTP_LOG(ctp::logWARNING, *_log)
                << format("Not converged within %1$d iterations.") % iterMax;
        return 2;
      }

      return 0;
    }

    void QMMachine::RunGDMA(QMMIter* thisIter, string& runFolder){
      if (_qmpack->getPackageName() != "gaussian" || _qmpack->getExecutable() != "g03") {
        throw std::runtime_error(" Invalid QMPackage: " + _qmpack->getPackageName()+ " Gaussian 03 only!");
      } else {
        GDMA gdma;
        gdma.Initialize(_gdma_options);
        gdma.setLog(_log);
        gdma.SetRunDir(runFolder);
        CTP_LOG(ctp::logINFO, *_log) << "Running GDMA " << flush;
        gdma.WriteInputFile();
        gdma.RunExternal();
        gdma.ParseOutputFile();
        thisIter->UpdateMPSFromGDMA(gdma.GetMultipoles(), _job->getPolarTop()->QM0());
      }
    }

    void QMMachine::RunGWBSE(string& runFolder){
      CTP_LOG(ctp::logDEBUG, *_log) << "Running GWBSE "<< flush;
      GWBSE gwbse = GWBSE(orb_iter_input);
      // define own logger for GW-BSE that is written into a runFolder logfile
      ctp::Logger gwbse_logger(ctp::logDEBUG);
      gwbse_logger.setMultithreading(false);
      gwbse.setLogger(&gwbse_logger);
      gwbse_logger.setPreface(ctp::logINFO, (format("\nGWBSE INF ...")).str());
      gwbse_logger.setPreface(ctp::logERROR, (format("\nGWBSE ERR ...")).str());
      gwbse_logger.setPreface(ctp::logWARNING, (format("\nGWBSE WAR ...")).str());
      gwbse_logger.setPreface(ctp::logDEBUG, (format("\nGWBSE DBG ...")).str());

      // Initialize with options
      gwbse.Initialize(_gwbse_options);
      // actual GW-BSE run
      gwbse.Evaluate();

      // write logger to log file
      ofstream ofs;
      string gwbse_logfile = runFolder + "/gwbse.log";
      ofs.open(gwbse_logfile.c_str(), ofstream::out);
      if (!ofs.is_open()) {
        throw runtime_error("Bad file handle: " + gwbse_logfile);
      }
      ofs << gwbse_logger << endl;
      ofs.close();
    }

    bool QMMachine::RunDFT(string& runFolder, std::vector<std::shared_ptr<ctp::PolarSeg> >& MultipolesBackground){
      ctp::Logger dft_logger(ctp::logDEBUG);
      dft_logger.setMultithreading(false);
      dft_logger.setPreface(ctp::logINFO, (format("\nGWBSE INF ...")).str());
      dft_logger.setPreface(ctp::logERROR, (format("\nGWBSE ERR ...")).str());
      dft_logger.setPreface(ctp::logWARNING, (format("\nGWBSE WAR ...")).str());
      dft_logger.setPreface(ctp::logDEBUG, (format("\nGWBSE DBG ...")).str());
      _qmpack->setLog(&dft_logger);

      _qmpack->setMultipoleBackground(MultipolesBackground);

      // setting RUNDIR for the external QMPackages, dummy for internal
      _qmpack->setRunDir(runFolder);

      CTP_LOG(ctp::logDEBUG, *_log) << "Writing input file " << runFolder << flush;
      CTP_LOG(ctp::logDEBUG, *_log) << "Running DFT " << flush;
      _qmpack->WriteInputFile(orb_iter_input);

      orb_iter_input.WriteXYZ(runFolder + "/system_qm.xyz");

      _qmpack->Run(orb_iter_input);

      /* Parse information from the LOGFILE into orbitals, if
       * external QMPackage is run. Dummy for internal DFTENGINE.
       * The QMPackage's MOcoefficients are not automatically
       * parsed in DFT-only calculations and ESP fits for
       * polarized QMMM are expected in the LOGFILE.
       * IF the internal ESPFITs should be used, the MOcoefficients
       * need to be parsed too.
       */
      bool success1= _qmpack->ParseLogFile(orb_iter_input);

      bool success2=_qmpack->ParseOrbitalsFile(orb_iter_input);
      _qmpack->CleanUp();

      ofstream ofs;
      string dft_logfile = runFolder + "/dft.log";
      ofs.open(dft_logfile.c_str(), ofstream::out);
      if (!ofs.is_open()) {
        throw runtime_error("Bad file handle: " + dft_logfile);
      }
      ofs << dft_logger << endl;
      ofs.close();
      _qmpack->setLog(_log);
        return success1&&success2;
    }

    bool QMMachine::Iterate(string jobFolder, int iterCnt) {

      // CREATE ITERATION OBJECT & SETUP RUN DIRECTORY
      QMMIter *thisIter = this->CreateNewIter();
      int iter = iterCnt;
      string runFolder = jobFolder + "/iter_" + boost::lexical_cast<string>(iter);

      bool created = boost::filesystem::create_directory(runFolder);
      if (created)
        CTP_LOG(ctp::logDEBUG, *_log) << "Created directory " << runFolder << flush;
      else
        CTP_LOG(ctp::logWARNING, *_log) << "Could not create directory " << runFolder << flush;

      // RUN CLASSICAL INDUCTION & SAVE
      _job->getPolarTop()->PrintPDB(runFolder + "/QM0_MM1_MM2.pdb");
      _xind->Evaluate(_job);

      if(!_xind->hasConverged()){
        throw std::runtime_error("Classical induction did not converge");
      }

      thisIter->setE_FM(_job->getEF00(), _job->getEF01(), _job->getEF02(),
              _job->getEF11(), _job->getEF12(), _job->getEM0(),
              _job->getEM1(), _job->getEM2(), _job->getETOT());

      /* Translate atoms in QM0() to QMAtoms in orbitals object
       * to be used in writing the QM input files for the
       * external QMPackages or directly in internal DFT engine.
       * DRAWBACK: QMAtom positions are fixed for all iterations
       *           unless the UPDATE function at the end of the
       *           iteration updates orb_iter_input (TO BE TESTED)
       */
      QMInterface qminterface;
      if (iterCnt == 0) qminterface.GenerateQMAtomsFromPolarSegs(_job->getPolarTop(), orb_iter_input);

      /* Generate list of polar segments in the MM1() and MM2()
       * region to be used in writing the background multipoles
       * in the external QMPackages or in the direct evaluation
       * in the internal DFT engine.
       */
      std::vector<std::shared_ptr<ctp::PolarSeg> > MultipolesBackground = qminterface.GenerateMultipoleList(_job->getPolarTop());

      bool success=RunDFT(runFolder, MultipolesBackground);
      if (!success) {
        return 1;
      }

double energy_ex=0.0;
      if (_do_gwbse) {
        if (_initialstate.Type()==QMStateType::Gstate) {
         CTP_LOG(ctp::logDEBUG, *_log) <<"QMMMachine no GWBSE necessary for " + _initialstate.ToLongString();
        }
        RunGWBSE(runFolder);
        CTP_LOG(ctp::logDEBUG, *_log) << "Excited state via GWBSE: " << flush;
        CTP_LOG(ctp::logDEBUG, *_log) << "state:" << _initialstate.ToLongString() << flush;
        _filter.PrintInfo();

        QMState newstate= _filter.CalcStateAndUpdate(orb_iter_input);
        energy_ex=orb_iter_input.getExcitedStateEnergy(newstate)* tools::conv::hrt2ev;

        if (!_static_qmmm) {
          Density2Charges(newstate);
        }

      } //_do_gwbse

      /* new ESP fit only required for
       * - polarizable QMMM
       * AND
       * - GWBSE or DFT with internal DFTENGINE
       */
      if (!_static_qmmm && _qmpack->getPackageName() == "xtp" && !_do_gwbse) {
        QMState gsstate=QMState(QMStateType::Gstate,0,false);
        Density2Charges(gsstate);
      } // for polarized QMMM

      if (tools::globals::verbose) {
        CTP_LOG(ctp::logDEBUG, *_log) << "Calculated partial charges" << flush;
        for (const QMAtom* atom : orb_iter_input.QMAtoms()) {
          CTP_LOG(ctp::logDEBUG, *_log) << atom->getType() << " " << atom->getPartialcharge() << flush;
        }
      }

      // EXTRACT & SAVE QM ENERGIES
      double energy_sf = orb_iter_input.getSelfEnergy();
      double energy_qmsf = orb_iter_input.getQMEnergy();
      double energy_qm = energy_qmsf - energy_sf;
      thisIter->setQMSF(energy_qm, energy_sf, energy_ex);
      _job->setEnergy_QMMM(thisIter->getQMEnergy(), thisIter->getGWBSEEnergy(), thisIter->getSFEnergy(),
              thisIter->getQMMMEnergy());

      // EXTRACT & SAVE QMATOM DATA
      std::vector< QMAtom* > &atoms = orb_iter_input.QMAtoms();

      thisIter->UpdatePosChrgFromQMAtoms(atoms, _job->getPolarTop()->QM0());

      if (_do_gdma) {
        RunGDMA(thisIter, runFolder);
      }

      // serialize this iteration
      if (_do_archive) {
        // save orbitals
        std::string orb_file = runFolder + "/system.orb";
        CTP_LOG(ctp::logDEBUG, *_log) << "Archiving data to " << orb_file << flush;
        orb_iter_input.WriteToCpt(orb_file);
      }

      CTP_LOG(ctp::logINFO, *_log)
              << format("Summary - iteration %1$d:") % (iterCnt + 1) << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... QM Size  = %1$d atoms") % int(atoms.size()) << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(QM)    = %1$+4.9e") % thisIter->getQMEnergy() << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(GWBSE) = %1$+4.9e") % thisIter->getGWBSEEnergy() << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(SF)    = %1$+4.9e") % thisIter->getSFEnergy() << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(FM)    = %1$+4.9e") % thisIter->getFMEnergy() << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(MM)    = %1$+4.9e") % thisIter->getMMEnergy() << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... E(QMMM)  = %1$+4.9e") % thisIter->getQMMMEnergy() << flush;
      if (!_static_qmmm) {
        CTP_LOG(ctp::logINFO, *_log)
                << format("... RMS(dR)  = %1$+4.9e") % thisIter->getRMSdR() << flush;
        CTP_LOG(ctp::logINFO, *_log)
                << format("... RMS(dQ)  = %1$+4.9e") % thisIter->getRMSdQ() << flush;
        CTP_LOG(ctp::logINFO, *_log)
                << format("... SUM(dQ)  = %1$+4.9e") % thisIter->getSUMdQ() << flush;
      }
      // CLEAN DIRECTORY

      return 0;
    }

    void QMMachine::Density2Charges(const QMState& state) {

      Eigen::MatrixXd DMAT = orb_iter_input.DensityMatrixFull(state);

      int iter = _iters.size() - 1;
      Eigen::MatrixXd DMAT_mixed;
      if (iter == 0) {
        DMAT_mixed = DMAT;
      } else {
        DMAT_mixed = _alpha * _DMAT_old + (1 - _alpha) * DMAT;
      }

      _DMAT_old = DMAT_mixed;
      BasisSet dftbs;
      if (orb_iter_input.getDFTbasis() != "") {
        dftbs.LoadBasisSet(orb_iter_input.getDFTbasis());
        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Loaded DFT Basis Set " << orb_iter_input.getDFTbasis() << flush;
      }else{
        throw std::runtime_error("Basisset in orb_iter_input not set");
      }
      // fill DFT AO basis by going through all atoms
      AOBasis dftbasis;
      dftbasis.AOBasisFill(dftbs, orb_iter_input.QMAtoms());
      Espfit esp = Espfit(_log);
      esp.Fit2Density(orb_iter_input.QMAtoms(), DMAT_mixed, dftbasis, "medium");

      return;
    }

    QMMIter *QMMachine::CreateNewIter() {

      QMMIter *newIter = new QMMIter(_iters.size());
      this->_iters.push_back(newIter);
      return newIter;
    }

    bool QMMachine::hasConverged() {

    bool  convg_dR = false;
    bool  convg_dQ = false;
    bool  convg_dE_QM = false;
    bool  convg_dE_MM = false;

      if (_iters.size() > 1) {

        QMMIter *iter_0 = _iters[_iters.size() - 2];
        QMMIter *iter_1 = _iters[_iters.size() - 1];

        double dR = iter_1->getRMSdR();
        double dQ = iter_1->getRMSdQ();
        double dE_QM = iter_1->getQMEnergy() - iter_0->getQMEnergy();
        double dE_MM = iter_1->getMMEnergy() - iter_0->getMMEnergy();

        CTP_LOG(ctp::logINFO, *_log)
                << format("... dE_QM  = %1$+4.9e") % dE_QM << flush;
        CTP_LOG(ctp::logINFO, *_log)
                << format("... dE_MM  = %1$+4.9e") % dE_MM << flush;

        if (dR <= _crit_dR) convg_dR = true;
        if (dQ <= _crit_dQ) convg_dQ = true;
        if (dE_QM * dE_QM <= _crit_dE_QM * _crit_dE_QM) convg_dE_QM = true;
        if (dE_MM * dE_MM <= _crit_dE_MM * _crit_dE_MM) convg_dE_MM = true;
      }

      _isConverged = ((convg_dR && convg_dQ) && (convg_dE_QM && convg_dE_MM));

      CTP_LOG(ctp::logINFO, *_log)
              << format("... Convg dR = %s") % (convg_dR ? "true" : "false") << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... Convg dQ = %s") % (convg_dQ ? "true" : "false") << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... Convg QM = %s") % (convg_dE_QM ? "true" : "false") << flush;
      CTP_LOG(ctp::logINFO, *_log)
              << format("... Convg MM = %s") % (convg_dE_MM ? "true" : "false") << flush;

      return _isConverged;
    }

  }
}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <map>
#include <fstream>
#include <string>

#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/trim.hpp>

#include "Tudat/InputOutput/multiDimensionalArrayReader.h"

namespace tudat
{

namespace input_output
{

//! Function to parse a block of values read from a file into a multi-array of size 1.
boost::multi_array< double, 1 > parseRawOneDimensionalCoefficientsFromFile(
        const std::vector< int > independentVariableSize,
        const Eigen::MatrixXd& coefficientsBlock )
{
    boost::multi_array< double, 1 > coefficientMultiarray;

    // Check input consistency
    if ( independentVariableSize.size( ) == 1 )
    {
        // Define size of multi-array
        coefficientMultiarray.resize( boost::extents[ independentVariableSize.at( 0 ) ] );

        // Parse data
        for ( int i = 0; i < independentVariableSize.at( 0 ); i++ )
        {
            coefficientMultiarray[ i ] = coefficientsBlock( i, 0 );
        }
    }
    else
    {
        throw std::runtime_error( "Error, expected size of 1 dimension when parsing 1-dimensional data into multi-array." );
    }
    return coefficientMultiarray;
}

//! Function to parse a block of values read from a file into a multi-array of size 2.
boost::multi_array< double, 2 > parseRawTwoDimensionalCoefficientsFromFile(
        const std::vector< int > independentVariableSize,
        const Eigen::MatrixXd& coefficientsBlock )
{
    boost::multi_array< double, 2 > coefficientMultiarray;

    // Check input consistency
    if ( independentVariableSize.size( ) == 2 )
    {
        // Define size of multi-array
        coefficientMultiarray.resize( boost::extents[ independentVariableSize.at( 0 ) ][ independentVariableSize.at( 1 ) ]  );

        // Parse data
        for ( int i = 0; i < independentVariableSize.at( 0 ); i++ )
        {
            for ( int j = 0; j < independentVariableSize.at( 1 ); j++ )
            {
                coefficientMultiarray[ i ][ j ] = coefficientsBlock( i, j );
            }
        }
    }
    else
    {
        throw std::runtime_error( "Error, expected size of 2 dimensions when parsing 1-dimensional data into multi-array." );
    }
    return coefficientMultiarray;
}

//! Function to parse a block of values read from a file into a multi-array of size 3.
boost::multi_array< double, 3 > parseRawThreeDimensionalCoefficientsFromFile(
        const std::vector< int > independentVariableSize,
        const Eigen::MatrixXd& coefficientsBlock )
{
    boost::multi_array< double, 3 > coefficientMultiarray;

    // Check input consistency
    if ( independentVariableSize.size( ) == 3 )
    {
        // Define size of multi-array
        coefficientMultiarray.resize( boost::extents[ independentVariableSize.at( 0 ) ]
                [ independentVariableSize.at( 1 ) ][ independentVariableSize.at( 2 ) ] );

        // Parse data
        int currentStartRow = 0;
        for ( int k = 0; k < independentVariableSize.at( 2 ); k++ )
        {
            for ( int i = 0; i < independentVariableSize.at( 0 ); i++ )
            {
                for ( int j = 0; j < independentVariableSize.at( 1 ); j++ )
                {
                    coefficientMultiarray[ i ][ j ][ k ] = coefficientsBlock( i + currentStartRow, j );
                }
            }
            currentStartRow += independentVariableSize.at( 0 );
        }
    }
    else
    {
        throw std::runtime_error( "Error, expected size of 3 dimensions when parsing 1-dimensional data into multi-array." );
    }
    return coefficientMultiarray;
}

//! Function to parse a block of values read from a file into a multi-array of size 4.
boost::multi_array< double, 4 > parseRawFourDimensionalCoefficientsFromFile(
        const std::vector< int > independentVariableSize,
        const Eigen::MatrixXd& coefficientsBlock )
{
    boost::multi_array< double, 4 > coefficientMultiarray;

    // Check input consistency
    if ( independentVariableSize.size( ) == 4 )
    {
        // Define size of multi-array
        coefficientMultiarray.resize( boost::extents[ independentVariableSize.at( 0 ) ]
                [ independentVariableSize.at( 1 ) ][ independentVariableSize.at( 2 ) ][ independentVariableSize.at( 3 ) ] );

        // Parse data
        if ( coefficientsBlock.rows( ) == ( independentVariableSize.at( 0 ) * independentVariableSize.at( 2 ) *
                                            independentVariableSize.at( 3 ) ) )
        {
            int currentStartRowFourthDimension = 0;
            for ( int l = 0; l < independentVariableSize.at( 3 ); l++ )
            {
                int currentStartRowThirdDimension = 0;
                for ( int k = 0; k < independentVariableSize.at( 2 ); k++ )
                {
                    for ( int i = 0; i < independentVariableSize.at( 0 ); i++ )
                    {
                        for ( int j = 0; j < independentVariableSize.at( 1 ); j++ )
                        {
                            coefficientMultiarray[ i ][ j ][ k ][ l ] = coefficientsBlock( i + currentStartRowThirdDimension +
                                                                                currentStartRowFourthDimension, j );
                        }
                    }
                    currentStartRowThirdDimension += independentVariableSize.at( 0 );
                }
                currentStartRowFourthDimension += independentVariableSize.at( 2 ) * independentVariableSize.at( 0 );
            }
        }
        else if ( coefficientsBlock.cols( ) == ( independentVariableSize.at( 1 ) * independentVariableSize.at( 3 ) ) )
        {
            int currentStartColumn = 0;
            for ( int l = 0; l < independentVariableSize.at( 3 ); l++ )
            {
                int currentStartRow = 0;
                for ( int k = 0; k < independentVariableSize.at( 2 ); k++ )
                {
                    for ( int i = 0; i < independentVariableSize.at( 0 ); i++ )
                    {
                        for ( int j = 0; j < independentVariableSize.at( 1 ); j++ )
                        {
                            coefficientMultiarray[ i ][ j ][ k ][ l ] = coefficientsBlock( i + currentStartRow, j + currentStartColumn );
                        }
                    }
                    currentStartRow += independentVariableSize.at( 0 );
                }
                currentStartColumn += independentVariableSize.at( 1 );
            }
        }
        else
        {
            throw std::runtime_error( "Error, the way the 4 dimensional data is stored is not currently supported. The fourth dimension can "
                                      "either be stored as extra columns, or as repeating blocks." );
        }
    }
    else
    {
        throw std::runtime_error( "Error, expected size of 4 dimensions when parsing 1-dimensional data into multi-array." );
    }
    return coefficientMultiarray;
}

//! Function to read a coefficient file (data on a structured grid as a function of N independent variables)
void readCoefficientsFile(
        const std::string fileName,
        std::vector< std::vector< double > >& independentVariables,
        Eigen::MatrixXd& coefficientBlock )
{
    // Open file and create file stream.
    std::fstream stream( fileName.c_str( ), std::ios::in );

    // Check if file opened correctly.
    if ( stream.fail( ) )
    {
        throw std::runtime_error( "Data file could not be opened: " + fileName );
    }

    // Initialize boolean that gets set to true once the file header is passed.
    bool isHeaderPassed = 0;
    bool isFirstLinePassed = 0;

    // Line based parsing
    std::string line;
    std::vector< std::string > vectorOfIndividualStrings;

    // Read file line-by-line
    int numberOfDataLinesParsed = 0;
    int numberOfIndependentVariables = -1;
    while ( !stream.fail( ) && !stream.eof( ) )
    {
        // Get line from stream
        std::getline( stream, line );

        // Trim input string (removes all leading and trailing whitespaces).
        boost::algorithm::trim( line );

        // Skip empty and comment lines
        if ( line.size( ) > 0 && !( line.at( 0 ) == '#' ) )
        {
            // Split string into multiple strings, each containing one element from a line from the data file.
            boost::algorithm::split( vectorOfIndividualStrings,
                                     line,
                                     boost::algorithm::is_any_of( "\t ;, " ),
                                     boost::algorithm::token_compress_on );

            // If this is the first line that is read, it should contain the number of independent variables
            if ( !isFirstLinePassed )
            {
                if ( vectorOfIndividualStrings.size( ) != 1 )
                {
                    throw std::runtime_error( "Error when reading multi-array, expected number of independent variables." );
                }
                numberOfIndependentVariables = std::stoi( vectorOfIndividualStrings.at( 0 ) );
                isFirstLinePassed = true;
            }
            // If the file header is not passed, this should contain the independent variables
            else if ( !isHeaderPassed )
            {
                std::vector< double > currentDataPoints;
                for ( unsigned int i = 0; i < vectorOfIndividualStrings.size( ); i++ )
                {
                    currentDataPoints.push_back( std::stod( vectorOfIndividualStrings.at( i ) ) );
                }
                independentVariables.push_back( currentDataPoints );
            }
            else if ( isHeaderPassed )
            {
                // Check line consistency
                if ( vectorOfIndividualStrings.size( ) != static_cast< unsigned int >( coefficientBlock.cols( ) ) )
                {
                    throw std::runtime_error(
                                "Error on data line " + std::to_string( numberOfDataLinesParsed ) +
                                " found " + std::to_string( vectorOfIndividualStrings.size( ) ) +
                                " columns, but expected " + std::to_string( coefficientBlock.cols( ) ) );
                }
                else if ( numberOfDataLinesParsed > coefficientBlock.rows( ) )
                {
                    throw std::runtime_error(
                                "Error on data line " + std::to_string( numberOfDataLinesParsed ) +
                                " expected " +  std::to_string( coefficientBlock.rows( ) ) + "rows." );
                }
                else
                {
                    // Parse data from current line into output matrix.
                    for ( unsigned int i = 0; i < vectorOfIndividualStrings.size( ); i++ )
                    {
                        coefficientBlock( numberOfDataLinesParsed, i ) =
                                std::stod( vectorOfIndividualStrings.at( i ) );
                    }
                    numberOfDataLinesParsed++;
                }
            }

            // If the number of independent variables read is the same as the number of independent variables that
            // should be defined, allocate memory for output matrix.
            if ( ( static_cast< int >( independentVariables.size( ) ) == numberOfIndependentVariables ) && !isHeaderPassed )
            {
                // Check input consistency
                if ( independentVariables.size( ) == 0 )
                {
                    throw std::runtime_error( "Error when reading multi-array, no header found." );
                }
                else
                {
                    // Get information on 4 dimensional file
                    bool fourthDimensionAlongColumns = false;
                    if ( independentVariables.size( ) == 4 )
                    {
                        // Get current position in file
                        std::streampos currentPositionInFile = stream.tellg( );

                        // Read and process next line
                        std::getline( stream, line );
                        while ( line.size( ) == 0 )
                        {
                            std::getline( stream, line );
                        }
                        boost::algorithm::trim( line );
                        boost::algorithm::split( vectorOfIndividualStrings,
                                                 line,
                                                 boost::algorithm::is_any_of( "\t ;, " ),
                                                 boost::algorithm::token_compress_on );

                        // Detect way data was stored
                        unsigned int numberOfColumnsInFile = vectorOfIndividualStrings.size( );
                        if ( numberOfColumnsInFile == ( independentVariables.at( 1 ).size( ) * independentVariables.at( 3 ).size( ) ) )
                        {
                            fourthDimensionAlongColumns = true;
                        }

                        // Rewind to previous position
                        stream.seekg( currentPositionInFile );
                    }

                    // Define size of output matrix, and allocate memory
                    int numberOfRows = independentVariables.at( 0 ).size( );
                    int numberOfColumns = 1;
                    if ( independentVariables.size( ) > 1 )
                    {
                        if ( !fourthDimensionAlongColumns )
                        {
                            numberOfColumns = independentVariables.at( 1 ).size( );
                            for ( unsigned int i = 2; i < independentVariables.size( ); i++ )
                            {
                                numberOfRows *= independentVariables.at( i ).size( );
                            }
                        }
                        else
                        {
                            numberOfColumns = independentVariables.at( 1 ).size( ) * independentVariables.at( 3 ).size( );
                            for ( unsigned int i = 2; i < independentVariables.size( ); i++ )
                            {
                                numberOfRows *= ( i != 3 ) ? independentVariables.at( i ).size( ) : 1;
                            }
                        }
                    }
                    coefficientBlock.setZero( numberOfRows, numberOfColumns );
                }
                isHeaderPassed = true;
            }

        }
    }

    if ( numberOfDataLinesParsed != coefficientBlock.rows( ) )
    {
        throw std::runtime_error(
                    "Error at end of coefficient file reader, found " +
                    std::to_string( numberOfDataLinesParsed ) +
                    " lines, but expected " +  std::to_string( coefficientBlock.rows( ) ) + "rows." );
    }
}

//! Function to retrieve the number of independent variables that the coefficients in a file are given for.
int getNumberOfIndependentVariablesInCoefficientFile( const std::string& fileName )
{
    // Open file and create file stream.
    std::fstream stream( fileName.c_str( ), std::ios::in );

    // Check if file opened correctly.
    if ( stream.fail( ) )
    {
       throw std::runtime_error( "Data file could not be opened: " + fileName );
    }

    std::string line;
    std::vector< std::string > vectorOfIndividualStrings;

    // Retrieve number of independent variables from file.
    int numberOfIndependentVariables = -1;
    while ( !stream.fail( ) && !stream.eof( ) && ( numberOfIndependentVariables < 0 ) )
    {
        // Get line from stream
        std::getline( stream, line );
        boost::algorithm::trim( line );

        if ( line.size( ) > 0 && !( line.at( 0 ) == '#' ) )
        {
            boost::algorithm::split( vectorOfIndividualStrings,
                                     line,
                                     boost::algorithm::is_any_of( "\t ;, " ),
                                     boost::algorithm::token_compress_on );
            try
            {
                numberOfIndependentVariables = std::stod( vectorOfIndividualStrings.at( 0 ) );
            }
            catch( std::runtime_error )
            {
                throw std::runtime_error( "Error when reading coefficicent file size, input is inconsistent." );
            }
        }
    }
    return numberOfIndependentVariables;
}

} // namespace input_output

} // namespace tudat


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <cstddef>
#include <deque>

#include "DataStructures/DataVector.hpp"

/// \ingroup ControlSystemGroup
/// A weighted exponential averager of \f$Q\f$ and its derivatives
/// implementing Appendix A in \cite Hemberger2012jz
///
/// The purpose of the averager is to provide \f$Q\f$ and 'smoothed' numerical
/// derivatives of \f$Q\f$ up to the `DerivOrder`'th derivative. The 0th
/// derivative of \f$Q\f$ is not typically averaged, since no differencing is
/// necessary and no noise is introduced. The `average_0th_deriv_of_q` option
/// allows for specifying that the 0th derivative should be averaged in addition
/// to the derivatives. This may be desirable for control systems where
/// \f$Q\f$ contains some 'noise', e.g. size control typically uses an averaged
/// \f$Q\f$ since the raw computed \f$Q\f$ is a function of the minimum on a
/// surface and may jump around discontinuously.
///
/// The averager is designed to support an arbitrary DerivOrder, however the
/// private get_derivs method currently only supports DerivOrder = 2.
/// If an additional DerivOrder is needed, finite differencing needs to be
/// implemented to specifically handle that order (as it seems like overkill to
/// generalize the differencing stencil at this time).
template <size_t DerivOrder>
class Averager {
 public:
  /// `avg_timescale_frac` determines the exponential averaging timescale
  /// through \f$\tau_\mathrm{avg} = \f$`avg_timescale_frac`\f$\times \tau\f$,
  /// where \f$\tau\f$ is the damping time.
  /// `avg_timescale_frac` must be positive.
  /// `average_0th_deriv_of_q` determines whether the call operator returns an
  /// averaged or unaveraged quantity for the 0th derivative of \f$Q\f$.
  /// `true` returns an averaged 0th derivative of \f$Q\f$.
  /// `false` returns the raw 0th derivative of \f$Q\f$.
  /// The derivatives are always averaged (to reduce noise due to numerical
  /// differentiation), so the `average_0th_deriv_of_q` option only specifies
  /// whether to return an averaged value for the 0th derivative piece of the
  /// function.
  Averager(double avg_timescale_frac, bool average_0th_deriv_of_q) noexcept;

  // Explicitly defined move constructor due to the fact that the std::deque
  // move constructor is not marked noexcept
  Averager(Averager&& rhs) noexcept;
  Averager& operator=(Averager&& rhs) noexcept;
  Averager(const Averager&) = delete;
  Averager& operator=(const Averager&) = delete;
  ~Averager() = default;

  /// Returns \f$Q\f$ and its derivatives at \f$t=\f$`time`, provided there is
  /// sufficient data. The averager is limited by the need for at least
  /// (`DerivOrder` + 1) data points in order to provide the `DerivOrder`'th
  /// derivative. If sufficient data is available, it returns \f$Q\f$ and
  /// averaged derivatives of \f$Q\f$ up to the `DerivOrder`'th derivative, at
  /// \f$t=\f$`time`. If `using_average_0th_deriv_of_q()` is `true`, then the
  /// returned 0th derivative of \f$Q\f$ is also averaged. In the case that
  /// there is insufficient data, the operator returns an
  /// unitialized `boost::optional` (`boost::none`).
  const boost::optional<std::array<DataVector, DerivOrder + 1>>& operator()(
      double time) const noexcept;
  /// A function that allows for resetting the averager.
  void clear() noexcept;
  /// The function responsible for updating the averaged values
  /// at \f$t=\f$`time`. Requires `raw_q` (the raw components of \f$Q(t)\f$)
  /// and `timescales` (the associated damping times for each component).
  void update(double time, const DataVector& raw_q,
              const DataVector& timescales) noexcept;
  /// Returns the latest time at which the averager has sufficient data to
  /// return \f$Q\f$ and its derivatives.
  double last_time_updated() const noexcept;
  /// Returns the exponentially averaged time at \f$t=\f$`time`. The time is
  /// averaged along side \f$Q\f$ to determine the effective time at which
  /// the average value is computed. The effective time is retarded, due to the
  /// weighting of past times.
  double average_time(double time) const noexcept;
  /// Returns a bool corresponding to whether `average_0th_deriv_of_q`
  /// is `true`/`false`.
  bool using_average_0th_deriv_of_q() const noexcept {
    return average_0th_deriv_of_q_;
  };

 private:
  /// Returns the function and numerical derivatives up to the
  /// `DerivOrder`'th derivative using backward finite differencing that
  /// supports non-uniform time spacing. Since we use a stencil size of
  /// (`DerivOrder` + 1), the order of the finite difference varies for
  /// different derivatives. Assuming \f$M\lte\f$`DerivOrder`, the \f$M\f$'th
  /// derivative is of order (`DerivOrder` + 1 - \f$M\f$).
  ///
  /// NOTE: the derivative function currently only supports DerivOrder = 2,
  /// and contains a static_assert to guard against the possible instantiation
  /// of another DerivOrder, for which finite differencing does not
  /// currently support.
  std::array<DataVector, DerivOrder + 1> get_derivs() const noexcept;

  double avg_tscale_frac_;
  bool average_0th_deriv_of_q_;
  boost::optional<std::array<DataVector, DerivOrder + 1>> averaged_values_{};
  boost::optional<std::array<DataVector, DerivOrder + 1>> boost_none_ =
      boost::none;
  std::deque<double> times_;
  std::deque<DataVector> raw_qs_;
  double weight_k_ = 0.0;
  double tau_k_ = 0.0;
};


//  Copyright (c) 2018-2019
//  Cem Bassoy
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Fraunhofer and Google in producing this work
//  which started as a Google Summer of Code project.
//

/// \file tensor.hpp Definition for the tensor template class

#ifndef BOOST_UBLAS_TENSOR_IMPL_HPP
#define BOOST_UBLAS_TENSOR_IMPL_HPP

#include <boost/config.hpp>

#include <initializer_list>

#include "algorithms.hpp"
#include "expression.hpp"
#include "expression_evaluation.hpp"
#include "extents.hpp"
#include "strides.hpp"
#include "index.hpp"

namespace boost { namespace numeric { namespace ublas {

template<class T, class F, class A>
class tensor;

template<class T, class F, class A>
class matrix;

template<class T, class A>
class vector;

///** \brief Base class for Tensor container models
// *
// * it does not model the Tensor concept but all derived types should.
// * The class defines a common base type and some common interface for all
// * statically derived Tensor classes
// * We implement the casts to the statically derived type.
// */
//template<class C>
//class tensor_container:
//		public detail::tensor_expression<C>
//{
//public:
//	static const unsigned complexity = 0;
//	typedef C container_type;
//	typedef tensor_tag type_category;

//	BOOST_UBLAS_INLINE
//	const container_type &operator () () const {
//			return *static_cast<const container_type *> (this);
//	}
//	BOOST_UBLAS_INLINE
//	container_type &operator () () {
//			return *static_cast<container_type *> (this);
//	}
//};

/** @brief A dense tensor of values of type \c T.
		*
		* For a \f$n\f$-dimensional tensor \f$v\f$ and \f$0\leq i < n\f$ every element \f$v_i\f$ is mapped
		* to the \f$i\f$-th element of the container. A storage type \c A can be specified which defaults to \c unbounded_array.
		* Elements are constructed by \c A, which need not initialise their value.
		*
		* @tparam T type of the objects stored in the tensor (like int, double, complex,...)
		* @tparam A The type of the storage array of the tensor. Default is \c unbounded_array<T>. \c <bounded_array<T> and \c std::vector<T> can also be used
		*/
template<class T, class F = first_order, class A = std::vector<T,std::allocator<T>> >
class tensor:
		public detail::tensor_expression<tensor<T, F, A>,tensor<T, F, A>>
{

	static_assert( std::is_same<F,first_order>::value ||
								 std::is_same<F,last_order >::value, "boost::numeric::tensor template class only supports first- or last-order storage formats.");

	using self_type  = tensor<T, F, A>;
public:

	template<class derived_type>
	using tensor_expression_type = detail::tensor_expression<self_type,derived_type>;

	template<class derived_type>
	using matrix_expression_type = matrix_expression<derived_type>;

	template<class derived_type>
	using vector_expression_type = vector_expression<derived_type>;

	using super_type = tensor_expression_type<self_type>;

//	static_assert(std::is_same_v<tensor_expression_type<self_type>, detail::tensor_expression<tensor<T,F,A>,tensor<T,F,A>>>, "tensor_expression_type<self_type>");

	using array_type  = A;
	using layout_type = F;

	using size_type       = typename array_type::size_type;
	using difference_type = typename array_type::difference_type;
	using value_type      = typename array_type::value_type;

	using reference       = typename array_type::reference;
	using const_reference = typename array_type::const_reference;

	using pointer         = typename array_type::pointer;
	using const_pointer   = typename array_type::const_pointer;

	using iterator        = typename array_type::iterator;
	using const_iterator  = typename array_type::const_iterator;

	using reverse_iterator        = typename array_type::reverse_iterator;
	using const_reverse_iterator  = typename array_type::const_reverse_iterator;

	using tensor_temporary_type = self_type;
	using storage_category = dense_tag;

	using strides_type = basic_strides<std::size_t,layout_type>;
	using extents_type = shape;

	using matrix_type     = matrix<value_type,layout_type,array_type>;
	using vector_type     = vector<value_type,array_type>;

	/** @brief Constructs a tensor.
	 *
	 * @note the tensor is empty.
	 * @note the tensor needs to reshaped for further use.
	 *
	 */
	BOOST_UBLAS_INLINE
	constexpr tensor ()
		: tensor_expression_type<self_type>() // container_type
		, extents_()
		, strides_()
		, data_()
	{
	}

	/** @brief Constructs a tensor with an initializer list
	 *
	 * By default, its elements are initialized to 0.
	 *
	 * @code tensor<float> A{4,2,3}; @endcode
	 *
	 * @param l initializer list for setting the dimension extents of the tensor
	 */
	explicit BOOST_UBLAS_INLINE
	tensor (std::initializer_list<size_type> l)
		: tensor_expression_type<self_type>()
		, extents_ (std::move(l))
		, strides_ (extents_)
		, data_    (extents_.product())
	{
	}

	/** @brief Constructs a tensor with a \c shape
	 *
	 * By default, its elements are initialized to 0.
	 *
	 * @code tensor<float> A{extents{4,2,3}}; @endcode
	 *
	 * @param s initial tensor dimension extents
	 */
	explicit BOOST_UBLAS_INLINE
	tensor (extents_type const& s)
		: tensor_expression_type<self_type>() //tensor_container<self_type>()
		, extents_ (s)
		, strides_ (extents_)
		, data_    (extents_.product())
	{}

	/** @brief Constructs a tensor with a \c shape and initiates it with one-dimensional data
	 *
	 * @code tensor<float> A{extents{4,2,3}, array }; @endcode
	 *
	 *
	 *  @param s initial tensor dimension extents
	 *  @param a container of \c array_type that is copied according to the storage layout
	 */
	BOOST_UBLAS_INLINE
	tensor (extents_type const& s, const array_type &a)
		: tensor_expression_type<self_type>() //tensor_container<self_type>()
		, extents_ (s)
		, strides_ (extents_)
		, data_    (a)
	{
		if(this->extents_.product() != this->data_.size())
			throw std::runtime_error("Error in boost::numeric::ublas::tensor: size of provided data and specified extents do not match.");
	}

	/** @brief Constructs a tensor using a shape tuple and initiates it with a value.
	 *
	 *  @code tensor<float> A{extents{4,2,3}, 1 }; @endcode
	 *
	 *  @param e initial tensor dimension extents
	 *  @param i initial value of all elements of type \c value_type
	 */
	BOOST_UBLAS_INLINE
	tensor (extents_type const& e, const value_type &i)
		: tensor_expression_type<self_type>() //tensor_container<self_type> ()
		, extents_ (e)
		, strides_ (extents_)
		, data_    (extents_.product(), i)
	{}

	/** @brief Constructs a tensor from another tensor
	 *
	 *  @param v tensor to be copied.
	 */
	BOOST_UBLAS_INLINE
	tensor (const tensor &v)
		: tensor_expression_type<self_type>()
		, extents_ (v.extents_)
		, strides_ (v.strides_)
		, data_    (v.data_   )
	{}

	/** @brief Constructs a tensor from another tensor
	 *
	 *  @param v tensor to be moved.
	 */
	BOOST_UBLAS_INLINE
	tensor (tensor &&v)
		: tensor_expression_type<self_type>() //tensor_container<self_type> ()
		, extents_ (std::move(v.extents_))
		, strides_ (std::move(v.strides_))
		, data_    (std::move(v.data_   ))
	{}

	/** @brief Constructs a tensor with a matrix
	 *
	 * \note Initially the tensor will be two-dimensional.
	 *
	 *  @param v matrix to be copied.
	 */
	BOOST_UBLAS_INLINE
	tensor (const matrix_type &v)
		: tensor_expression_type<self_type>()
		, extents_ ()
		, strides_ ()
		, data_    (v.data())
	{
		if(!data_.empty()){
			extents_ = extents_type{v.size1(),v.size2()};
			strides_ = strides_type(extents_);
		}
	}

	/** @brief Constructs a tensor with a matrix
	 *
	 * \note Initially the tensor will be two-dimensional.
	 *
	 *  @param v matrix to be moved.
	 */
	BOOST_UBLAS_INLINE
	tensor (matrix_type &&v)
		: tensor_expression_type<self_type>()
		, extents_ {}
		, strides_ {}
		, data_    {}
	{
		if(v.size1()*v.size2() != 0){
			extents_ = extents_type{v.size1(),v.size2()};
			strides_ = strides_type(extents_);
			data_    = std::move(v.data());
		}
	}

	/** @brief Constructs a tensor using a \c vector
	 *
	 * @note It is assumed that vector is column vector
	 * @note Initially the tensor will be one-dimensional.
	 *
	 *  @param v vector to be copied.
	 */
	BOOST_UBLAS_INLINE
	tensor (const vector_type &v)
		: tensor_expression_type<self_type>()
		, extents_ ()
		, strides_ ()
		, data_    (v.data())
	{
		if(!data_.empty()){
			extents_ = extents_type{data_.size(),1};
			strides_ = strides_type(extents_);
		}
	}

	/** @brief Constructs a tensor using a \c vector
	 *
	 *  @param v vector to be moved.
	 */
	BOOST_UBLAS_INLINE
	tensor (vector_type &&v)
		: tensor_expression_type<self_type>()
		, extents_ {}
		, strides_ {}
		, data_    {}
	{
		if(v.size() != 0){
			extents_ = extents_type{v.size(),1};
			strides_ = strides_type(extents_);
			data_    = std::move(v.data());
		}
	}

	/** @brief Constructs a tensor with another tensor with a different layout
	 *
	 * @param other tensor with a different layout to be copied.
	 */
	BOOST_UBLAS_INLINE
	template<class other_layout>
	tensor (const tensor<value_type, other_layout> &other)
		: tensor_expression_type<self_type> ()
		, extents_ (other.extents())
		, strides_ (other.extents())
		, data_    (other.extents().product())
	{
		copy(this->rank(), this->extents().data(),
				 this->data(), this->strides().data(),
				 other.data(), other.strides().data());
	}

	/** @brief Constructs a tensor with an tensor expression
	 *
	 * @code tensor<float> A = B + 3 * C; @endcode
	 *
	 * @note type must be specified of tensor must be specified.
	 * @note dimension extents are extracted from tensors within the expression.
	 *
	 * @param expr tensor expression
	 */
	BOOST_UBLAS_INLINE
	template<class derived_type>
	tensor (const tensor_expression_type<derived_type> &expr)
		: tensor_expression_type<self_type> ()
		, extents_ ( detail::retrieve_extents(expr) )
		, strides_ ( extents_ )
		, data_    ( extents_.product() )
	{
		static_assert( detail::has_tensor_types<self_type, tensor_expression_type<derived_type>>::value,
									 "Error in boost::numeric::ublas::tensor: expression does not contain a tensor. cannot retrieve shape.");
		detail::eval( *this, expr );
	}

	/** @brief Constructs a tensor with a matrix expression
	 *
	 * @code tensor<float> A = B + 3 * C; @endcode
	 *
	 * @note matrix expression is evaluated and pushed into a temporary matrix before assignment.
	 * @note extents are automatically extracted from the temporary matrix
	 *
	 * @param expr matrix expression
	 */
	BOOST_UBLAS_INLINE
	template<class derived_type>
	tensor (const matrix_expression_type<derived_type> &expr)
		: tensor(  matrix_type ( expr )  )
	{
	}

	/** @brief Constructs a tensor with a vector expression
	 *
	 * @code tensor<float> A = b + 3 * b; @endcode
	 *
	 * @note matrix expression is evaluated and pushed into a temporary matrix before assignment.
	 * @note extents are automatically extracted from the temporary matrix
	 *
	 * @param expr vector expression
	 */
	BOOST_UBLAS_INLINE
	template<class derived_type>
	tensor (const vector_expression_type<derived_type> &expr)
		: tensor(  vector_type ( expr )  )
	{
	}

	/** @brief Evaluates the tensor_expression and assigns the results to the tensor
	 *
	 * @code A = B + C * 2;  @endcode
	 *
	 * @note rank and dimension extents of the tensors in the expressions must conform with this tensor.
	 *
	 * @param expr expression that is evaluated.
	 */
	BOOST_UBLAS_INLINE
	template<class derived_type>
	tensor &operator = (const tensor_expression_type<derived_type> &expr)
	{
		detail::eval(*this, expr);
		return *this;
	}

	tensor& operator=(tensor other)
	{
		swap (*this, other);
		return *this;
	}

	tensor& operator=(const_reference v)
	{
		std::fill(this->begin(), this->end(), v);
		return *this;
	}

	/** @brief Returns true if the tensor is empty (\c size==0) */
	BOOST_UBLAS_INLINE
	bool empty () const {
		return this->data_.empty();
	}

	/** @brief Returns the size of the tensor */
	BOOST_UBLAS_INLINE
	size_type size () const {
		return this->data_.size ();
	}

	/** @brief Returns the size of the tensor */
	BOOST_UBLAS_INLINE
	size_type size (size_type r) const {
		return this->extents_.at(r);
	}

	/** @brief Returns the number of dimensions/modes of the tensor */
	BOOST_UBLAS_INLINE
	size_type rank () const {
		return this->extents_.size();
	}

	/** @brief Returns the number of dimensions/modes of the tensor */
	BOOST_UBLAS_INLINE
	size_type order () const {
		return this->extents_.size();
	}

	/** @brief Returns the strides of the tensor */
	BOOST_UBLAS_INLINE
	strides_type const& strides () const {
		return this->strides_;
	}

	/** @brief Returns the extents of the tensor */
	BOOST_UBLAS_INLINE
	extents_type const& extents () const {
		return this->extents_;
	}

	/** @brief Returns a \c const reference to the container. */
	BOOST_UBLAS_INLINE
	const_pointer data () const {
		return this->data_.data();
	}

	/** @brief Returns a \c const reference to the container. */
	BOOST_UBLAS_INLINE
	pointer data () {
		return this->data_.data();
	}

	/** @brief Element access using a single index.
	 *
	 *  @code auto a = A[i]; @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size()
	 */
	BOOST_UBLAS_INLINE
	const_reference operator [] (size_type i) const {
		return this->data_[i];
	}

	/** @brief Element access using a single index.
	 *
	 *
	 *  @code A[i] = a; @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size()
	 */
	BOOST_UBLAS_INLINE
	reference operator [] (size_type i)
	{
		return this->data_[i];
	}

	/** @brief Element access using a multi-index or single-index.
	 *
	 *
	 *  @code auto a = A.at(i,j,k); @endcode or
	 *  @code auto a = A.at(i);     @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size() if sizeof...(is) == 0, else 0<= i < this->size(0)
	 *  @param is zero-based indices where 0 <= is[r] < this->size(r) where  0 < r < this->rank()
	 */
	template<class ... size_types>
	BOOST_UBLAS_INLINE
	const_reference at (size_type i, size_types ... is) const {
		if constexpr (sizeof...(is) == 0)
			return this->data_[i];
		else
			return this->data_[detail::access<0ul>(size_type(0),this->strides_,i,std::forward<size_types>(is)...)];
	}

	/** @brief Element access using a multi-index or single-index.
	 *
	 *
	 *  @code A.at(i,j,k) = a; @endcode or
	 *  @code A.at(i) = a;     @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size() if sizeof...(is) == 0, else 0<= i < this->size(0)
	 *  @param is zero-based indices where 0 <= is[r] < this->size(r) where  0 < r < this->rank()
	 */
	BOOST_UBLAS_INLINE
	template<class ... size_types>
	reference at (size_type i, size_types ... is) {
		if constexpr (sizeof...(is) == 0)
			return this->data_[i];
		else
			return this->data_[detail::access<0ul>(size_type(0),this->strides_,i,std::forward<size_types>(is)...)];
	}

	/** @brief Element access using a single index.
	 *
	 *
	 *  @code A(i) = a; @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size()
	 */
	BOOST_UBLAS_INLINE
	const_reference operator()(size_type i) const {
		return this->data_[i];
	}

	/** @brief Element access using a single index.
	 *
	 *  @code A(i) = a; @endcode
	 *
	 *  @param i zero-based index where 0 <= i < this->size()
	 */
	BOOST_UBLAS_INLINE
	reference operator()(size_type i){
		return this->data_[i];
	}

	/** @brief Generates a tensor index for tensor contraction
	 *
	 *
	 *  @code auto Ai = A(_i,_j,k); @endcode
	 *
	 *  @param i placeholder
	 *  @param is zero-based indices where 0 <= is[r] < this->size(r) where  0 < r < this->rank()
	 */
	BOOST_UBLAS_INLINE
	template<std::size_t I, class ... index_types>
	decltype(auto) operator() (index::index_type<I> p, index_types ... ps) const
	{
		constexpr auto N = sizeof...(ps)+1;
		if( N != this->rank() )
			throw std::runtime_error("Error in boost::numeric::ublas::operator(): size of provided index_types does not match with the rank.");

		return std::make_pair( std::cref(*this),  std::make_tuple( p, std::forward<index_types>(ps)... ) );
	}

	/** @brief Reshapes the tensor
	 *
	 *
	 * (1) @code A.reshape(extents{m,n,o});     @endcode or
	 * (2) @code A.reshape(extents{m,n,o},4);   @endcode
	 *
	 * If the size of this smaller than the specified extents than
	 * default constructed (1) or specified (2) value is appended.
	 *
	 * @note rank of the tensor might also change.
	 *
	 * @param e extents with which the tensor is reshaped.
	 * @param v value which is appended if the tensor is enlarged.
	 */
	BOOST_UBLAS_INLINE
	void reshape (extents_type const& e, value_type v = value_type{})
	{
		this->extents_ = e;
		this->strides_ = strides_type(this->extents_);

		if(e.product() != this->size())
			this->data_.resize (this->extents_.product(), v);
	}

	friend void swap(tensor& lhs, tensor& rhs) {
		std::swap(lhs.data_   , rhs.data_   );
		std::swap(lhs.extents_, rhs.extents_);
		std::swap(lhs.strides_, rhs.strides_);
	}

	/// \brief return an iterator on the first element of the tensor
	BOOST_UBLAS_INLINE
	const_iterator begin () const {
		return data_.begin ();
	}

	/// \brief return an iterator on the first element of the tensor
	BOOST_UBLAS_INLINE
	const_iterator cbegin () const {
		return data_.cbegin ();
	}

	/// \brief return an iterator after the last element of the tensor
	BOOST_UBLAS_INLINE
	const_iterator end () const {
		return data_.end();
	}

	/// \brief return an iterator after the last element of the tensor
	BOOST_UBLAS_INLINE
	const_iterator cend () const {
		return data_.cend ();
	}

	/// \brief Return an iterator on the first element of the tensor
	BOOST_UBLAS_INLINE
	iterator begin () {
		return data_.begin();
	}

	/// \brief Return an iterator at the end of the tensor
	BOOST_UBLAS_INLINE
	iterator end () {
		return data_.end();
	}

	/// \brief Return a const reverse iterator before the first element of the reversed tensor (i.e. end() of normal tensor)
	BOOST_UBLAS_INLINE
	const_reverse_iterator rbegin () const {
		return data_.rbegin();
	}

	/// \brief Return a const reverse iterator before the first element of the reversed tensor (i.e. end() of normal tensor)
	BOOST_UBLAS_INLINE
	const_reverse_iterator crbegin () const {
		return data_.crbegin();
	}

	/// \brief Return a const reverse iterator on the end of the reverse tensor (i.e. first element of the normal tensor)
	BOOST_UBLAS_INLINE
	const_reverse_iterator rend () const {
		return data_.rend();
	}

	/// \brief Return a const reverse iterator on the end of the reverse tensor (i.e. first element of the normal tensor)
	BOOST_UBLAS_INLINE
	const_reverse_iterator crend () const {
		return data_.crend();
	}

	/// \brief Return a const reverse iterator before the first element of the reversed tensor (i.e. end() of normal tensor)
	BOOST_UBLAS_INLINE
	reverse_iterator rbegin () {
		return data_.rbegin();
	}

	/// \brief Return a const reverse iterator on the end of the reverse tensor (i.e. first element of the normal tensor)
	BOOST_UBLAS_INLINE
	reverse_iterator rend () {
		return data_.rend();
	}

#if 0
	// -------------
	// Serialization
	// -------------

	/// Serialize a tensor into and archive as defined in Boost
	/// \param ar Archive object. Can be a flat file, an XML file or any other stream
	/// \param file_version Optional file version (not yet used)
	template<class Archive>
	void serialize(Archive & ar, const unsigned int /* file_version */){
		ar & serialization::make_nvp("data",data_);
	}
#endif

private:

	extents_type extents_;
	strides_type strides_;
	array_type data_;
};

}}} // namespaces

#endif


/*! \file eigenvaluesearch.cpp
    \brief エネルギー固有値検索を行うクラスの実装

    Copyright ©  2015 @dc1394 All Rights Reserved.
    This software is released under the BSD 2-Clause License.
*/

#include "eigenvaluesearch.h"
#include <cmath>                // for std::fabs, std::log10
#include <iomanip>              // for std::setprecision
#include <iostream>             // for std::cot, std::cerr
#include <boost/assert.hpp>     // for BOOST_ASSERT
#include <boost/cast.hpp>       // for boost::numeric_cast
#include <gsl/gsl_errno.h>      // for GSL_SUCCESS
#include <gsl/gsl_roots.h>      // for gsl_root_fsolver

namespace schrac {
    // #region staticメンバ変数

    bool EigenValueSearch::nodeok = false;

    // #endregion staticメンバ変数

    // #region コンストラクタ

    EigenValueSearch::EigenValueSearch(std::shared_ptr<Data> const & pdata, std::shared_ptr<DiffData> const & pdiffdata, std::shared_ptr<Rho> const & prho, std::shared_ptr<Vhartree> const & pvh) :
        PData([this]() { return std::cref(pdata_); }, nullptr),
        PDiffSolver([this]() { return std::cref(pdiffsolver_); }, nullptr),
        loop_(1),
        pdata_(pdata),
        pdiffdata_(pdiffdata),
        pvh_(pvh)
    {
        initialize(prho);
        setoutstream();
    }

    // #endregion コンストラクタ

    // #region publicメンバ関数

    bool EigenValueSearch::search()
    {
        for (; loop_ < EigenValueSearch::EVALSEARCHMAX; loop_++) {
            if (!rough_search()) {
                return false;
            }

            if (brent() && EigenValueSearch::nodeok) {
                return true;
            }
            else {
                pdiffsolver_->E_ += DE_;

                if (pdiffsolver_->E_ > 0.0) {
                    return false;
                }
            }
        }

        return false;
    }

    // #endregion publicメンバ関数

    // #region privateメンバ関数

    bool EigenValueSearch::brent()
    {
        std::unique_ptr<gsl_root_fsolver, decltype(&gsl_root_fsolver_free)> s(
            gsl_root_fsolver_alloc(gsl_root_fsolver_brent),
            gsl_root_fsolver_free);

        gsl_function F;

        F.function = &func_D;
        F.params = reinterpret_cast<void *>(pdiffsolver_.get());

        gsl_root_fsolver_set(s.get(), &F, Emin_, Emax_);

        for (; loop_ < EVALSEARCHMAX; loop_++) {
            auto const ret = gsl_root_fsolver_iterate(s.get());

            switch (ret)
            {
            case GSL_EBADFUNC:
                std::cerr << "the iteration encountered a singular point where"
                    << "the function or its derivative evaluated to Inf or NaN.\n";
                    return false;
                break;

            case GSL_EZERODIV:
                std::cerr << "the derivative of the function vanished at the iteration point,"
                    << "preventing the algorithm from continuing without a division by zero.\n";
                    return false;
                break;

            default:
                break;
            }

            if (pdata_->chemical_symbol_ == Data::Chemical_Symbol[0]) {
                info(func_D(pdiffsolver_->E_, F.params), pdiffsolver_->E_);
            }

            pdiffsolver_->E_ = gsl_root_fsolver_root(s.get());

            auto const status = gsl_root_test_interval(
                gsl_root_fsolver_x_lower(s.get()),
                gsl_root_fsolver_x_upper(s.get()),
                0.0,
                pdata_->eps_);

            if (status == GSL_SUCCESS) {
                break;
            }
        }

        return loop_ != EVALSEARCHMAX;
    }

    void EigenValueSearch::info() const
    {
        std::cout << "i = " << loop_ << ", D = " << Dold << ", node = "
            << pdiffdata_->thisnode_;

        if (EigenValueSearch::nodeok) {
            std::cout << " (OK)" << std::endl;
        }
        else {
            std::cout << " (NG)" << std::endl;
        }
    }

    void EigenValueSearch::info(double D, double E) const
    {
        std::cout << "i = " << loop_ << ", D = "
            << D << ", E = " << E
            << ", node = "
            << pdiffdata_->thisnode_;

        if (EigenValueSearch::nodeok) {
            std::cout << " (OK)" << std::endl;
        }
        else {
            std::cout << " (NG)" << std::endl;
        }
    }

    void EigenValueSearch::initialize(std::shared_ptr<Rho> const & prho)
    {
        switch (pdata_->eq_type_) {
        case Data::Eq_type::SCH:
                Eapprox_ = Eapprox_sch(pdata_);
            break;

        case Data::Eq_type::SDIRAC:
        case Data::Eq_type::DIRAC:
                Eapprox_ = Eapprox_dirac(pdata_);
            break;

        default:
                BOOST_ASSERT("何かがおかしい！！");
            break;
        }

        pdiffsolver_ = std::make_shared<DiffSolver>(pdata_, pdiffdata_, prho, pvh_);

        if (pdata_->search_lowerE_) {
            pdiffsolver_->E_ = *pdata_->search_lowerE_;
            DE_ = - pdiffsolver_->E_ / static_cast<double>(pdata_->num_of_partition_);
        } else {
            pdiffsolver_->E_ = Eapprox_;
            DE_ = - pdiffsolver_->E_ / static_cast<double>(pdata_->num_of_partition_);
            pdiffsolver_->E_ -= 3.0 * DE_;
        }
    }

    bool EigenValueSearch::rough_search()
    {
        auto pdiffsolver = reinterpret_cast<void *>(pdiffsolver_.get());
        Dold = func_D(pdiffsolver_->E_, pdiffsolver);

        if (pdata_->chemical_symbol_ == Data::Chemical_Symbol[0]) {
            info();
        }

        ++loop_;

        for (; loop_ < EVALSEARCHMAX; loop_++) {
            pdiffsolver_->E_ += DE_;

            if (pdiffsolver_->E_ > 0.0) {
                return false;
            }

            auto const Dnew = func_D(pdiffsolver_->E_, pdiffsolver);

            if (Dnew * Dold < 0.0) {
                Emax_ = pdiffsolver_->E_;
                Emin_ = pdiffsolver_->E_ - DE_;

                break;
            } else {
                Dold = Dnew;
            }

            if (pdata_->chemical_symbol_ == Data::Chemical_Symbol[0]) {
                info();
            }
        }

        return loop_ != EVALSEARCHMAX;
    }

    void EigenValueSearch::setoutstream() const
    {
        std::cout.setf(std::ios::fixed, std::ios::floatfield);
        std::cout << std::setprecision(
            boost::numeric_cast<std::streamsize>(std::fabs(std::log10(pdata_->eps_))));
    }

    // #endregion privateメンバ関数

    // #region 非メンバ関数

    double func_D(double E, void * params)
    {
        auto pdiffsolver = reinterpret_cast<DiffSolver *>(params);
        pdiffsolver->initialize(E);
        pdiffsolver->solve_diff_equ();

        EigenValueSearch::nodeok =
            pdiffsolver->PDiffData()->node_ == pdiffsolver->PDiffData()->thisnode_;

        auto [L, M] = pdiffsolver->getMPval();
        return M[0] - (L[0] / L[1]) * M[1];
    }

    double Eapprox_dirac(std::shared_ptr<Data> const & pdata)
    {
        auto const nr = static_cast<double>(pdata->n_) - pdata->j_ - 0.5;
        auto const lambda = std::sqrt(sqr(pdata->kappa_) - sqr(pdata->Z_ / Data::c));
        auto const denominator = std::sqrt(sqr(nr + lambda) + sqr(pdata->Z_ / Data::c));

        return ((nr + lambda) / denominator - 1.0) * sqr(Data::c);
    }

    double Eapprox_sch(std::shared_ptr<Data> const & pdata)
    {
        return -sqr(pdata->Z_ / static_cast<double>(pdata->n_)) / 2.0;
    }

    // #endregion 非メンバ関数
}