#include "../include/ParameterPopulation.h"
#include "../include/io_functions.h"
#include "../include/constants.h"

using namespace std;

////////////////////////////// Helper functions ///////////////////////////////

inline bool
floatsDiffer(const double & x, const double & y)
{
  return (abs(x - y) / (abs(x + y + 1.0)) > 1.0e-9);
}

inline double
randomParamValue(const ParameterDescription & param, RandGen & r)
{
  // return a random value consistant with the ranges and type of the parameter
  //  description, using random number generator r
  switch(param.type()) {
    case ParameterDescription::Constant:
      return param.defaultValue();
    case ParameterDescription::LogDistributed:
      return param.minRange * pow(param.maxRange / param.minRange,
                                  r.openUniform());
    case ParameterDescription::UniformDistributed:
      return param.minRange + (param.maxRange - param.minRange) *
                              r.openUniform();
    case ParameterDescription::Invalid:
    default:
      throw ParameterPopulationException("Invalid parameter ranges for " +
                                         param.name);
      return NaN;
  }
}

inline bool
compatible(const ParameterDescription & pd1,
           const ParameterDescription & pd2)
{
  // check if two parameter descriptions are compatible (i.e. one could
  // plausibly be from start-up, and the other from resuming

  // the names must be the same
  if(pd1.name != pd2.name) {
    cout << pd1.name << " != " << pd2.name << '\n';
    cout.flush();
    return false;
  }

  // the types must be the same
  if(pd1.type() != pd2.type()) {
    cout << "incompatible desc types\n"; cout.flush();
    return false;
  }

  if(pd1.type() == ParameterDescription::Constant) {
    if(floatsDiffer(pd1.defaultValue(), pd2.defaultValue())) {
        cout << "incompatible const\n"; cout.flush();
        cout << pd1 << '\n' << pd2 << '\n'; cout.flush();
      return false;
    }
  }
  else {
    if(floatsDiffer(pd1.minAllowed, pd2.minAllowed) ||
       floatsDiffer(pd1.maxAllowed, pd2.maxAllowed)) {
      // the min and max allowed values must be the same
      cout << "incompatible ranges\n"; cout.flush();
      cout << pd1 << '\n' << pd2 << '\n'; cout.flush();
      return false;
    }
  }

  // the ranges don't have to be the same, since that changes during fitting
  return true;
}

inline bool
compatible(const ParameterDescriptionList & pdlResume,
           const ParameterDescriptionList & pdlStartup)
{
  // check if two parameter description lists are compatible (i.e. one could
  // plausibly be from start-up, and the other from resuming

  // Check if they have the same length. The resume description may be longer
  // due to added state descriptions
  if(pdlResume.size() < pdlStartup.size()) {
    cout << "incompatible pdl sizes\n"; cout.flush();
    return false;
  }

  // loop through parameter descriptions and check if any of them are
  // incompatible. Loop over the startup descriptions
  ParameterDescriptionList::const_iterator pdItr1 = pdlStartup.begin();
  const ParameterDescriptionList::const_iterator end1 = pdlStartup.end();
  ParameterDescriptionList::const_iterator pdItr2 = pdlResume.begin();
  for(; pdItr1 != end1; ++pdItr1, ++pdItr2)
    if(!compatible(*pdItr1, *pdItr2)) {
      cout << "incompatible pds\n"; cout.flush();
      return false;
    }

  // All parameter descriptions compatible, so the lists are compatible
  return true;
}

inline void
jumpPastComment(istream & in)
{
  string dummy;
  while(in.good()) {
    switch(in.peek()) {
      case ' ':
      case '\t':
      case '\n':
        // ignore white-space
        in.ignore(1);
        break;

      case '#':
        // comment, remove rest of line
        getline(in, dummy);
        break;

      default:
        // non-comment, non-white-space
        return;
    }
  }
}

inline size_t
getSize(istream & in)
{
  // get a non-empty, non-comment line and read in a size_t

  string line, rawLine;
  SplitLine splitLine;
  while(splitLine.empty()) {
    if(in.bad())
      throw ParameterPopulationException("Error reading size from stream");

    getline(in, line);
    rawLine = line;
    in.peek();  //peek helps trip eof flag

      // look for comment
    size_t commentInd = line.find('#');
    if(commentInd != std::string::npos) {
      // remove everything after comment
      line.erase(commentInd);
    }

    if(line.empty())
      // empty line or just space
      continue;

    // split line up into words
    splitLine = split(line);
    if(splitLine.empty())
      // just empty space on line
      continue;
  }

  // should be just one element in line (the size_t)
  if(splitLine.size() != 1)
    throw ParameterPopulationException("Bad line reading ParameterPopulation: "
                                       + rawLine);

  // convert line to size_t
  return stringToSize(splitLine[0]);
}

inline size_t
numWordsInNextLine(istream & in)
{
  // compute the number of words left in the next line, but leave in unchanged

  // get the starting position of in stream
  streampos startPos = in.tellg();

  // read in the next line
  string line;
  getline(in, line);
  // split it into words
  SplitLine splitLine = split(line);
  // compute the number of words
  size_t numWords = splitLine.size();

  // rewind to starting position
  in.seekg(startPos, ios::beg);

  return numWords;
}

double
maxDistance(const ParameterSet & pS1, const ParameterSet & pS2,
            const ParameterDescriptionList & pDList)
{
  double maxDist = 0.0;
  for(size_t n = 0; n < pDList.size(); ++n) {
    switch(pDList[n].type()) {
      case ParameterDescription::UniformDistributed:
        maxDist = std::max( maxDist, abs(pS1.parameters[n] -
                                         pS2.parameters[n]) );
        break;

      case ParameterDescription::LogDistributed:
        maxDist = std::max( maxDist, abs(log(pS1.parameters[n] /
                                             pS2.parameters[n])) );
        break;

      default:
        ;
    }
  }
  return maxDist;
}

template <class T>
inline T
square(const T & x)
{
  return x*x;
}

inline double
pDistance(const ParameterSet & p1, const ParameterSet & p2,
          const ParameterDescriptionList & parameterDescriptions)
{
  double d = 0.0;
  auto p1Itr = p1.parameters.cbegin();
  auto p2Itr = p2.parameters.cbegin();
  for(const auto & desc : parameterDescriptions) {
    switch(desc.type()) {
      case ParameterDescription::LogDistributed:
        d += square(log(*p1Itr / *p2Itr));
        break;
      case ParameterDescription::UniformDistributed:
        d += square(*p1Itr - *p2Itr);
        break;
      default:
        ;
    }
    ++p1Itr;
    ++p2Itr;
  }
  return d;
}

///////////////////// GenerationSummary member functions //////////////////////
GenerationSummary::GenerationSummary(const ParameterSet & paramSet,
                                     const Timer & genTime,
                                     const Timer & totTime,
                                     const double & f,
                                     const double & logRange,
                                     const double & uniformRange) :
  fRange (f), maxLogRange (logRange), maxUniformRange (uniformRange)
{
  // assign bestParameters
  bestParameters = paramSet;

  // assign generationTime by:
  stringstream ssGen;
  //   -writing genTime's elapsed time to a stringstream
  ssGen << genTime;
  //   -saving as a string
  generationTime = ssGen.str();

  // similarly assing totalTime:
  stringstream ssTot;
  //   -writing genTime's elapsed time to a stringstream
  ssTot << totTime;
  //   -saving as a string
  totalTime = ssTot.str();
}

///////////////////// GenerationSummary friend functions //////////////////////
ostream &
operator<<(ostream & out, const GenerationSummary & genSum)
{
    // record format flags
  const ios_base::fmtflags formatFlags = out.flags();
  // record width;
  const streamsize width = out.width();
  // record precision
  const streamsize precision = out.precision();

  // calculate a few widths to ensure all the significant info in the double
  //  is output
  const streamsize doublePrecision = numeric_limits<double>::digits10 + 1;
  const streamsize expWidth = (streamsize)ceil(
    log10((double)numeric_limits<double>::max_exponent10));
  const streamsize doubleWidth = doublePrecision + expWidth + 4;

  // output the summary
  //   -output the two time strings
  out.width(24);
  out << left << genSum.generationTime;
  out.width(24);
  out << left << genSum.totalTime;

  //   -output the range descriptions
  //      -set output decimal precision
  out.precision(doublePrecision);
  // set to default-style output
  out.unsetf(ios_base::floatfield);

  //      -output fRange, maxLogRange, maxUniformRange
  out.width(doubleWidth);
  out << left << genSum.fRange;
  out.width(doubleWidth);
  out << left << genSum.maxLogRange;
  out.width(doubleWidth);
  out << left << genSum.maxUniformRange;

  //   -output the best parameter set
  out << genSum.bestParameters;

  // restore flags
  out.setf(formatFlags);
  // restore width
  out.width(width);
  // restore precision
  out.precision(precision);

  return out;
}

istream &
operator>>(istream & in, GenerationSummary & genSum)
{
  // skip whitespace
  in >> ws;
  // get the generation time (which ends in 's')
  getline(in, genSum.generationTime, 's');
  // the 's' is discarded by getline, so append it to the end
  genSum.generationTime += 's';

  // skip whitespace
  in >> ws;
  // get the total time (which ends in 's')
  getline(in, genSum.totalTime, 's');
  // the 's' is discarded by getline, so append it to the end
  genSum.totalTime += 's';

  // older versions of ParameterPopulation don't output fRange, maxLogRange,
  // maxUniformRange. To distinguish this, count the number of words left on
  // the line
  const size_t numWords = numWordsInNextLine(in);
  const size_t paramSize = 1 + genSum.bestParameters.parameters.size();
  if(numWords == paramSize) {
    // didn't output the range values
    genSum.fRange = NaN;
    genSum.maxLogRange = NaN;
    genSum.maxUniformRange = NaN;
  }
  else if(numWords == paramSize + 3) {
    // output the range values
    genSum.fRange = getDouble(in);
    genSum.maxLogRange = getDouble(in);
    genSum.maxUniformRange = getDouble(in);
  }
  else {
    // line is corrupted
    throw ParameterPopulationException("Invalid parameter history line");
  }

  // get the best parameters for this generation
  in >> genSum.bestParameters;

  return in;
}

///////////////// ParameterPopulation public member functions /////////////////

ParameterPopulation::ParameterPopulation(void)
{
  // initializing public values, these can be changed as convenient:
  logToDisk = true;
  logToTerminal = true;
  resumeFileName = "resume.txt";
  resultsFileName = "results.txt";

  tol = 1.0e-3;
  fTol = 1.0e-4;
  maxGenerationNum = 10000;
  maxDiffScale = 0.5;

  // initializing protected / public values, don't edit these:
  generationNum = 1;
  numEvaluations = 0;
  numGenEvaluations = 0;
  generationSize = 0;
  bestParameters.value = Inf;

  initialized = false;
  clean = false;
  resumed = false;
  optimal = false;
}

ParameterPopulation::~ParameterPopulation(void)
{
  cleanup();
}

void ParameterPopulation::geneticAlgorithm(void)
{
  // optimize population with genetic algorithm

  // start the overall timer
  totalTime.start();
  // initialize (load in parameter descriptions, derived-class data, etc)
  initialize();

  // this can't work with population size < 3
  if(populationSize < 3)
    throw ParameterPopulationException("Population size must be >= 3");

  if(generationSize == 0)
    generationSize = populationSize;

 maxGenerationNum = 1000 * parameterDescriptions.numFitParameters();

  try {
    // try to resume
    resume();
    if(resumed) {
      // resume a previous session

      if(numGenEvaluations >= populationSize) {
        // stopped after previous generation was complete, so start a new
        // generation
        numGenEvaluations = 0;
        if(generationNum == generationHistory.size())
          ++generationNum;
        // start generation timer
        generationTime.start();
      }
      // don't need to start generationTime generation is in progress so,
      // it's already started
    }
    else {
      // start a new session

      optimal = false;
      numEvaluations = 0;
      numGenEvaluations = 0;
      // start generation timer
      generationTime.start();
    }

    // make sure we have the parameter descriptions
    if(parameterDescriptions.size() == 0)
      throw ParameterPopulationException("No parameter descriptions loaded");

    // make sure the population size has been set
    if(populationSize == 0)
      throw ParameterPopulationException("Population size not set");

    // evaluate the first generation of THIS session
    evaluateGeneration();

    // continue until optimal (or interrupted)
    while(!optimal) {
      // start the timer for this generation
      generationTime.start();
      ++generationNum;

      // evaluate the population, indicate progress
      evaluateGeneration();
    }
  }
  catch(exception & e) {
    // best to quit without saving, because the state may be messed up
    // (also if signal is caught during evaluateGeneration(), progress will be
    // saved)
    cout << e.what() << '\n';
    cout.flush();
    // try to clean up before exit
    cleanup();
    // rethrow the exception
    exit(0);
    throw;
  }
}

const size_t &
ParameterPopulation::getGenerationNum(void) const
{
  return generationNum;
}

const size_t &
ParameterPopulation::getPopulationSize(void) const
{
  return populationSize;
}

const size_t &
ParameterPopulation::getNumEvaluations(void) const
{
  return numEvaluations;
}

const size_t &
ParameterPopulation::getNumGenEvaluations(void) const
{
  return numGenEvaluations;
}

const ParameterSet &
ParameterPopulation::getBestParameters(void) const
{
  return bestParameters;
}

void
ParameterPopulation::setParameterDescriptions(
                            const ParameterDescriptionList & paramDescriptions)
{
  // copy in the new list of parameter descriptions
  parameterDescriptions = paramDescriptions;

  // resize bestParameters
  bestParameters.parameters.resize(parameterDescriptions.size());
}

void
ParameterPopulation::setPopulationSize(const size_t & newPopulationSize)
{
  // set maximum population size, and reserve memory
  populationSize = newPopulationSize;

  if(population.size() <= populationSize)
    population.reserve(populationSize);
  else
    population.resize(populationSize);
}

//////////////////// ParameterPopulation friend functions /////////////////////

ostream &
operator<<(ostream & out, const ParameterPopulation & paramPop)
{
  // write out the list of parameter descriptions
  out << paramPop.parameterDescriptions;

  // write out the best parameters
  out << "# Current best parameters:\n";
  out << paramPop.bestParameters << '\n';

  // write out the timers
  out << "# Total time:\n";
  out << paramPop.totalTime << '\n';
  out << "# Current generation time:\n";
  out << paramPop.generationTime << '\n' << '\n';

  // write out the number of evaluations
  out << paramPop.numEvaluations << " # number of parameter sets evaluated\n";
  // write out the number of evaluations in this generation
  out << paramPop.numGenEvaluations
      << " # number of evaluations this generation\n";
  // write out the generation number
  out << paramPop.generationNum << " # current generation number\n\n";

  // write out the size of the generation history
  out << paramPop.generationHistory.size() << " # generation history\n";

  // write out the history of generation summaries
  for(const auto & genHist : paramPop.generationHistory)
    out << genHist;
  out << '\n';

  // write out the current population size
  out << paramPop.population.size() << " # population\n";

  // write out the members of the population
  for(const auto & pSet : paramPop.population)
    out << pSet;

  return out;
}

istream &
operator>>(istream & in, ParameterPopulation & paramPop)
{
  // read in the list of parameter descriptions
  ParameterDescriptionList resumeDescriptions;
  in >> resumeDescriptions;
  if(paramPop.parameterDescriptions.empty())
    // no existing parameter descriptions, so set them to the resume
    // descriptions
    paramPop.setParameterDescriptions(resumeDescriptions);
  if( compatible(resumeDescriptions, paramPop.parameterDescriptions) )
    // have parameter descriptions read in from start up that are compatible
    // with the resume descriptions. The resume descriptions are a refinement
    // due to fitting, so use them
    paramPop.parameterDescriptions = resumeDescriptions;
  else
    // inconsistent parameter descriptions
    throw ParameterPopulationException("Parameter descriptions in resume file "
                                       "are inconsistent with start up");

  // read in the best parameters
  jumpPastComment(in);
  in >> paramPop.bestParameters;

  // read in the timers
  jumpPastComment(in);
  in >> paramPop.totalTime;
  jumpPastComment(in);
  in >> paramPop.generationTime;

  // read in the number of evaluations
  paramPop.numEvaluations = getSize(in);
  jumpPastComment(in);
  // read in the number of evaluations this generation
  paramPop.numGenEvaluations = getSize(in);
  jumpPastComment(in);
  // read in the generation number
  paramPop.generationNum = getSize(in);

  // read in the size of the the generation history
  jumpPastComment(in);
  size_t len = getSize(in);
  paramPop.generationHistory.resize(len);

  // read in the history of best parameters
  jumpPastComment(in);
  const size_t numParams = paramPop.bestParameters.parameters.size();
  for(auto & genHist : paramPop.generationHistory) {
    genHist.bestParameters.parameters.resize(numParams);
    in >> genHist;
  }

  // read in the population size
  len = getSize(in);
  if(paramPop.populationSize == 0)
    // population not already resized
    paramPop.setPopulationSize(len);

  const size_t numRead = std::min(len, paramPop.populationSize);

  paramPop.population.resize(numRead);

  // read in the members of the population
  jumpPastComment(in);
  for(auto & pSet : paramPop.population) {
    pSet.parameters.resize(numParams);
    in >> pSet;
  }

  return in;
}

/////////////// ParameterPopulation protected member functions ////////////////

void
ParameterPopulation::evaluateGeneration(void)
{
  // Once-per-generation evaluation function:
  //   -evaluate the parameter population
  //   -maintain bestParameters and history,
  //   -test for optimality
  //   -save progress
  //   -spit out progress indicator

  try {
    signalHandler.check();  // throw exception if signal caught

    // evaluate the population (derived-class specific function)
    evaluatePopulation();

    // sort the population to be in order from best to worst
    signalHandler.check();  // throw exception if signal caught

    // test to see if this generation has achieved optimality
    //   (set this->optimal = true)
    testOptimal();

    // save history of this generation
    saveHistory();

    // save progress
    signalHandler.check();  // throw exception if signal caught
    saveProgress();
  }
  catch(SignalException & sigExcept) {
    // signal caught, save progress before quitting
    saveProgress();
    // try to clean up
    cleanup();
    // re-throw the exception
    throw;
  }
  catch(exception & e) {
    // some other exception, just try to clean up
    cleanup();
    // re-throw the exception
    throw;
  }

  // print out progress indicator
  summarizeGeneration(generationNum);
}

void
ParameterPopulation::randomPSet(ParameterSet & pSet, RandGen & randGen) const
{
  // randomly generate a parameter set consistent with parameter descriptions

  // set value to NaN (flag that it needs to be evaluated)
  pSet.value = NaN;
  pSet.diffScale = maxDiffScale;

  // loop through different parameters within pSet, assign random values
  auto paramDescriptionItr = parameterDescriptions.cbegin();
  for(double & paramVal : pSet.parameters) {
    // assign the parameter a random value within range
    paramVal = randomParamValue(*paramDescriptionItr, randGen);
    ++paramDescriptionItr;
  }
}

void
ParameterPopulation::appendIn(ParameterSet & newPSet)
{
  // Append in a newly evaluated parameter set into the population, keeping the
  // population ordered in increasing value.
  // Increase the population size by 1.

  if(population.empty()) {
    // this is the first parameter set
    population.push_back(newPSet);
    setBestParameters(newPSet);
    return;
  }
  // Add copy of parameter set to population
  population.push_back(newPSet);

  // make iterator to newly added pSet
  auto oldPSetItr1 = population.rbegin();
  auto oldPSetItr2 = oldPSetItr1;
  // keep swapping until the parameter sets are in value order
  for(++oldPSetItr2; oldPSetItr2 != population.rend(); ++oldPSetItr2) {
    if(oldPSetItr1->value >= oldPSetItr2->value)
      break;
    // swap parameter sets
    oldPSetItr2->swap(*oldPSetItr1);
    ++oldPSetItr1;
  }

  // Swapped all the way to the front, the new set is the best ParameterSet
  if(oldPSetItr2 == population.rend())
    setBestParameters(*oldPSetItr1);
}

void
ParameterPopulation::swapIn(ParameterSet & newPSet)
{
  // Swap in a newly evaluated parameter set into the population, keeping the
  // population ordered in increasing value.
  // Kicking out the worst parameterSet, keeping the population size constant.

  // Form iterator to worst existing member of the population
  auto oldPSetItr1 = population.rbegin();
  if(newPSet.value >= oldPSetItr1->value)
    // newPSet isn't good enough to join the population
    return;

  // swap the worst parameter set out, and the new one in
  std::swap(newPSet, *oldPSetItr1);
  //newPSet.swap(*oldPSetItr1);

  // keep swapping until the parameter sets are in value order
  auto oldPSetItr2 = oldPSetItr1;
  for(++oldPSetItr2; oldPSetItr2 != population.rend(); ++oldPSetItr2) {
    if(oldPSetItr1->value >= oldPSetItr2->value)
      break;
    // swap parameter sets
    std::swap(*oldPSetItr2, *oldPSetItr1);
    //oldPSetItr2->swap(*oldPSetItr1);
    ++oldPSetItr1;
  }

  // Swapped all the way to the front, the new set is the best ParameterSet
  if(oldPSetItr2 == population.rend())
    setBestParameters(*oldPSetItr1);
}

void
ParameterPopulation::differentialCross(ParameterSet & pSet, RandGen & randGen)
  const
{
  // produce a new member of the population by using a differential crossover
  //   -three parents chosen from breeding population
  //   -parent1 more fit than parent2
  //      -an improvement direction vector obtained from parent1 - parent2
  //      -this vector is scaled by a random uniform number "diffScale"
  //   -parent3 is a starting location updated by improvement vector
  //      -for uniform parameters update as:
  //         child = parent3 + diffScale * (parent1 - parent2)
  //      -for logarithmic parameters update as:
  //         child = parent3 * pow(parent1 / parent2, diffScale)

  // assign the new pSet an undefined value
  pSet.value = NaN;

  // get the three parents from the breeding population (weighted
  // preferentially to better fitness), chosen so that ind1 < ind2 (swap if
  // necessary), implying that the fitness of parent1 is better than the
  // fitness of parent2
  //   -pick ind2 using cutoff-1 and ind3 using cutoff-2 because we're
  //    excluding the possibility of any of the indices matching

  // Randomly draw indices to three members of the population
  size_t ind1, ind2, ind3;
  get3ParentInds(ind1, ind2, ind3, randGen);

  // get progenitors from the population, via the indices
  const ParameterSet & parent1 = population[ind1];
  const ParameterSet & parent2 = population[ind2];
  const ParameterSet & parent3 = population[ind3];

  // throw a uniform random number to scale the differential cross
  static const double minDiffScale = 0.01;
  /*
  static const double maxDiffScale2 = log(5.0) /
    log((double)parameterDescriptions.numFitParameters() + 1.0);
  */
  //static const double maxDiffScale2 = 12.0 /
  //  ((double)parameterDescriptions.numFitRegularParameters() + 2.0);
  static const double maxDiffScale2 = 0.5;

  const size_t compInd = ind3 == 0 ? 1 :
    (size_t) round( 0.1 * (double) ind3 );
  const double & compVal = population[compInd].value;
  const double valueFact =
    abs( (parent3.value - compVal) / (parent2.value - parent1.value) );
  const double diffScale = std::min(std::max(
    pow( 2.0, -1.0 + 2.0 * randGen.openUniform() ) * valueFact,
    minDiffScale
  ), maxDiffScale2);

  //static const double diffRange = maxDiffScale2 - minDiffScale;
  //const double diffScale = minDiffScale + diffRange * randGen.openUniform();

  pSet.diffScale = diffScale;

  // loop over each parameter, computing new value via differential crossover
  auto p1Itr = parent1.parameters.cbegin();
  auto p2Itr = parent2.parameters.cbegin();
  auto p3Itr = parent3.parameters.cbegin();
  auto paramDescriptionItr = parameterDescriptions.cbegin();
  for(auto & newParamVal : pSet.parameters) {
    switch(paramDescriptionItr->type()) {
      case ParameterDescription::Constant:
        // this parameter is constant, set to only allowed value
        newParamVal = paramDescriptionItr->maxAllowed;
        break;

      case ParameterDescription::LogDistributed:
        // use logarithmic differential crossover
        newParamVal = *p3Itr * pow(*p1Itr / *p2Itr, diffScale);

        // ensure newParamVal is in bounds
        if(newParamVal < paramDescriptionItr->minAllowed)
          newParamVal = paramDescriptionItr->minAllowed;
        else if(!(newParamVal <= paramDescriptionItr->maxAllowed))
          newParamVal = paramDescriptionItr->maxAllowed;
        break;

      case ParameterDescription::UniformDistributed:
        // use linear differential crossover
        newParamVal = *p3Itr + diffScale * (*p1Itr - *p2Itr);

        // ensure newParamVal is in bounds
        if(newParamVal < paramDescriptionItr->minAllowed)
          newParamVal = paramDescriptionItr->minAllowed;
        else if(!(newParamVal <= paramDescriptionItr->maxAllowed))
          newParamVal = paramDescriptionItr->maxAllowed;
        break;

      case ParameterDescription::Invalid:
      default:
        throw ParameterPopulationException("Invalid parameter \"" +
                                           paramDescriptionItr->name + "\"");
    }

    ++p1Itr;
    ++p2Itr;
    ++p3Itr;
    ++paramDescriptionItr;
  }
}

#ifdef USE_EXPERIMENTAL_CROSS
#include <Eigen/Eigenvalues>
using namespace Eigen;
void
ParameterPopulation::experimentalCross(ParameterSet & pSet, RandGen & randGen)
  const
{
  // try to implement stochastic quadratic optimizer

  const size_t numFitParameters = parameterDescriptions.numFitParameters();
  // How many parameter sets needed to exactly specify quadratic fit?
  // numQuad = numSecondDerivs + numFirstDerivs + 1
  //         = numHessian + numFitParameters + 1
  //         = numFitP * (NumFitP + 1) / 2 + numFitP + 1
  const size_t numQuad = (numFitParameters + 1) * (numFitParameters + 2) / 2;
  const size_t numExtra = 2; // max(2, 0.1 * numFitParameters)
  const size_t numParents = numQuad + numExtra;
  //const size_t numParents = min(numQuad + numExtra, (size_t)10);

  // Form list of numParents randomly-chosen parameter indices
  // redo this to use set?
  vector<size_t> parentInds (numParents);
  getNParentInds(parentInds, numParents, randGen);

  // 1. Find the best parent, then
  // 2. Fill out a matrix of parameter differences between the remaining parents
  //    and the best parent
  // 3. Fill out a vector of value differences between the remaining parents and
  //    the best parent
  // quadraticMinDataMatric = paramVecs - bestParamVec
  // quadraticMinDataVector = paramVals - bestParamVal
  quadraticMinDataMatrix.resize(numParents-1, numFitParameters);
  quadraticMinDataVector.resize(numParents-1);
  fillQuadraticMinData(parentInds);

  // Fill out a series of equations describing a quadratic fit to the
  //   quadraticMinData
  // Form a (numParents-1) x (numQuad-1) matrix (quadraticFitMatrix) where:
  //   -each row corresponds to a parameter difference from quadraticMinData
  //   -the first numFitParams columns are the data in quadraticMinDataMatrix
  //   -the next numHessian columns are quadratic combinations of this data
  //      e.g. minDataMat(column1) * minDataMat(column7)
  // Then quadraticFitMatrix * quadraticCombinedCoefs = quadraticMinDataVector
  //   is a forumula specifying coefficients for a quadratic fit to the parents
  quadraticFitMatrix.resize(numParents-1, numQuad-1);
  fillQuadraticFitMatrix();

  // Solve those equations to compute the coefficients of the quadratic fit
  // 1. Solve for quadraticCombinedCoefs:
  //    quadraticFitMatrix * quadraticCombinedCoefs = quadraticMinDataVector
  // 2. Break quadratic combined coefs into a matrix, so that for an arbitrary
  //    deviation from the bestParent dP, the change in value dV is:
  //    dV = dP' * quadCoefsMat * dP - 2 * quadCoefsVec * dP
  quadraticCombinedCoefs.resize(numQuad-1);
  quadraticCoefficientsMatrix.resize(numFitParameters, numFitParameters);
  quadraticCoefficientsVector.resize(numFitParameters);
  computeQuadraticFitCoefficients();

  // find reglarized "minimum" from quadraticFitCoefficients
  // Since dV = dP' * quadCoefsMat * dP - 2 * quadCoefsVec * dP,
  //  the unregularized extremum/saddlepoint is found by taking dV/dP = 0:
  //  minDP = quadCoefsMat^-1 * quadCoefsVec
  // Function is more complex because we need to regularize and guarantee
  // "minimum" (or at least decrease in value)
  quadExtremum.resize(numFitParameters);
  solveQuadraticFit();

  // quadExtremum now holds information about how to alter best parent pSet
  //   to get to minimum. Fill out a new pSet from that info
  fillPSetFromQuadraticFit(pSet, parentInds[0]);
}

inline void
ParameterPopulation::fillQuadraticMinData(const vector<size_t> & parentInds)
  const
{
  // Fill out a matrix of differences between randomly chosen parameters and
  //   the best pSet among them

  // the best (lowest value) parentInd is at the beginning of the vector
  const size_t minInd = parentInds[0];

  // 2. Fill out a matrix of parameter differences between the remaining parents
  //    and the best parent
  // 3. Fill out a vector of value differences between the remaining parents and
  //    the best parent
  // quadraticMinDataMatric = paramVecs - bestParamVec
  // quadraticMinDataVector = paramVals - bestParamVal
  const auto & minPSet = population[minInd];
  size_t row = 0;
  for(const size_t & ind : parentInds) {
    if(ind == minInd) {
      continue;
    }
    const auto & diffPSet = population[ind];
    size_t col = 0;
    for(size_t paramNum = 0; paramNum < minPSet.parameters.size(); ++paramNum){
      if(parameterDescriptions[paramNum].isConstant()) {
        continue;
      }
      else if(parameterDescriptions[paramNum].isLogDistributed()) {
        quadraticMinDataMatrix(row,col) =
          log(diffPSet.parameters[paramNum] / minPSet.parameters[paramNum]);
      }
      else {
        quadraticMinDataMatrix(row,col) =
          diffPSet.parameters[paramNum] - minPSet.parameters[paramNum];
      }
      ++col;
    }
    quadraticMinDataVector(row) = diffPSet.value - minPSet.value;
    ++row;
  }
}

inline void
ParameterPopulation::fillQuadraticFitMatrix(void) const
{
  // Fill out a series of equations describing a quadratic fit to the
  //   quadraticMinData

  // Form a (numParents-1) x (numQuad-1) matrix (quadraticFitMatrix) where:
  //   -each row corresponds to a parameter difference from quadraticMinData
  //   -the first numFitParams columns are the data in quadraticMinDataMatrix
  //   -the next numHessian columns are quadratic combinations of this data
  //      e.g. minDataMat(column1) * minDataMat(column7)
  // Then quadraticFitMatrix * quadCoefs = quadraticMinDataVector is a forumula
  //   specifying coefficients for a quadratic fit to the parents
  const size_t numFitParameters = parameterDescriptions.numFitParameters();
  for(size_t row = 0; row < (size_t)quadraticFitMatrix.rows(); ++row) {
    size_t col = 0;
    for(size_t dataCol = 0; dataCol < numFitParameters; ++dataCol, ++col) {
      quadraticFitMatrix(row, col) = quadraticMinDataMatrix(row, dataCol);
    }
    for(size_t dataCol1 = 0; dataCol1 < numFitParameters; ++dataCol1) {
      for(size_t dataCol2 = dataCol1;
          dataCol2 < numFitParameters;
          ++dataCol2, ++col) {
        quadraticFitMatrix(row, col) = quadraticMinDataMatrix(row, dataCol1)
                                     * quadraticMinDataMatrix(row, dataCol2);
      }
    }
  }
}

inline void
ParameterPopulation::computeQuadraticFitCoefficients(void) const
{
  // Solve equations to compute the coefficients of the quadratic fit
  // 1. Solve for quadraticCombinedCoefs:
  //    quadraticFitMatrix * quadraticCombinedCoefs = quadraticMinDataVector
  quadraticCombinedCoefs
    = quadraticFitMatrix.jacobiSvd(ComputeThinU | ComputeThinV).solve(quadraticMinDataVector);

  // 2. Break quadratic combined coefs into a matrix, so that for an arbitrary
  //    deviation from the bestParent dP, the change in value dV is:
  //    dV = dP' * quadCoefsMat * dP - 2 * quadCoefsVec * dP
  size_t vecInd;
  const size_t numFitParameters = parameterDescriptions.numFitParameters();
  for(vecInd = 0; vecInd < numFitParameters; ++vecInd) {
    // factor of -0.5 because we want dV ~ -2 * quadCoefsVec * dP, but
    //  dV ~ quadCombinedCoefs * dV
    quadraticCoefficientsVector[vecInd] = -0.5 * quadraticCombinedCoefs[vecInd];
  }
  for(size_t row = 0; row < numFitParameters; ++row) {
    quadraticCoefficientsMatrix(row,row) = quadraticCombinedCoefs[vecInd];
    ++vecInd;
    for(size_t col = row + 1; col < numFitParameters; ++col, ++vecInd) {
      // factor of +0.5 because dV ~ dP' * quadCoefsMat * dP, but that
      // counts each off-diagonal product twice (top and bottom triangle),
      // whereas quadraticCombinedCoefs counts each product once
      quadraticCoefficientsMatrix(row,col)
        = 0.5 * quadraticCombinedCoefs[vecInd];
      quadraticCoefficientsMatrix(col,row)
        = quadraticCoefficientsMatrix(row,col);
    }
  }
}

inline void
ParameterPopulation::solveQuadraticFit(void) const
{
  // find reglarized "minimum" from quadraticFitCoefficients
  // Since dV = dP' * quadCoefsMat * dP - 2 * quadCoefsVec * dP,
  //  the unregularized extremum/saddlepoint is found by taking dV/dP = 0:
  //  minDP = quadCoefsMat^-1 * quadCoefsVec
  // This function is more complicated because we need to regularize and
  // guarantee "minimum" (or at least decrease in value)

  // quadraticFitCoefficientsMatrix is real, symmetric, so can diagonalize:
  SelfAdjointEigenSolver<MatrixXd> es (quadraticCoefficientsMatrix);
  // now:
  // quadraticFitCoefficientsMatrix == es.eigenvectors()
  //                                 * es.eigenvalues().asDiagonal()
  //                                 * es.eigenvectors.transpose()
  // so the unregularized extremum is at:
  // ex = es.eigenvectors() * es.eigenvalues().asDiagonal().inverse()
  //      * es.eigenvectors.transpose() * quadraticFitCoefficientsVector;

  // To regularize and deal with local maxima/saddlepoints, work in the space
  // where quadraticFitCoefficientsMatrix is diagonal!
  // 1. Transform quadraticFitCoefficientsVector and quadraticMinDataMatrix into
  //    eigenspace of quadraticFitCoefficients
  quadraticCoefficientsVector
    = es.eigenvectors().transpose() * quadraticCoefficientsVector;
  quadraticMinDataMatrix = quadraticMinDataMatrix * es.eigenvectors();

  // 2. Find the smallest hyper-rectangle bounding that space evenly around
  //    the best parent. Then scale it up to allow modest expansion of space
  const double quadScaleFactor = 2.0;
  quadBounds = quadScaleFactor
    * quadraticMinDataMatrix.cwiseAbs().colwise().maxCoeff();

  // 3. Find "minimum" by dividing quadraticFitCoefficientsVector by
  //    abs(es.eigenvalues), while bounding to hyper-rectangle
  const size_t numFitParameters = parameterDescriptions.numFitParameters();
  for(size_t ind = 0; ind < numFitParameters; ++ind) {
    const double vec_n = quadraticCoefficientsVector[ind];

    if(vec_n > 0) {
      quadExtremum[ind] = min(vec_n / abs(es.eigenvalues()[ind]),
                              quadBounds[ind]);
    }
    else if(vec_n < 0) {
      quadExtremum[ind] = max(vec_n / abs(es.eigenvalues()[ind]),
                              -quadBounds[ind]);
    }
    else {
      // vec_n is exactly 0.0
      quadExtremum[ind] = 0.0;
    }
  }

  // 4. Transform solution back to original space
  quadExtremum = es.eigenvectors() * quadExtremum;
}

void
ParameterPopulation::fillPSetFromQuadraticFit(ParameterSet & pSet,
                                              const size_t minInd) const
{
  // quadExtremum now holds information about how to alter best parent pSet
  //   to get to minimum. Fill out a new pSet from that info

  // assign the new pSet an undefined value
  pSet.value = NaN;

  // modify minPSet according to the deltas in quadExtremum
  const auto & minPSet = population[minInd];

  size_t col = 0;
  auto paramDescItr = parameterDescriptions.cbegin();
  for(size_t paramNum = 0;
      paramNum < pSet.parameters.size();
      ++paramNum, ++paramDescItr) {
    double & newParamVal = pSet.parameters[paramNum];
    if(paramDescItr->isConstant()) {
      newParamVal = minPSet.parameters[paramNum];
      continue;
    }
    else if(paramDescItr->isLogDistributed()) {
      newParamVal = minPSet.parameters[paramNum] * exp(quadExtremum[col]);
    }
    else {
      newParamVal = minPSet.parameters[paramNum] + quadExtremum[col];
    }

    ++col;

    // ensure newParamVal is in bounds
    if(newParamVal < paramDescItr->minAllowed) {
      newParamVal = paramDescItr->minAllowed;
    }
    else if(!(newParamVal <= paramDescItr->maxAllowed)) {
      newParamVal = paramDescItr->maxAllowed;
    }
  }
}

#endif

void
ParameterPopulation::setBestParameters(const ParameterSet & newBestParameters)
{
  // set the best parameters and save them

  bestParameters = newBestParameters;
  saveBestParameters();
}

void
ParameterPopulation::saveBestParameters(void) const
{
  // save bestParameters to results file

  if(!initialized || !logToDisk)
    // don't bother trying if not initialized, or not logging to disk
    return;

  // these better be true
  assert(!bestParameters.parameters.empty());
  assert(!parameterDescriptions.empty());

    // open results file for writing
  ofstream out (resultsFileName.c_str());
  if(out.fail())
    throw ParameterPopulationException("Can't open " + resultsFileName +
                                       " for writing");

  try {
    // calculate a few widths to ensure all the significant info in the double
    //  is output
    const streamsize doublePrecision = numeric_limits<double>::digits10 + 1;
    const streamsize expWidth = (streamsize)ceil(
      log10((double)numeric_limits<double>::max_exponent10));
    const streamsize doubleWidth = doublePrecision + expWidth + 4;

    // set output decimal precision
    out.precision(doublePrecision);

    // get length of longest parameter description name
    ParameterDescriptionList::const_iterator descItr =
      parameterDescriptions.begin();
    streamsize nameWidth = 5;
    for(; descItr != parameterDescriptions.end(); ++descItr)
      if(descItr->name.size() > (size_t)nameWidth)
        nameWidth = descItr->name.size();
    nameWidth += 1;

    // write out the function value
    out.width(nameWidth);
    out << left << "value";
    out.width(doubleWidth);
    out << right << bestParameters.value << '\n';

    // write out the parameters
    vector<double>::const_iterator bestItr = bestParameters.parameters.begin();
    descItr = parameterDescriptions.begin();
    for(; descItr != parameterDescriptions.end(); ++descItr, ++bestItr) {
      out.width(nameWidth);
      out << left << descItr->name;
      out.width(doubleWidth);
      out << right << *bestItr << '\n';
    }

    out.close();
  }
  catch(exception & e){
    // close the file
    out.close();
    // re-throw error
    throw;
  }
}

//////////////// ParameterPopulation private member functions /////////////////

void
ParameterPopulation::saveProgress(void) const
{
  // save state of ParameterPopulation to resume file

  if(!initialized || !logToDisk)
    // don't bother trying if not initialized, or not logging to disk
    return;

  // these better be true
  assert(!population.empty());
  assert(!bestParameters.parameters.empty());
  assert(!parameterDescriptions.empty());

  // create a temporary resume file to avoid destroying old resume information
  // if shutdown occurs during saving
  const string tempResumeFileName = resumeFileName + ".temp";

  // move the old resume information to the temporary file
  rename(resumeFileName.c_str(), tempResumeFileName.c_str());

  // open resume file for writing
  ofstream fileOut (resumeFileName.c_str());
  if(fileOut.fail())
    throw ParameterPopulationException("Can't open " + resumeFileName +
                                       " for writing");
  try {
    // write out *this
    fileOut << *this;

    // derived-class specific save routine
    saveSpecific(fileOut);

    // close resume file
    fileOut.close();
  }
  catch(exception & e) {
    // close the file
    fileOut.close();

    // remove the resume file
    remove(resumeFileName.c_str());

    // copy the old version back
    rename(tempResumeFileName.c_str(), resumeFileName.c_str());

    // re-throw error
    throw;
  }

  // remove temporary file
  remove(tempResumeFileName.c_str());

  if(logToTerminal) {
    cout << "Progress saved to " << resumeFileName << '\n';
    cout.flush();
  }
}

void
ParameterPopulation::resume(void)
{
  // check to see if program was killed during saveProgress()
  // (can happen with kill -9, or pulled plug)
  const string tempResumeFileName = resumeFileName + ".temp";
  if(fileExists(tempResumeFileName)) {
    if(logToTerminal) {
      cout << "Cleaning up corrupted resume file...";
      cout.flush();
    }

    // remove the (presumably corrupt) permanent resume file
    remove(resumeFileName.c_str());
    // restore temporary resume file
    rename(tempResumeFileName.c_str(), resumeFileName.c_str());

    if(logToTerminal) {
      cout << "done.\n";
      cout.flush();
    }
  }

  ifstream fileIn (resumeFileName.c_str());
  if(fileIn.fail()) {
    // if resume file can't be opened, don't resume
    resumed = false;
    return;
  }

  if(logToTerminal) {
    cout << "Resuming previous session from " << resumeFileName << " ... ";
    cout.flush();
  }

  try {
    // read in *this
    fileIn >> *this;

    // derived-class specific resume routine
    resumeSpecific(fileIn);

    // close resume file
    fileIn.close();
  }
  catch(exception & e) {
    // close resume file
    fileIn.close();
    // re-throw error
    throw;
  }

  if(logToTerminal) {
    cout << "success.\n";
    cout.flush();
  }
  resumed = true;

  // test if population is optimal
  testOptimal();

  // re-display the progress from the previous session(s)
  for(size_t genNum = 1; genNum <= generationHistory.size(); ++genNum)
    summarizeGeneration(genNum);
  return;
}

void
ParameterPopulation::testOptimal(void)
{
  // Max current range of log-distributed params
  maxLogRange = 1.0;
  // Max current range of uniform-distributed params
  maxUniformRange = 0.0;

  // initialize the min and max range of each parameter description
  auto paramItr = population.front().parameters.cbegin();
  for(auto & paramDescription : parameterDescriptions) {
    if(paramDescription.isConstant()) {
      ++paramItr;
      continue;
    }

    // initialize this parameter description's min/max to the best parameter
    // set's value
    paramDescription.minRange = *paramItr;
    paramDescription.maxRange = paramDescription.minRange;

    ++paramItr;
  }

  // keep track if the population has been fully evaluted
  bool populationEvaluated = (population.size() >= populationSize);

  maxDiffScale = 0.0;
  // loop over parameter sets
  for(const auto & pSet : population) {
    // for each parameter set:
    //   1. check if it's been evaluated
    //   2. update ranges of each parameter description

    if(isNaN(pSet.value))
      populationEvaluated = false;
    else {
      if(pSet.diffScale > maxDiffScale) {
        maxDiffScale = pSet.diffScale;
      }
    }

    paramItr = pSet.parameters.cbegin();
    for(auto & paramDescription : parameterDescriptions) {
      if(!paramDescription.isConstant())
        paramDescription.updateRange(*paramItr);
      ++paramItr;
    }
  }

  // update the overall maxLogRange and maxUniformRange
  for(auto & paramDescription : parameterDescriptions) {
    if(paramDescription.isLogDistributed()) {
      // parameter is log-distributed, update maxLogRange
      if(paramDescription.range() > maxLogRange)
        maxLogRange = paramDescription.range();
    }
    else {
      // parameter is uniform-distributed, update maxUniformRange
      if(paramDescription.range() > maxUniformRange)
        maxUniformRange = paramDescription.range();
    }
  }

  // range of values of competetive parameter sets
  fRange = population.back().value - population.front().value;

  // This is the test to see if the function is optimized

  if(parameterDescriptions.numFitParameters() == 0) {
    optimal = true;
  }
  else if(populationEvaluated && fRange < fTol && maxUniformRange < tol &&
           (maxLogRange - 1.0) < tol) {
    optimal = true;
  }
  else if(generationNum > maxGenerationNum) {
    optimal = true;
  }
  else {
    optimal = false; // provisional, may change in a bit
    /*
    const size_t lookBack = 1000;
    if(generationNum > lookBack) {
      const double smallTol = fTol * 0.1;

      const double deltaValue =
        generationHistory[generationNum - lookBack].bestParameters.value -
        bestParameters.value;

      if(generationNum > lookBack && fRange < smallTol && deltaValue < smallTol){
          optimal = true;
      }
    }
    */
  }
}

void
ParameterPopulation::saveHistory(void)
{
  // save current generation's history
  // NOTE: this function may be called more than once per generation, so can't
  // simply call generationHistory.push_back(...)

  // resize generationHistory
  generationHistory.resize(generationNum,
    GenerationSummary(bestParameters, generationTime, totalTime,
                      fRange, maxLogRange, maxUniformRange) );
}

void
ParameterPopulation::summarizeGeneration(const size_t & genNum) const
{
  // print a summary of the requested generation
  assert(genNum > 0);

  // get a pointer to the requested generation summary
  GenerationSummary const* const histPtr = &generationHistory[genNum - 1];

  // print out a the details of the summary
  if(logToTerminal) {
    cout << "####### Generation " << genNum << " #######\n";
    cout << "Best value = " << histPtr->bestParameters.value << '\n';
    cout << "Evaluation time:  " << histPtr->generationTime << '\n';
    cout << "Total elapsed time:  " << histPtr->totalTime << '\n';
    if(optimal && genNum == generationNum)
      cout << "Finished optimizing.\n";
    cout.flush();
  }
}

inline void
ParameterPopulation::get3ParentInds(size_t & ind1, size_t & ind2,
                                    size_t & ind3, RandGen & randGen) const
{
  // Randomly draw indices to three members of the population

  const size_t popSize = population.size();
  ind1 = getParent(popSize, randGen);
  ind2 = getParent(popSize - 1, randGen);
  ind3 = getParent(popSize - 2, randGen);
  if(ind2 >= ind1) {
    ++ind2;
    if(ind3 >= ind1)
      ++ind3;
    if(ind3 >= ind2)
      ++ind3;
  }
  else {
    if(ind3 >= ind2)
      ++ind3;
    if(ind3 >= ind1)
      ++ind3;
  }
}

inline void
ParameterPopulation::getNParentInds(vector<size_t> & parentInds,
                                    const size_t & numParents,
                                    RandGen & randGen) const
{
  // Randomly draw indices to numParents members of the population.
  // Ensure that the first parentInd is the best one

  // create list of all possible parents
  static vector<size_t> allInds;  // change to not be vector?
  allInds.resize(population.size());
  size_t n = 0;
  for(size_t & ind : allInds) {
    ind = n;
    ++n;
  }

  // resize the vector of indices appropriately
  parentInds.resize(numParents);

  for(size_t & parentInd : parentInds) {
    // randomly select a valid member of the population
    const size_t randomInd = randGen.unsignedInt(allInds.size());
    auto allItr = allInds.begin() + randomInd;
    // set the current parent to the selected member
    parentInd = *allItr;
    // If it's better (lower index) than the first parentInd, swap them
    if(parentInd < parentInds[0]) {
      swap(parentInd, parentInds[0]);
    }
    // remove this population member from further consideration
    allInds.erase(allItr);
  }
}

inline size_t
ParameterPopulation::getParent(const size_t & cutoff, RandGen & randGen) const
{
  // return a parent index from the breeding population [0, cutoff - 1]

  return (size_t)randGen.unsignedInt(cutoff);
}


#include <boost/hana/fwd/partition.hpp>


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOGICAL_NOT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOGICAL_NOT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-boolean
    This function object returns the return the logical negation of the argument.

    Infix notation can be used with operator '!'

    @par Header <boost/simd/function/logical_not.hpp>

    @see logical_notor,  logical_xor, logical_andnot,  logical_notand,  logical_ornot, logical_and,  logical_or

    @par Example:

      @snippet logical_not.cpp logical_not

    @par Possible output:

      @snippet logical_not.txt logical_not

  **/
  as_logical_t<Value0> logical_not(Value0 const& x, Value1 const& y);
} }
#endif

#include <boost/simd/function/scalar/logical_not.hpp>
#include <boost/simd/function/simd/logical_not.hpp>

#endif


/********************************************************************
	created:	2012/11/16
	created:	16:11:2012   16:59
	filename: 	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\test\EnvelopFinderTest.cpp
	file path:	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\test
	file base:	SimpleFinderTest
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/

#include "GAGPL/SPECTRUM/SimpleFinder.h"
#include "GAGPL/SPECTRUM/PeakList.h"

#include <iostream>
#include <sstream>
#include <fstream>
#include <utility>
#include <boost/algorithm/string.hpp>
#include <stdlib.h>

using namespace gag;
using namespace std;
using namespace param;

RichList readSpectrum(const std::string& filename)
{
	std::cout << "Load spectrum..." << std::endl;

	std::ifstream infile(filename.c_str());

	std::string line;

	RichList spec;
	if(infile.is_open())
	{
		while(std::getline(infile, line))
		{
			std::istringstream is;
			is.str(line);
			double k1, k2, k3, k4;
			// The four columns are: m/z, intensity, resolution, s/n
			is >> k1 >> k2 >> k3 >> k4;
			//gag::Peak pk(k1, k2);
			RichPeakPtr pk = createRichPeak(k1, k2, k3, k4);
			spec.addPeak(pk);
		}
	}
	infile.close();
	infile.clear();
	return spec;

}

// argv[1] -- filename without suffix.
// argv[2] -- precursor m/z
// argv[3] -- precursor charge

int main(int argc, char *argv[])
{
	// Test 1
	//cout << "Test 1: Manual interpretation" << endl;

	//EnvelopFinder env_finder1(spec1);

	//std::vector<EnvelopPtr> env_pool1 = env_finder1.getEnvelops();
	//BOOST_FOREACH(EnvelopPtr env, env_pool1)
	//{
	//	env_finder1.printEnvelop(env);
	//}
	// Test 2
	//cout << endl;
	try {
		cout << "Test 2: Real spectrum" << endl;
		// 1. TBD: Read the data and examine the format of the data.

		std::string filename = "./data/spectrum/";
		filename.append(argv[1]);
		filename.append(".txt");

		RichList spectrum = readSpectrum(filename);

		Param& param = Param::Instance();
		param.setParameter<double>("precursor_mz", atof(argv[2]));
		param.setParameter<int>("precursor_charge", atoi(argv[3]));

		// Threshold for most abundant peak in an envelop (mono peak in current case.
		//param.setParameter<double>("signal_noise", 15.0);
		param.setParameter<std::string>("instrument_type", "ft");

		// threshold for observed peak.
		param.setParameter<double>("lower_bound_sn",5.0);
		param.setParameter<double>("learning_rate", 0.01);
		param.setParameter<double>("max_intensity_shift", 0.3);
		param.setParameter<int>("max_sulfur_num",6);
		param.setParameter<int>("num_missing_peaks",1);

		SimpleFinder env_finder2(spectrum);

		// 2. print the identified envelops.
		std::multimap<RichPeakPtr, EnvelopPtr> env_pool = env_finder2.getEnvelops();

		cout << "Output the results into file." << endl;

		std::string path = "./data/output/";
		path.append(argv[1]);
		path.append("_mono_list.txt");

		ofstream outfile(path.c_str());

		if(outfile.is_open()) {
			outfile << "MZ\tIntensity\tCharge\n";
			outfile.precision(5);
			cout.precision(5);

			for(std::multimap<RichPeakPtr, EnvelopPtr>::iterator iter = env_pool.begin(); iter != env_pool.end(); iter++)
			{
				env_finder2.printEnvelop(iter->second);

				outfile << fixed << iter->first->mz << "\t" << iter->first->intensity << "\t" << iter->second->charge_state << "\n";
			}

			outfile.close();
		} else {
			std::cout << "Unable to open file!\n" << std::endl;
		}

		std::string und_pks = "./data/output/";
		und_pks.append(argv[1]);
		und_pks.append("_undetermined_list.txt");
		ofstream outfile2(und_pks.c_str());

		if(outfile2.is_open()) {
			outfile2 << "MZ\tIntensity\tCharge\n";
			outfile2.precision(5);

			std::multimap<RichPeakPtr, int>& pks = env_finder2.getUndeterminedPeaks();
			for(std::multimap<RichPeakPtr, int>::iterator iter = pks.begin(); iter!= pks.end(); iter++)
			{
				if(iter->first->pk_type == "NOISE")
					continue;
				outfile2 << fixed << iter->first->mz << "\t" << iter->first->intensity << "\t" << iter->second << "\n";
			}
			outfile2.close();

		} else {
			std::cout << "Unable to open file " << und_pks << std::endl;
		}
	} catch(std::exception& e) {
		cout << "Exception: " << e.what() << endl;
	}

	return 0;

}


#include <df/optimization/deformationGraphRegularization.h>

#include <vector>
#include <Eigen/Core>

#include <df/util/dualQuaternion.h> // TODO
#include <df/util/typeList.h>

namespace df {

template <typename Scalar, typename ScalarOpt, int Options, template <typename,int...> class TransformT>
inline void computeEdgeResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar,Options> & transformA,
                                const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar,Options> & transformB,
                                Eigen::Matrix<ScalarOpt,3,1,Eigen::DontAlign> & residual,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexADisplacementByTransformA,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexADisplacementByTransformB,
                                Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexARelativeToVertexB) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    vertexADisplacementByTransformA = transformA.translation();

    vertexARelativeToVertexB = vertexA - vertexB;

    vertexADisplacementByTransformB = transformB * vertexARelativeToVertexB;

    const Vec3 vertexAWarpedByTransformA = vertexA + vertexADisplacementByTransformA;

    const Vec3 vertexAWarpedByTransformB = vertexB + vertexADisplacementByTransformB;

    residual = (vertexAWarpedByTransformA - vertexAWarpedByTransformB).template cast<ScalarOpt>();

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
inline void fillEdgeJacobianAndResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar> & transformA,
                                        const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar> & transformB,
                                        internal::JacobianAndResidual<ScalarOpt,3,12> & jacobianAndResidual,
                                        const IntToType<internal::TransformUpdateLeftMultiply> ) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    Vec3 vertexADisplacementByTransformA, vertexADisplacementByTransformB, vertexARelativeToVertexB;

    computeEdgeResidual(vertexA,transformA,vertexB,transformB,jacobianAndResidual.r,
                        vertexADisplacementByTransformA, vertexADisplacementByTransformB,
                        vertexARelativeToVertexB);

    // the first block is for vertex A
    jacobianAndResidual.J.template block<3,6>(0,0) << 1, 0, 0,  0, vertexADisplacementByTransformA(2), -vertexADisplacementByTransformA(1),
                                                      0, 1, 0,  -vertexADisplacementByTransformA(2), 0, vertexADisplacementByTransformA(0),
                                                      0, 0, 1,  vertexADisplacementByTransformA(1), -vertexADisplacementByTransformA(0), 0;

    // the second block is for vertex B
    jacobianAndResidual.J.template block<3,6>(0,6) << -1,  0,  0,  0, -vertexADisplacementByTransformB(2), vertexADisplacementByTransformB(1),
                                                       0, -1,  0,  vertexADisplacementByTransformB(2), 0, -vertexADisplacementByTransformB(0),
                                                       0,  0, -1,  -vertexADisplacementByTransformB(1), vertexADisplacementByTransformB(0), 0;

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT>
inline void fillEdgeJacobianAndResidual(const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexA, const TransformT<Scalar> & transformA,
                                        const Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> & vertexB, const TransformT<Scalar> & transformB,
                                        internal::JacobianAndResidual<ScalarOpt,3,12> & jacobianAndResidual,
                                        const IntToType<internal::TransformUpdateRightMultiply> ) {

    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;
    typedef Eigen::Matrix<ScalarOpt,3,3,Eigen::DontAlign> Mat3;

    Vec3 vertexADisplacementByTransformA, vertexADisplacementByTransformB, vertexARelativeToVertexB;

    computeEdgeResidual(vertexA,transformA,vertexB,transformB,jacobianAndResidual.r,
                        vertexADisplacementByTransformA, vertexADisplacementByTransformB,
                        vertexARelativeToVertexB);

    // the first block is for vertex A
    jacobianAndResidual.J.template block<3,3>(0,0) = transformA.rotationMatrix().template cast<ScalarOpt>();
    jacobianAndResidual.J.template block<3,3>(0,3) = Mat3::Zero();

    // the second block is for vertex B
    jacobianAndResidual.J.template block<3,3>(0,6) = -transformB.rotationMatrix().template cast<ScalarOpt>();

    Mat3 partial;
    partial << 0, vertexARelativeToVertexB(2), -vertexARelativeToVertexB(1),
               -vertexARelativeToVertexB(2), 0, vertexARelativeToVertexB(0),
               vertexARelativeToVertexB(1), -vertexARelativeToVertexB(0), 0;

    jacobianAndResidual.J.template block<3,3>(0,9) = -transformB.rotationMatrix().template cast<ScalarOpt>()*partial;

}

template <typename Scalar, uint Rows>
struct JacobianFillColUnroller {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & globalJacobianTriplets,
                            const Eigen::Matrix<Scalar,Rows,1,Eigen::DontAlign> & localJacobianColumn,
                            const uint firstRowIndex, const uint columnIndex) {

//        std::cout << "colFill " << firstRowIndex << ", " << columnIndex << ":" << std::endl << localJacobianColumn << std::endl << std::endl;

        const Scalar & localVal = localJacobianColumn(0);

        if (localVal != Scalar(0)) {

            globalJacobianTriplets.push_back(Triplet(firstRowIndex,columnIndex,localVal));

        }

        JacobianFillColUnroller<Scalar,Rows-1>::fill(globalJacobianTriplets,
                                                     localJacobianColumn.template tail<Rows-1>(),
                                                     firstRowIndex+1,columnIndex);

    }

};

template <typename Scalar>
struct JacobianFillColUnroller<Scalar,0> {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & /*globalJacobianTriplets*/,
                            const Eigen::Matrix<Scalar,0,1,Eigen::DontAlign> & /*localJacobianColumn*/,
                            const uint /*firstRowIndex*/, const uint /*columnIndex*/) { }

};

template <typename Scalar, uint Rows, uint Cols>
struct JacobianFillUnroller {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & globalJacobianTriplets,
                            const Eigen::Matrix<Scalar,Rows,Cols,Eigen::DontAlign> & localJacobian,
                            const uint firstRowIndex, const uint firstColumnIndex) {

//        std::cout << "matFill " << firstRowIndex << ", " << firstColumnIndex << ":" << std::endl << localJacobian << std::endl << std::endl;

        JacobianFillColUnroller<Scalar,Rows>::fill(globalJacobianTriplets,
                                                   localJacobian.template block<Rows,1>(0,0),
                                                   firstRowIndex, firstColumnIndex);

        JacobianFillUnroller<Scalar,Rows,Cols-1>::fill(globalJacobianTriplets,
                                                       localJacobian.template block<Rows,Cols-1>(0,1),
                                                       firstRowIndex, firstColumnIndex + 1);

    }

};

template <typename Scalar, uint Rows>
struct JacobianFillUnroller<Scalar,Rows,0> {

    typedef Eigen::Triplet<Scalar> Triplet;

    inline static void fill(std::vector<Triplet> & /*globalJacobianTriplets*/,
                            const Eigen::Matrix<Scalar,Rows,0,Eigen::DontAlign> & /*localJacobian*/,
                            const uint /*firstRowIndex*/, const uint /*firstColumnIndex*/) { }

};

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U>
void fillRegularizationResidualAndJacobianTripletList(std::vector<Eigen::Triplet<ScalarOpt> > & globalJacobianTripletList,
                                                      Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & globalResidual,
                                                      const NonrigidTransformer<Scalar,TransformT> & transformer) {

    typedef typename NonrigidTransformer<Scalar,TransformT>::Transform Transform;
    typedef Eigen::Matrix<Scalar,3,1,Eigen::DontAlign> Vec3;

    static constexpr uint ResidualDim = 3;
    static constexpr uint BlockDim = 6;

    uint runningRowCounter = 0;

    uint lowerLevelParameterStart = 0;
    uint higherLevelParameterStart;

    for (uint level = 0; level < transformer.numRegularizationTreeLevels() - 1; ++level) {

        const uint numLowerLevelVertices = transformer.numVerticesAtLevel(level);

        higherLevelParameterStart = lowerLevelParameterStart + numLowerLevelVertices*BlockDim;

        for (uint lowerLevelIndex = 0; lowerLevelIndex < numLowerLevelVertices; ++lowerLevelIndex) {

            const Vec3 & lowerLevelVertex = transformer.deformationGraphVertices(level)[lowerLevelIndex];

            const Transform & lowerLevelTransform = transformer.transforms(level)[lowerLevelIndex];

            const uint numNeighbors = transformer.numHigherLevelNeighbors(level,lowerLevelIndex);

            for (uint k = 0; k < numNeighbors; ++k) {

                const uint higherLevelIndex = transformer.higherLevelNeighbors(level,lowerLevelIndex)[k];

                const Vec3 & higherLevelVertex = transformer.deformationGraphVertices(level+1)[higherLevelIndex];

                const Transform & higherLevelTransform = transformer.transforms(level+1)[higherLevelIndex];

                internal::JacobianAndResidual<ScalarOpt,ResidualDim,2*BlockDim> jacobianAndResidual;

                fillEdgeJacobianAndResidual(lowerLevelVertex,lowerLevelTransform,
                                            higherLevelVertex,higherLevelTransform,
                                            jacobianAndResidual,IntToType<U>());

//                std::cout << level << ", " << lowerLevelIndex << ", " << k << ": " << std::endl << jacobianAndResidual.J << std::endl << std::endl;

//                std::vector<Eigen::Triplet<ScalarOpt> > localTripletList;
//                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(localTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
//                                                                           0, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

//                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(localTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
//                                                                           0, higherLevelParameterStart + higherLevelIndex*BlockDim);
//                Eigen::SparseMatrix<ScalarOpt> localJacobian(3,transformer.numVerticesTotal()*BlockDim);
//                localJacobian.setFromTriplets(localTripletList.begin(),localTripletList.end());
//                localJacobian.finalize();
//                std::cout << localJacobian.toDense() << std::endl << std::endl;

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
                                                                           runningRowCounter, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
                                                                           runningRowCounter, higherLevelParameterStart + higherLevelIndex*BlockDim);

//                std::cout << "(" << runningRowCounter << " - " << (runningRowCounter + ResidualDim) << ") / " << globalResidual.rows() << std::endl;
                globalResidual.template block<ResidualDim,1>(runningRowCounter,0) = jacobianAndResidual.r;

                runningRowCounter += ResidualDim;

                fillEdgeJacobianAndResidual(higherLevelVertex,higherLevelTransform,
                                            lowerLevelVertex,lowerLevelTransform,
                                            jacobianAndResidual,IntToType<U>());

//                std::cout << level << ", " << lowerLevelIndex << ", " << k << ": " << std::endl << jacobianAndResidual.J << std::endl << std::endl;

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,BlockDim),
                                                                           runningRowCounter, lowerLevelParameterStart + lowerLevelIndex*BlockDim);

                JacobianFillUnroller<ScalarOpt,ResidualDim,BlockDim>::fill(globalJacobianTripletList,jacobianAndResidual.J.template block<ResidualDim,BlockDim>(0,0),
                                                                           runningRowCounter, higherLevelParameterStart + higherLevelIndex*BlockDim);

//                std::cout << "(" << runningRowCounter << " - " << (runningRowCounter + ResidualDim) << ") / " << globalResidual.rows() << std::endl;
                globalResidual.template block<ResidualDim,1>(runningRowCounter,0) = jacobianAndResidual.r;

                runningRowCounter += ResidualDim;

            }

        }

        lowerLevelParameterStart = higherLevelParameterStart;

    }

}

template <typename Scalar, typename ScalarOpt, template <typename,int...> class TransformT, internal::TransformUpdateMethod U>
void computeRegularizerNormalEquations(const NonrigidTransformer<Scalar,TransformT> & transformer,
                                       Eigen::SparseMatrix<ScalarOpt> & JTJ,
                                       Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> & JTr) {

    // TODO: come up with a nice way to compute only the upper triangle of JTJ,
    // as that is the only part used in the solver anyway

    typedef Eigen::Triplet<ScalarOpt> Triplet;
    typedef Eigen::SparseMatrix<ScalarOpt> SparseMatX;
    typedef Eigen::Matrix<ScalarOpt,Eigen::Dynamic,1> VecX;

    static constexpr uint ResidualDim = 3;
    static constexpr uint BlockDim = 6;
    // TODO TODO TODO:
    static constexpr uint NumNeighbors = 4;

    const uint numTotalDeformationGraphVertices = transformer.numVerticesTotal();

    const uint numTopLevelVertices = transformer.numVerticesAtLevel(transformer.numRegularizationTreeLevels()-1);

    const uint totalResidualRows = 2 * (numTotalDeformationGraphVertices - numTopLevelVertices) * ResidualDim * NumNeighbors;

    std::vector<Triplet> globalJacobianTriplets;
    globalJacobianTriplets.reserve(totalResidualRows*2*BlockDim);

    VecX globalResidual(totalResidualRows);

    fillRegularizationResidualAndJacobianTripletList<Scalar,ScalarOpt,TransformT,U>(globalJacobianTriplets,globalResidual,transformer);

    const uint totalModelDimension = numTotalDeformationGraphVertices * BlockDim;

    SparseMatX globalJacobian(totalResidualRows,totalModelDimension);

    globalJacobian.setFromTriplets(globalJacobianTriplets.begin(),globalJacobianTriplets.end());

    globalJacobian.finalize();

    JTJ = globalJacobian.transpose()*globalJacobian;

    JTr = globalJacobian.transpose()*globalResidual;
}

template void computeRegularizerNormalEquations<float,double,DualQuaternion,internal::TransformUpdateLeftMultiply>
                                                     (const NonrigidTransformer<float,DualQuaternion> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,DualQuaternion,internal::TransformUpdateRightMultiply>
                                                     (const NonrigidTransformer<float,DualQuaternion> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,Sophus::SE3,internal::TransformUpdateLeftMultiply>
                                                     (const NonrigidTransformer<float,Sophus::SE3> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

template void computeRegularizerNormalEquations<float,double,Sophus::SE3,internal::TransformUpdateRightMultiply>
                                                     (const NonrigidTransformer<float,Sophus::SE3> &,
                                                      Eigen::SparseMatrix<double> &,
                                                      Eigen::VectorXd &);

} // namespace df


#include "decoder.h"
#include "lm.h"
#include <cmath>
#include <cassert>
#include <vector>
#include <map>
#include <fstream>
#include <boost/unordered_map.hpp>
#include <Python.h>
#include <boost/python/extract.hpp>
#include <numpy/noprefix.h>
#include <boost/timer/timer.hpp>

// NOTE
// Current bottlenecks according to gprof are
// std::sort (should be fixed now)
// unordered_map find
// unordered_map []

double combine(double a, double b, double c)
{
    double psum = exp(a) + exp(b) + exp(c);
    if (psum < EPS)
        return NEG_INF;
    return log(psum);
}

double prefix_hyp_score(PrefixHyp::Ptr hyp)
{
    return combine(hyp->p_nb, hyp->p_b) + COMP_BETA * hyp->nw;
}

bool comp_prefix_pair(const PrefixPair& x, const PrefixPair& y)
{
    // Want descending order
    //return prefix_hyp_score(x.second) > prefix_hyp_score(y.second);
    return x.second->score > y.second->score;
}

double decode_bg_lm(double* probs, int N, int T, PrefixTree& ptree,
        int beam, double alpha, double beta, PrefixKey& best_prefix)
{
    double bg = 0.0;
    float best_score = -1;

    PrefixMap Hold;
    PrefixMap Hcurr;
    PrefixMap Hnext;

    // Start things off
    std::vector<int> empty_prefix;
    PrefixHyp::Ptr empty_hyp(new PrefixHyp);
    empty_hyp->p_b = 0.0;
    empty_hyp->node = ptree.root;
    empty_hyp->wid = ptree.lm.start;
    empty_hyp->score = prefix_hyp_score(empty_hyp);
    Hcurr[empty_prefix] = empty_hyp;

    // Loop over time
    for (int t = 0; t < T; t++) {
        Hnext.clear();
        double probs_0t = probs[t*N+0];

        // Loop over prefixes
        for (PrefixMap::iterator iter=Hcurr.begin(); iter != Hcurr.end(); ++iter) {
            PrefixKey prefix = iter->first;
            PrefixHyp::Ptr hyp = iter->second;

            PrefixHyp::Ptr valsP(new PrefixHyp);
            if (Hnext.find(prefix) != Hnext.end())
                valsP = Hnext[prefix];
            else
                Hnext[prefix] = valsP;
            valsP->p_b = combine(hyp->p_nb + probs_0t, hyp->p_b + probs_0t, valsP->p_b);
            valsP->node = hyp->node;
            valsP->wid = hyp->wid;
            valsP->nw = hyp->nw;

            // Loop over characters
            // Here's where stuff needs to be optimized
            for (int i = 1; i < N; i++)
            {
                PrefixTreeNode::Ptr new_node;
                double probs_it = probs[t*N+i];

                bool skip = false;
                bool emit_word = false;
                if (i == ptree.space) {
                    if (hyp->node->is_word) {
                        emit_word = true;
                        new_node = ptree.root;
                    }
                    else {
                        skip = true;
                    }
                }
                else {
                    if (hyp->node->children.size() > 0 &&
                        hyp->node->children.find(i) != hyp->node->children.end() &&
                        (hyp->node->children[i]->is_prefix || hyp->node->children[i]->is_word)) {
                        new_node = hyp->node->children[i];
                    }
                    else
                        skip = true;
                }
                if (!skip) {
                    PrefixKey nprefix = prefix;
                    nprefix.push_back(i);
                    // TODO CHECK Assuming not yet in Hnext, may not be true
                    PrefixHyp::Ptr valsN(new PrefixHyp);
                    valsN->node = new_node;
                    if (Hnext.find(nprefix) != Hnext.end())
                        valsN = Hnext[nprefix];
                    else
                        Hnext[nprefix] = valsN;

                    if (emit_word) {
                        bg = alpha * ptree.lm.bg_prob(hyp->wid, hyp->node->word_id);
                        valsN->wid = hyp->node->word_id;
                        valsN->nw = hyp->nw + 1;
                    }
                    else {
                        bg = 0.0;
                        valsN->wid = hyp->wid;
                        valsN->nw = hyp->nw;
                    }

                    if (prefix.size() == 0 || (prefix.size() > 0 && i != prefix.back()))
                        valsN->p_nb = combine(hyp->p_nb + probs_it + bg, hyp->p_b + probs_it + bg, valsN->p_nb);
                    else
                        valsN->p_nb = combine(hyp->p_b + probs_it + bg, valsN->p_nb);

                    if (Hcurr.find(nprefix) == Hcurr.end()) {
                        PrefixHyp::Ptr oldhyp(new PrefixHyp);
                        if (Hold.find(nprefix) != Hold.end())
                            oldhyp = Hold[nprefix];
                        else
                            Hold[nprefix] = oldhyp;
                        valsN->p_b = combine(oldhyp->p_nb + probs_0t, oldhyp->p_b + probs_0t, valsN->p_b);
                        valsN->p_nb = combine(oldhyp->p_nb + probs_it, valsN->p_nb);
                    }
                    valsN->score = prefix_hyp_score(valsN);
                }
                if (prefix.size() > 0 && i == prefix.back()) {
                    valsP->p_nb = combine(hyp->p_nb + probs_it, valsP->p_nb);
                }

                valsP->score = prefix_hyp_score(valsP);
            }
        }

        Hold = Hnext;

        // Beam cutoff
        std::vector<PrefixPair> Hnext_vec(Hnext.begin(), Hnext.end());
        std::sort(Hnext_vec.begin(), Hnext_vec.end(), comp_prefix_pair);
        if (Hnext_vec.size() > beam)
            Hnext_vec.erase(Hnext_vec.begin() + beam, Hnext_vec.end());
        best_prefix = Hnext_vec[0].first;
        best_score = Hnext_vec[0].second->score;
        Hcurr = PrefixMap(Hnext_vec.begin(), Hnext_vec.end());
    }

    return best_score;
}

py::tuple decode_lm_wrapper(np::array probs, int beam, double alpha, double beta)
{
    // Check that we can use numeric_limits infinity, then comment out
    //assert(std::numeric_limits<double>::is_iec559);

    py::object shape = probs.attr("shape");
    unsigned int N = py::extract<unsigned int>(shape[0]);
    unsigned int T = py::extract<unsigned int>(shape[1]);
    std::cout << N << "x" << T << " probs" << std::endl;

    // Convert probs to double array, col-major
    double* data = static_cast<double*>(PyArray_DATA(probs.ptr()));

    // Write data for profiling in another script
    std::ofstream os("probs.dat", std::ios::binary);
    os.write(reinterpret_cast<const char*>(data), std::streamsize(N*T*sizeof(double)));
    os.close();

    // Build / load prefix tree
    // TODO Create a prefix tree server that keeps object in memory

    // TODO FIXME PARAM Import
    std::string CHARMAP_PATH = "/scail/group/deeplearning/speech/zxie/kaldi-stanford/kaldi-trunk/egs/wsj/s6/ctc-utils";
    PrefixTree ptree(CHARMAP_PATH + "/chars.txt",
                     CHARMAP_PATH + "/wordlist",
                     CHARMAP_PATH + "/lm_bg.arpa");

    // Decode!
    PrefixKey best_prefix;
    boost::timer::cpu_timer timer;
    double best_score = decode_bg_lm(data, N, T, ptree, beam, alpha, beta, best_prefix);
    boost::timer::cpu_times elapsed = timer.elapsed();
    std::cout << "decoding time (wall): " << elapsed.wall / 1e9 << std::endl;

    py::list py_prefix;
    for (int k = 0; k < best_prefix.size(); k++)
    {
        py_prefix.append(best_prefix[k]);
    }
    return py::make_tuple(py_prefix, best_score);
}

BOOST_PYTHON_MODULE(fastdecode)
{
    np::array::set_module_and_type("numpy", "ndarray");

    py::def("decode_lm_wrapper", &decode_lm_wrapper);
}


/*
 * Copyright 2009-2020 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.
 *
 */

#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE unitconverter_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/unitconverter.h"

using namespace std;
using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(unitconverter_test)

BOOST_AUTO_TEST_CASE(unitconverter_test_distance) {
  UnitConverter converter;

  double distance = 1;  // Assuming in meters
  double distance_new =
      converter.convert(DistanceUnit::meters, DistanceUnit::nanometers) *
      distance;
  BOOST_CHECK_EQUAL(1E9, distance_new);

  distance_new =
      converter.convert(DistanceUnit::meters, DistanceUnit::centimeters) *
      distance;
  BOOST_CHECK_EQUAL(100, distance_new);

  distance_new =
      converter.convert(DistanceUnit::meters, DistanceUnit::angstroms) *
      distance;
  BOOST_CHECK_EQUAL(1E10, distance_new);

  distance_new =
      converter.convert(DistanceUnit::meters, DistanceUnit::bohr) * distance;
  BOOST_CHECK_CLOSE(1.8897e+10, distance_new, 0.01);

  // Now assuming distance in angstroms
  distance_new =
      converter.convert(DistanceUnit::angstroms, DistanceUnit::nanometers) *
      distance;
  BOOST_CHECK_CLOSE(0.1, distance_new, 0.01);

  distance_new =
      converter.convert(DistanceUnit::angstroms, DistanceUnit::meters) *
      distance;
  BOOST_CHECK_CLOSE(1.E-10, distance_new, 0.01);

  // Now assuming distance in Bohr
  distance_new =
      converter.convert(DistanceUnit::nanometers, DistanceUnit::bohr) *
      distance;
  BOOST_CHECK_CLOSE(18.8973, distance_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_time) {
  UnitConverter converter;

  double time = 1;  // Assuming in seconds
  double time_new =
      converter.convert(TimeUnit::seconds, TimeUnit::microseconds) * time;
  BOOST_CHECK_CLOSE(1E6, time_new, 0.01);

  time_new = converter.convert(TimeUnit::seconds, TimeUnit::nanoseconds) * time;
  BOOST_CHECK_CLOSE(1E9, time_new, 0.01);

  time_new = converter.convert(TimeUnit::seconds, TimeUnit::picoseconds) * time;
  BOOST_CHECK_CLOSE(1E12, time_new, 0.01);

  time_new =
      converter.convert(TimeUnit::seconds, TimeUnit::femtoseconds) * time;
  BOOST_CHECK_CLOSE(1E15, time_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_mass) {
  UnitConverter converter;

  double mass = 1;  // Assuming in kilograms
  double mass_new =
      converter.convert(MassUnit::kilograms, MassUnit::grams) * mass;
  BOOST_CHECK_CLOSE(1E3, mass_new, 0.01);

  mass_new = converter.convert(MassUnit::kilograms, MassUnit::picograms) * mass;
  BOOST_CHECK_CLOSE(1E15, mass_new, 0.01);

  mass_new =
      converter.convert(MassUnit::kilograms, MassUnit::femtograms) * mass;
  BOOST_CHECK_CLOSE(1E18, mass_new, 0.01);

  mass_new = converter.convert(MassUnit::kilograms, MassUnit::attograms) * mass;
  BOOST_CHECK_CLOSE(1E21, mass_new, 0.01);

  mass_new =
      converter.convert(MassUnit::kilograms, MassUnit::atomic_mass_units) *
      mass;
  BOOST_CHECK_CLOSE(6.02213665E26, mass_new, 0.01);

  mass_new =
      converter.convert(MassUnit::kilograms, MassUnit::grams_per_mole) * mass;
  BOOST_CHECK_CLOSE(6.02213665E26, mass_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_energy) {
  UnitConverter converter;

  double energy = 1;  // Assuming in electron volts
  double energy_new = converter.convert(EnergyUnit::electron_volts,
                                        EnergyUnit::electron_volts) *
                      energy;
  BOOST_CHECK_CLOSE(1, energy_new, 0.01);

  energy_new =
      converter.convert(EnergyUnit::electron_volts, EnergyUnit::kilocalories) *
      energy;
  BOOST_CHECK_CLOSE(3.82929E-23, energy_new, 0.01);

  energy_new =
      converter.convert(EnergyUnit::electron_volts, EnergyUnit::joules) *
      energy;
  BOOST_CHECK_CLOSE(1.60218E-19, energy_new, 0.01);

  energy_new =
      converter.convert(EnergyUnit::electron_volts, EnergyUnit::hartrees) *
      energy;
  BOOST_CHECK_CLOSE(0.0367493, energy_new, 0.01);

  energy_new =
      converter.convert(EnergyUnit::electron_volts, EnergyUnit::kilojoules) *
      energy;
  BOOST_CHECK_CLOSE(1.602176E-22, energy_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_molar_energy) {
  UnitConverter converter;

  double energy = 1;  // Assuming in electron volts
  double energy_new =
      converter.convert(MolarEnergyUnit::electron_volts_per_mole,
                        MolarEnergyUnit::electron_volts_per_mole) *
      energy;
  BOOST_CHECK_CLOSE(1, energy_new, 0.01);

  energy_new = converter.convert(MolarEnergyUnit::electron_volts_per_mole,
                                 MolarEnergyUnit::kilocalories_per_mole) *
               energy;
  BOOST_CHECK_CLOSE(3.82929E-23, energy_new, 0.01);

  energy_new = converter.convert(MolarEnergyUnit::electron_volts_per_mole,
                                 MolarEnergyUnit::joules_per_mole) *
               energy;
  BOOST_CHECK_CLOSE(1.60218E-19, energy_new, 0.01);

  energy_new = converter.convert(MolarEnergyUnit::electron_volts_per_mole,
                                 MolarEnergyUnit::hartrees_per_mole) *
               energy;
  BOOST_CHECK_CLOSE(0.0367493, energy_new, 0.01);

  energy_new = converter.convert(MolarEnergyUnit::electron_volts_per_mole,
                                 MolarEnergyUnit::kilojoules_per_mole) *
               energy;
  BOOST_CHECK_CLOSE(1.602176E-22, energy_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_charge) {
  UnitConverter converter;

  double charge = 1.602176565E-19;  // Assuming in units of coulombs
  double charge_new =
      converter.convert(ChargeUnit::coulombs, ChargeUnit::e) * charge;
  BOOST_CHECK_CLOSE(1.000, charge_new, 0.01);

  charge = 1;  // Assuming in eV
  charge_new = converter.convert(ChargeUnit::e, ChargeUnit::coulombs) * charge;
  BOOST_CHECK_CLOSE(1.60217656E-19, charge_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_velocity) {
  UnitConverter converter;

  double velocity = 1;  // Assuming in units of angstroms_per_femtosecond
  double velocity_new =
      converter.convert(VelocityUnit::angstroms_per_femtosecond,
                        VelocityUnit::angstroms_per_picosecond) *
      velocity;
  BOOST_CHECK_CLOSE(1000, velocity_new, 0.01);

  velocity_new = converter.convert(VelocityUnit::angstroms_per_femtosecond,
                                   VelocityUnit::nanometers_per_picosecond) *
                 velocity;
  BOOST_CHECK_CLOSE(100, velocity_new, 0.01);

  // Assuming in units of nanometers_per_picosecond
  velocity_new = converter.convert(VelocityUnit::nanometers_per_picosecond,
                                   VelocityUnit::angstroms_per_femtosecond) *
                 velocity;
  BOOST_CHECK_CLOSE(0.01, velocity_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_force) {
  UnitConverter converter;

  double force = 1;  // Assuming kilojoules_per_nanometer
  double force_new = converter.convert(ForceUnit::kilojoules_per_nanometer,
                                       ForceUnit::kilocalories_per_angstrom) *
                     force;
  BOOST_CHECK_CLOSE(0.0239006, force_new, 0.01);

  force_new = converter.convert(ForceUnit::kilojoules_per_nanometer,
                                ForceUnit::kilojoules_per_angstrom) *
              force;
  BOOST_CHECK_CLOSE(0.1, force_new, 0.01);

  force_new = converter.convert(ForceUnit::kilojoules_per_nanometer,
                                ForceUnit::hatree_per_bohr) *
              force;
  BOOST_CHECK_CLOSE(1.2138E19, force_new, 0.01);

  force_new = converter.convert(ForceUnit::kilojoules_per_nanometer,
                                ForceUnit::newtons) *
              force;
  BOOST_CHECK_CLOSE(1E12, force_new, 0.01);
}

BOOST_AUTO_TEST_CASE(unitconverter_test_molar_force) {
  UnitConverter converter;

  double force = 1;  // Assuming kilojoules_per_mole_nanometer
  double force_new =
      converter.convert(MolarForceUnit::kilojoules_per_mole_nanometer,
                        MolarForceUnit::kilocalories_per_mole_angstrom) *
      force;
  BOOST_CHECK_CLOSE(0.0239006, force_new, 0.01);

  force_new = converter.convert(MolarForceUnit::kilojoules_per_mole_nanometer,
                                MolarForceUnit::kilojoules_per_mole_angstrom) *
              force;
  BOOST_CHECK_CLOSE(0.1, force_new, 0.01);

  force_new = converter.convert(MolarForceUnit::kilojoules_per_mole_nanometer,
                                MolarForceUnit::hatree_per_mole_bohr) *
              force;
  BOOST_CHECK_CLOSE(1.2138E19, force_new, 0.01);

  force_new = converter.convert(MolarForceUnit::kilojoules_per_mole_nanometer,
                                MolarForceUnit::newtons_per_mole) *
              force;
  BOOST_CHECK_CLOSE(1E12, force_new, 0.01);
}
BOOST_AUTO_TEST_SUITE_END()


//  Boost integer_fwd.hpp header file  ---------------------------------------//

//  (C) Copyright Dave Abrahams and Daryle Walker 2001. 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)

//  See http://www.boost.org/libs/integer for documentation.

#ifndef BOOST_INTEGER_FWD_HPP
#define BOOST_INTEGER_FWD_HPP

#include <boost/config.hpp>  // for BOOST_NO_INTRINSIC_WCHAR_T
#include <boost/cstdint.hpp> // For intmax_t
#include <boost/limits.hpp>  // for std::numeric_limits
#include <climits>           // for UCHAR_MAX, etc.
#include <cstddef>           // for std::size_t

namespace boost
{

#ifdef BOOST_NO_INTEGRAL_INT64_T
typedef unsigned long static_log2_argument_type;
typedef int static_log2_result_type;
typedef long static_min_max_signed_type;
typedef unsigned long static_min_max_unsigned_type;
#else
typedef boost::uintmax_t static_min_max_unsigned_type;
typedef boost::intmax_t static_min_max_signed_type;
typedef boost::uintmax_t static_log2_argument_type;
typedef int static_log2_result_type;
#endif

//  From <boost/cstdint.hpp>  ------------------------------------------------//

// Only has typedefs or using statements, with #conditionals

//  From <boost/integer_traits.hpp>  -----------------------------------------//

template <class T>
class integer_traits;

template <>
class integer_traits<bool>;

template <>
class integer_traits<char>;

template <>
class integer_traits<signed char>;

template <>
class integer_traits<unsigned char>;

#ifndef BOOST_NO_INTRINSIC_WCHAR_T
template <>
class integer_traits<wchar_t>;
#endif

template <>
class integer_traits<short>;

template <>
class integer_traits<unsigned short>;

template <>
class integer_traits<int>;

template <>
class integer_traits<unsigned int>;

template <>
class integer_traits<long>;

template <>
class integer_traits<unsigned long>;

#if !defined(BOOST_NO_INTEGRAL_INT64_T) && !defined(BOOST_NO_INT64_T) &&       \
    defined(BOOST_HAS_LONG_LONG)
template <>
class integer_traits<::boost::long_long_type>;

template <>
class integer_traits<::boost::ulong_long_type>;
#elif !defined(BOOST_NO_INTEGRAL_INT64_T) && !defined(BOOST_NO_INT64_T) &&     \
    defined(BOOST_HAS_MS_INT64)
template <>
class integer_traits<__int64>;

template <>
class integer_traits<unsigned __int64>;
#endif

//  From <boost/integer.hpp>  ------------------------------------------------//

template <typename LeastInt>
struct int_fast_t;

template <int Bits>
struct int_t;

template <int Bits>
struct uint_t;

#if !defined(BOOST_NO_INTEGRAL_INT64_T) && defined(BOOST_HAS_LONG_LONG)
template <boost::long_long_type MaxValue> // maximum value to require support
#else
template <long MaxValue>          // maximum value to require support
#endif
struct int_max_value_t;

#if !defined(BOOST_NO_INTEGRAL_INT64_T) && defined(BOOST_HAS_LONG_LONG)
template <boost::long_long_type MinValue> // minimum value to require support
#else
template <long MinValue>          // minimum value to require support
#endif
struct int_min_value_t;

#if !defined(BOOST_NO_INTEGRAL_INT64_T) && defined(BOOST_HAS_LONG_LONG)
template <boost::ulong_long_type MaxValue> // maximum value to require support
#else
template <unsigned long MaxValue> // maximum value to require support
#endif
struct uint_value_t;

//  From <boost/integer/integer_mask.hpp>  -----------------------------------//

template <std::size_t Bit>
struct high_bit_mask_t;

template <std::size_t Bits>
struct low_bits_mask_t;

template <>
struct low_bits_mask_t<::std::numeric_limits<unsigned char>::digits>;

//  From <boost/integer/static_log2.hpp>  ------------------------------------//

template <static_log2_argument_type Value>
struct static_log2;

template <>
struct static_log2<0u>;

//  From <boost/integer/static_min_max.hpp>  ---------------------------------//

template <static_min_max_signed_type Value1, static_min_max_signed_type Value2>
struct static_signed_min;

template <static_min_max_signed_type Value1, static_min_max_signed_type Value2>
struct static_signed_max;

template <static_min_max_unsigned_type Value1,
          static_min_max_unsigned_type Value2>
struct static_unsigned_min;

template <static_min_max_unsigned_type Value1,
          static_min_max_unsigned_type Value2>
struct static_unsigned_max;

namespace integer
{
//  From <boost/integer/common_factor_ct.hpp>

#ifdef BOOST_NO_INTEGRAL_INT64_T
typedef unsigned long static_gcd_type;
#else
typedef boost::uintmax_t static_gcd_type;
#endif

template <static_gcd_type Value1, static_gcd_type Value2>
struct static_gcd;
template <static_gcd_type Value1, static_gcd_type Value2>
struct static_lcm;

//  From <boost/integer/common_factor_rt.hpp>

template <typename IntegerType>
class gcd_evaluator;
template <typename IntegerType>
class lcm_evaluator;

} // namespace integer

} // namespace boost

#endif // BOOST_INTEGER_FWD_HPP


///////////////////////////////////////////////////////////////////////////////
// statistics::model::wrap::aggregate::model_covariate_parameter.hpp         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_WRAP_AGGREGATE_MODEL_COVARIATE_PARAMETER_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_WRAP_AGGREGATE_MODEL_COVARIATE_PARAMETER_HPP_ER_2009
#include <boost/statistics/model/wrap/aggregate/model_parameter.hpp>
#include <boost/statistics/model/wrap/unary/covariate.hpp>

namespace boost{
namespace statistics{
namespace model{

    template<typename M,typename X,typename P>
    class model_covariate_parameter_ :
        public model_parameter_<M,P>,
        public covariate_wrapper<X>
    {
        typedef model_parameter_<M,P>           model_parameter_;
        typedef covariate_wrapper<X>            covariate_;

        public:

        typedef model_covariate_parameter_<M,X,P>
                                model_covariate_parameter_type;

        // Construction
        model_covariate_parameter_();
        model_covariate_parameter_(model_parameter_ mp,covariate_ cov);
        model_covariate_parameter_(
            typename model_parameter_::model_wrapper_type model,
            covariate_ cov,
            typename model_parameter_::parameter_wrapper_type par
        );
        model_covariate_parameter_(
            const model_covariate_parameter_&
        );

    };

    // Implementation //

    template<typename M,typename X,typename P>
    model_covariate_parameter_<M,X,P>
        ::model_covariate_parameter_()
    : model_parameter_(),covariate_(){}

    template<typename M,typename X,typename P>
    model_covariate_parameter_<M,X,P>::model_covariate_parameter_(
        model_parameter_ mp, covariate_ cov
    )
     : model_parameter_( mp ), covariate_( cov ){}

    template<typename M,typename X,typename P>
    model_covariate_parameter_<M,X,P>::model_covariate_parameter_(
        typename model_parameter_::model_wrapper_type m,
        covariate_ cov,
        typename model_parameter_::parameter_wrapper_type p
    )
     : model_parameter_( m, p ), covariate_( cov ){}

    template<typename M,typename X,typename P>
    model_covariate_parameter_<M,X,P>::model_covariate_parameter_(
        const model_covariate_parameter_& that
    ) : model_parameter_( that ), covariate_( that ) {}

    // Free functions

    template<typename M,typename X,typename P>
    model_covariate_parameter_<M,X,P>
    make_model_covariate_parameter(const M& m,const X& x,const P& p){
        typedef model_covariate_parameter_<M,X,P> result_type;
        return result_type(m,x,p);
    }

}// model
}// statistics
}// boost

#endif